[Solved] MPU9250 and VL53L0X initialisation failure!

Hello,

I am using CORE2-ROS with RasPI 3B configuration with newest at this moment rosbot_fw_230400.bin firmware (v0.9.2). RasPi communicates with CORE2 via /dev/serial0 and baud rate 230400 with no issues. I also wired MPU-9250 to S1 connector on CORE2. MPU-9250 was tested prior with Teensy v3.2 controller and it worked there.
However when I executed roslaunch rosbot_ekf all.launch, I ve got error:
[ERROR] [1578716967.303733]: MPU9250 initialisation failure!

I was also trying to disable MPU9250 sensor with command:
husarion@husarion:~$ rosservice call /config "command: 'EIMU’
data: ‘0’"

data: ‘’
result: 0

but after that I still get the same error when executing roslaunch rosbot_ekf all.launch

So my questions are:

  • Is there something wrong with rosbot_fw_230400.bin?
  • How I can test MPU-9250 or VL53L0X while its connected to CORE2? Is there available Python script?
  • Why rosservice call /config "command: ‘EIMU’ command did not disable MPU-9250? It also did not disable disconnected VL53L0X sensors when I executed rosservice call /config "command: ‘EDSE’

Here is roslaunch rosbot_ekf all.launch output for your reference:

husarion@husarion:~/ros_workspace$ roslaunch rosbot_ekf all.launch
… logging to /home/husarion/.ros/log/efca0334-342a-11ea-9638-b827eb49be0d/roslaunch-husarion-2982.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://husarion:33943/

SUMMARY

CLEAR PARAMETERS

  • /rosbot_ekf/

PARAMETERS

  • /rosbot_ekf/acceleration_gains: [0.8, 0.0, 0.0, 0…
  • /rosbot_ekf/acceleration_limits: [1.3, 0.0, 0.0, 0…
  • /rosbot_ekf/base_link_frame: base_link
  • /rosbot_ekf/control_config: [True, False, Fal…
  • /rosbot_ekf/control_timeout: 0.2
  • /rosbot_ekf/debug: False
  • /rosbot_ekf/debug_out_file: /path/to/debug/fi…
  • /rosbot_ekf/deceleration_gains: [1.0, 0.0, 0.0, 0…
  • /rosbot_ekf/deceleration_limits: [1.3, 0.0, 0.0, 0…
  • /rosbot_ekf/frequency: 20
  • /rosbot_ekf/imu0: /imu
  • /rosbot_ekf/imu0_config: [False, False, Fa…
  • /rosbot_ekf/imu0_differential: True
  • /rosbot_ekf/imu0_linear_acceleration_rejection_threshold: 0.8
  • /rosbot_ekf/imu0_nodelay: False
  • /rosbot_ekf/imu0_pose_rejection_threshold: 0.8
  • /rosbot_ekf/imu0_queue_size: 4
  • /rosbot_ekf/imu0_relative: True
  • /rosbot_ekf/imu0_remove_gravitational_acceleration: True
  • /rosbot_ekf/imu0_twist_rejection_threshold: 0.8
  • /rosbot_ekf/initial_estimate_covariance: [‘1e-9’, 0, 0, 0,…
  • /rosbot_ekf/map_frame: map
  • /rosbot_ekf/odom0: /odom/wheel
  • /rosbot_ekf/odom0_config: [True, True, True…
  • /rosbot_ekf/odom0_differential: False
  • /rosbot_ekf/odom0_nodelay: False
  • /rosbot_ekf/odom0_queue_size: 6
  • /rosbot_ekf/odom0_relative: True
  • /rosbot_ekf/odom_frame: odom
  • /rosbot_ekf/print_diagnostics: True
  • /rosbot_ekf/process_noise_covariance: [0.05, 0, 0, 0, 0…
  • /rosbot_ekf/publish_acceleration: False
  • /rosbot_ekf/publish_tf: True
  • /rosbot_ekf/sensor_timeout: 0.2
  • /rosbot_ekf/stamped_control: False
  • /rosbot_ekf/transform_time_offset: 0.0
  • /rosbot_ekf/transform_timeout: 0.0
  • /rosbot_ekf/two_d_mode: False
  • /rosbot_ekf/use_control: True
  • /rosbot_ekf/world_frame: odom
  • /rosdistro: kinetic
  • /rosversion: 1.12.14
  • /serial_node/baud: 230400
  • /serial_node/port: /dev/serial0

NODES
/
imu_publisher (tf/static_transform_publisher)
msgs_conversion (rosbot_ekf/msgs_conversion)
rosbot_ekf (robot_localization/ekf_localization_node)
serial_node (rosserial_python/serial_node.py)

auto-starting new master
process[master]: started with pid [2992]
ROS_MASTER_URI=http://master:11311

setting /run_id to efca0334-342a-11ea-9638-b827eb49be0d
process[rosout-1]: started with pid [3005]
started core service [/rosout]
process[serial_node-2]: started with pid [3023]
process[msgs_conversion-3]: started with pid [3024]
process[rosbot_ekf-4]: started with pid [3025]
process[imu_publisher-5]: started with pid [3031]
[INFO] [1578716964.978488]: ROS Serial Python Node
[INFO] [1578716965.000728]: Connecting to /dev/serial0 at 230400 baud
[INFO] [1578716967.122263]: Note: publish buffer size is 768 bytes
[INFO] [1578716967.130979]: Setup service server on /config [rosbot_ekf/Configuration]
[INFO] [1578716967.174615]: Setup publisher on /battery [sensor_msgs/BatteryState]
[INFO] [1578716967.184096]: Setup publisher on /pose [geometry_msgs/PoseStamped]
[INFO] [1578716967.193667]: Setup publisher on /velocity [geometry_msgs/Twist]
[INFO] [1578716967.203203]: Setup publisher on /range/fr [sensor_msgs/Range]
[INFO] [1578716967.212568]: Setup publisher on /range/fl [sensor_msgs/Range]
[INFO] [1578716967.222178]: Setup publisher on /range/rr [sensor_msgs/Range]
[INFO] [1578716967.231625]: Setup publisher on /range/rl [sensor_msgs/Range]
[INFO] [1578716967.240995]: Setup publisher on /joint_states [sensor_msgs/JointState]
[INFO] [1578716967.251839]: Setup publisher on /mpu9250 [rosbot_ekf/Imu]
[INFO] [1578716967.261028]: Setup publisher on /buttons [std_msgs/UInt8]
[INFO] [1578716967.264668]: Note: subscribe buffer size is 768 bytes
[INFO] [1578716967.284680]: Setup subscriber on /cmd_vel [geometry_msgs/Twist]
[INFO] [1578716967.288567]:


| ___ | _ |/ || | | | / |
| |
/ /| | | |\ --. | |__ ___ | |_ | |_ __ __ | / | | | | –. | ’
\ / _ \ | __| | |\ \ /\ / /
| |\ \ \ _/ //_
/ /| |
) || (
) || |_ | | \ V V /
_| _| _/ _/ |./ _/ _| || _/_/

Firmware version: 0.9.2

[ERROR] [1578716967.327457]: MPU9250 initialisation failure!
[ERROR] [1578716967.330740]: VL53L0X sensors initialisation failure!


VL53L0X sensors were not hooked up.

Thank you very much!
Leo

Hi,
That is because the MPU9250 should be connected to the second hSensor port with the following wiring:

  • SENS2_PIN4 <-> MPU9250 SDA
  • SENS2_PIN3 <-> MPU9250 SCL
  • SENS2_PIN1 <-> MPU9250 INT

For the distance sensors wiring please refer to the beginning of the file : rosbot-firmware-new/rosbot_sensors.h at master · husarion/rosbot-firmware-new · GitHub

Both the distance sensors and inertial sensor are initialized after board’s power cycle/reset. If initialization fails the failure message will be printed each time the rosserial connection is being established.

Cheers

Thank you very much, Szymon!
Moving MPU9250 to S2 (SENS2) port resolved the issue!

When I had initialization issue in a first place, I was using wiring diagram that was published on Rosbot assembly instructions document and in tutorials as well. So I have another question, if you don’t mind. Do you guys have new CORE2-ROS wiring diagram for new Mbed OS, including VL53L0X sensors?

Another question. In /src/rosbot_sensors.h there is pins definition for VL53L0X
#define SENSOR_FR_XSHOUT_PIN SENS6_PIN1
#define SENSOR_FL_XSHOUT_PIN SENS6_PIN2
#define SENSOR_RR_XSHOUT_PIN SENS6_PIN4
#define SENSOR_RL_XSHOUT_PIN SENS6_PIN3
#define SENSORS_SDA_PIN SENS1_PIN4
#define SENSORS_SCL_PIN SENS1_PIN3

Based on the definition above:

  • How about SCL; SDA pins definition for SENS3, SENS4, SENS5, SENS6?
  • Why all XSHOUT connected to pins on SENS6 only (PIN1,2,3,4)?

So logically, should we just connect VL53L0X on each SENS3, SENS4, SENS5, SENS6 same way as following? :
GPIO → PIN1
XSHUT → PIN2
SCL → PIN3
SDA → PIN4
+5V → PIN5
GND → PIN6

Sorry for possible confusion I might created, or if I misunderstand something :grinning:

Thanks a lot!
Leo

Hi,
You’ve probably used some outdated documentation. Please refer to CORE2 | Husarion for available peripherals on different ports. If you want to develop custom firmware in Mbed framework, here are files that define available pins and peripherals:

The I2C instances are available on hSensor 1 & 2 ports and one on hExt port. MPU9250 is connected to hSensor 2. That leaves only one hSensor with I2C for distance sensors. Remember that I2C is a bus protocol so many devices can be connected to the one instance.

Hi Szymon,

Once again, Thank You very much for the reply!

You are right, I was using logic and wiring diagram from older documentation trying to “translate” it for new Mbed environment. Right now I am not trying to develop custom framework, my current goal is to get sensors, RPlidar and motors working within existing firmware with very minimal or no modifications, so I can use it as a working base. I checked documentation you mentioned few days ago and there are pins definitions which is very valuable for future reference, however I could not find wiring diagram or wiring instructions for 4 VL53L0X sensors. As far as understand from rosbot-firmware-new/rosbot_sensors.h at master · husarion/rosbot-firmware-new · GitHub all 4 XSHUT pins connected to SENSE6 (hSensor 6) PIN1,2,3,4, but I could not find wiring information about all 4 VL53L0X I2C pins SCL and SDA. I understand that I2C bus protocol and devices could be connected to one instance, but what is actual physical connection of those pins is unclear, since all functionality and logic of physical ports controls by your software :slight_smile:

I think, it will be ideal, if we can just get wiring diagram similar to the one described in ROSbot_assembly_instruction.pdf or it’s online version but with latest changes. This current wiring diagram has old IR sensors SHARP GP2Y0A41SK0F, but we need new VL53L0X.

Much appreciated and thanks again,
Leo

Hy Szymon,

While I was waiting for your reply, in a mean time I was “brainstorming” and analyzing pins definitions and the software code :grinning: , so I am thinking the solution is the following:

VL53L0X XSHUT pins connected to only SENSE6 port (pin1,2,3,4) based on this definition:
#define SENSOR_FR_XSHOUT_PIN SENS6_PIN1
#define SENSOR_FL_XSHOUT_PIN SENS6_PIN2
#define SENSOR_RR_XSHOUT_PIN SENS6_PIN4
#define SENSOR_RL_XSHOUT_PIN SENS6_PIN3

All VL53L0X SDA pins connected together and to SENS1_PIN4
All VL53L0X SCL pins connected together and to SENS1_PIN3
All VL53L0X VCC pins connected together and to SENS1_PIN5 (or to pin 5 on any sensor port)
All VL53L0X GND pins connected together and to SENS1_PIN6 (or to pin 6 on any sensor port)

We don’t use VL53L0X GPIO1 pin and leave it not connected.

We can use additional simple breakout board for SDA SCL and power pins to make less wire clutter

Is this all true? Please let me know.

Thank you very much,
Leo

Hi,
Yes, that’s the correct pins description for distance sensors. I mentioned the file with this definitions in my first response. Unfortunately, we don’t make available internal ROSbot connection diagrams, however you should have all information required to connect the sensors now.

Using the additional breakout board for SDA, SCL signals and power is a good idea. You can also use some splicing connectors like this: https://www.amazon.com/dp/B07QPXYG8H/ref=cm_sw_em_r_mt_dp_U_OZWjEbVF7V07V

Cheers,
Szymon

Thank you very much Szymon! I will proceed with wiring and hopefully everything will work together. By the way, do you have test scripts available to do quick test of all sensors one by one (MPU9250 and VL53L0X) while they are connected to CORE2? That would be very beneficial to everyone, since we can test wiring and sensors by themselves. I have some sensor test scripts to work with standalone arduino, however to create own scripts for the CORE2 system would be probably a bit more challenging to avoid conflicts with existing CORE2 firmware and running ROS software, so its easier to reuse existing that you might have:) .

Once again, thank you very much for the support and sharing the knowledge!

-Leo

Hello Szymon!

Unfortunately I am still receiving “VL53L0X sensors initialisation failure!” when i am executing roslaunch rosbot_ekf all.launch MPU9250 initialized normally, no errors.

Here are my observations and measurements.

  • 4 VL53L0X sensors where connected as mentioned above:
    All VL53L0X SDA pins connected together and to SENS1_PIN4
    All VL53L0X SCL pins connected together and to SENS1_PIN3
    All VL53L0X VCC pins connected together and to SENS1_PIN5
    All VL53L0X GND pins connected together and to SENS1_PIN6

    SENSOR_FR_XSHOUT_PIN connected SENS6_PIN1
    ENSOR_FL_XSHOUT_PIN connected SENS6_PIN2
    SENSOR_RR_XSHOUT_PIN connected SENS6_PIN4
    SENSOR_RL_XSHOUT_PIN connected SENS6_PIN3

  • I was using oscilloscope to measure signals on SDA, SCL and XSHUT pins while roslaunch rosbot_ekf all.launchwas running
    SDA +5v, no pulses
    SCL +5v, no pulses
    XSHUT +3.3v, no pulses on each pin (SENS6 PIN1,2,3,4)

  • I even did the same measurement on just one VL53L0X (others were temporary disconnected during the measurement). Same results. I connected sensors back after this experiment.

  • I also build arduino based tester and tested each sensor individually, all of them operational and showed correct distance measurement.

So based on these observations, here are my questions:

  • are there other dependencies on other hardware / software components when executing roslaunch rosbot_ekf all.launch? (RasPI, CORE2, MPU9250 and VL53L0X where connected, of course and roslaunch rosbot_ekf all.launch was executing right after RasPI boot)
  • could it be firmware (rosbot_fw_230400.bin) or software issue?
  • Are there any scripts available to test sensors within CORE2? As far as I understand, even if one of the VL53L0X sensors fail, it will show complete failure.

Thank you very much!
Leo

Hi Leo,

Sorry to hear that you still have problems with accommodating distance sensors into your project. Mind that we really want to help you troubleshoot this.

First of all please tell me what kind of carrier board for VL53L01X do you use? Do you use Pololu’s one?

I consulted the CORE2 PCB designer and there was indeed a minor hardware change in the board that is required for I2C communication to work with multiple sensors on a single bus. New batches of board have this change, so it is possible that your board is not affected. Please check these resistor arrays:

If you find different values there, that means you have board from the older batch. To make the sensors work you would need to replace these two elements with https://www.digikey.com/product-detail/en/yageo/YC164-JR-0722RL/YC164J-22CT-ND/1005636 or similar resistor arrays. Required resistance is between 2.2R - 22R. You will need to use hot air gun for this.

If it comes to wiring up single sensor at a time, it won’t work. At the moment firmware initializes sensors just after power cycle/reset and each four of them have to pass the procedure. Otherwise the program won’t proceed with further setup and no measurement will be done. This behavior might be changed in future releases.

Regards,
Szymon

Hello Szymon!

Thanks for detailed information!
I have CJVL53L0XV2 sensors - marked on the board (GY VL53L0X V2 by vendor specs), so I believe they are not Pololu’s, but similar.

I have 331 SMD array resistors on CORE2 and we need 220 - big difference (330 ohm and 22 ohm). Are these are pull up resistors? To me 22 ohm value kind of low…
Honestly, I don’t want mess around with replacing SMD elements, I could easily damage another components or board :grinning:
Is it possible to use regular resistors on breakout board I am using for connecting VL53L0X sensors and connect +5v via 2 new resistors - one to SCL and one to SDA pin? I figured since the current resistor is 330 ohm on board and if we put 33 ohm on breakout board so total in parallel it would be about 30 ohm. We can choose different resistor value if needed. If solution with breakout board will not work, unfortunately we would need to replace the CORE2 board with the latest one. Please let me know.

Thank you,
Leo

These are series resistors, so I believe the change is required to make this work.

Is there any other alternatives besides replacing SMD resistor arrays or replacing the board? Probably many people with older CORE2 boards have the same question. Is possible to use regular resistors on external breakout board connected to S1 SCL SDA pins to fix the issue?

Thank you,
Leo

Hi,
You can try I2C in the hExt port. There is lower series resistance on pins there, so it may work. It will require small change in the firmware though. I will be able to confirm if it works on Monday.

Kind Regards,
Szymon

Hi,
Unfortunately, only one distance sensor can communicate on hExt i2c bus. If you want to wire up all four of them, the resistor replacement will be required. In case you don’t want to do it yourself, please contact us via support@husarion.com.

Sorry for the inconvenience.

Regards,
Szymon

Thank you Szymon! I will contact support@husarion.com for replacement and let you know it’s outcome.

Thanks for the advice,
Leo