Multiple VL53L1X on RPi with software I2C not working

[nav2u]

Hello,

I've been trying to get multiple VL53L1X to work with a Raspberry pi. I've used the TCA9548A multiplexer but had some issues (see issue #26) and so decided to try using software I2C on the Raspberry Pi as an alternate.

When using the software I2C with multiple sensors, it seems like distance is only read from one sensor and that distance is then displayed as results for all the other sensor running on different I2C busses.

Some testing I did:

• If I use a single I2C bus, I get data from the correct sensor. Tested this on all buses individually (1 through to 6, excluding bus 2). I think this confirms that all the sensors are working and that the software I2C bus has been enabled correctly.
• Ran two sensors together on buses 1 and 3. Distance results for both sensors were being read from sensor on bus 3. *
• Ran the above test again but initialised/opened sensor on bus 3 first and then bus 1. Distance results for both sensors were being read from sensor on bus 1. *
• Ran all five sensors together on buses 1, 3, 4, 5 and 6. Distance results for all sensors were being read from a single sensor on bus 6. *
• Ran the above test again but switched around the sensor initialisation in the following bus order 1, 3, 4, 6, 5. Distance results for all sensors were being read from sensor on bus 5. *

( *Confirmed by physically blocking individual sensors )

So it seems like the sensor on the last initialised/opened bus is always used by the get_distance function (I'm not sure how to explain whats going on here)? Any ideas what might be going on here? Perhaps I've missed something?

Code I used for testing:

import time
import sys
import signal
import VL53L1X

tof1 = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=0x29)
tof2 = VL53L1X.VL53L1X(i2c_bus=3, i2c_address=0x29)
tof3 = VL53L1X.VL53L1X(i2c_bus=4, i2c_address=0x29)
tof4 = VL53L1X.VL53L1X(i2c_bus=5, i2c_address=0x29)
tof5 = VL53L1X.VL53L1X(i2c_bus=6, i2c_address=0x29)

time.sleep(1)
tof1.open()
tof1.start_ranging(3)
tof2.open()
tof2.start_ranging(3)
tof3.open()
tof3.start_ranging(3)
tof4.open()
tof4.start_ranging(3)
tof5.open()
tof5.start_ranging(3)

def exit_handler(signal, frame):
    global running
    running = False
    tof1.stop_ranging()
    tof2.stop_ranging()
    tof3.stop_ranging()
    tof4.stop_ranging()
    tof5.stop_ranging()
    print()
    sys.exit(0)

running = True
signal.signal(signal.SIGINT, exit_handler)

while running:
    distance_in_mm = tof1.get_distance()
    print("Sensor 1 distance: {}mm".format(distance_in_mm))
    distance_in_mm = tof2.get_distance()
    print("Sensor 2 distance: {}mm".format(distance_in_mm))
    time.sleep(0.5)
    distance_in_mm = tof3.get_distance()
    print("Sensor 3 distance: {}mm".format(distance_in_mm))
    distance_in_mm = tof4.get_distance()
    print("Sensor 4 distance: {}mm".format(distance_in_mm))
    time.sleep(0.5)
    distance_in_mm = tof5.get_distance()
    print("Sensor 5 distance: {}mm".format(distance_in_mm))

[Gadgetoid]

I'm willing to bet- although I have no hardware with me right now to perform any testing- that the handing of Python i2c functions into the C library is what's causing this problem. See:

vl53l1x-python/python/VL53L1X.py

Lines 116 to 159 in a0418c5

	def _configure_i2c_library_functions(self):
	# I2C bus read callback for low level library.
	def _i2c_read(address, reg, data_p, length):
	ret_val = 0
	msg_w = i2c_msg.write(address, [reg >> 8, reg & 0xff])
	msg_r = i2c_msg.read(address, length)
	self._i2c.i2c_rdwr(msg_w, msg_r)
	if ret_val == 0:
	for index in range(length):
	data_p[index] = ord(msg_r.buf[index])
	return ret_val
	# I2C bus write callback for low level library.
	def _i2c_write(address, reg, data_p, length):
	ret_val = 0
	data = []
	for index in range(length):
	data.append(data_p[index])
	msg_w = i2c_msg.write(address, [reg >> 8, reg & 0xff] + data)
	self._i2c.i2c_rdwr(msg_w)
	return ret_val
	# I2C bus write callback for low level library.
	def _i2c_multi(address, reg):
	ret_val = 0
	# Write to the multiplexer
	self._i2c.write_byte(address, reg)
	return ret_val
	# Pass i2c read/write function pointers to VL53L1X library.
	self._i2c_multi_func = _I2C_MULTI_FUNC(_i2c_multi)
	self._i2c_read_func = _I2C_READ_FUNC(_i2c_read)
	self._i2c_write_func = _I2C_WRITE_FUNC(_i2c_write)
	_TOF_LIBRARY.VL53L1_set_i2c(self._i2c_multi_func, self._i2c_read_func, self._i2c_write_func)

The most relevant lines are:

vl53l1x-python/python/VL53L1X.py

Lines 155 to 159 in a0418c5

	# Pass i2c read/write function pointers to VL53L1X library.
	self._i2c_multi_func = _I2C_MULTI_FUNC(_i2c_multi)
	self._i2c_read_func = _I2C_READ_FUNC(_i2c_read)
	self._i2c_write_func = _I2C_WRITE_FUNC(_i2c_write)
	_TOF_LIBRARY.VL53L1_set_i2c(self._i2c_multi_func, self._i2c_read_func, self._i2c_write_func)

While each instance of the VL53L1X class gets its own instance of the _TOF_LIBRARY.initialise() return value, ie a VL53L1_DEV object, the call to set i2c functions is global, setting the i2c read/write/muilti pointers for all instances:

vl53l1x-python/api/platform/vl53l1_platform.c

Lines 117 to 135 in 505fd6b

	// calls the i2c write to multiplexer function
	static int (*i2c_multi_func)(uint8_t address, uint8_t reg) = NULL;
	// calls read_i2c_block_data(address, reg, length)
	static int (*i2c_read_func)(uint8_t address, uint8_t reg,
	uint8_t *list, uint8_t length) = NULL;
	// calls write_i2c_block_data(address, reg, list)
	static int (*i2c_write_func)(uint8_t address, uint8_t reg,
	uint8_t *list, uint8_t length) = NULL;
	static pthread_mutex_t i2c_mutex = PTHREAD_MUTEX_INITIALIZER;
	void VL53L1_set_i2c(void *multi_func, void *read_func, void *write_func)
	{
	i2c_multi_func = multi_func;
	i2c_read_func = read_func;
	i2c_write_func = write_func;
	}

The Fix

As near as I can tell, nothing calls these i2c_write or i2c_read functions directly. They are handled via wrappers that receive an instance of the VL53L1_DEV object:

vl53l1x-python/api/platform/vl53l1_platform.c

Lines 225 to 245 in 505fd6b

	VL53L1_Error VL53L1_WriteMulti(VL53L1_DEV pdev, uint16_t index, uint8_t *pdata, uint32_t count) {
	return i2c_write(pdev, index, pdata, count);
	//VL53L1_Error Status = VL53L1_ERROR_NONE;
	//return Status;
	}
	// the ranging_sensor_comms.dll will take care of the page selection
	VL53L1_Error VL53L1_ReadMulti(VL53L1_DEV pdev, uint16_t index, uint8_t *pdata, uint32_t count) {
	return i2c_read(pdev, index, pdata, count);
	//VL53L1_Error Status = VL53L1_ERROR_NONE;
	//return Status;
	}
	VL53L1_Error VL53L1_WrByte(VL53L1_DEV pdev, uint16_t index, uint8_t data) {
	return i2c_write(pdev, index, &data, 1);
	//VL53L1_Error Status = VL53L1_ERROR_NONE;
	//return Status;
	}

The right solultion, I believe, is therefore to move the i2c_multi_func, i2c_read_func and i2c_write_func function pointers out of the global namespace, and instead encapsulate them into VL523L1X here:

vl53l1x-python/api/platform/vl53l1_platform_user_data.h

Lines 78 to 92 in 14bbf3a

	typedef struct {
	VL53L1_DevData_t Data;
	uint8_t I2cDevAddr;
	uint8_t comms_type;
	uint16_t comms_speed_khz;
	uint32_t new_data_ready_poll_duration_ms;
	I2C_HandleTypeDef *I2cHandle;
	uint8_t TCA9548A_Device; /*!< Device number on TCA9548A I2C Multiplexer or 255 if TCA9548A not being used */
	uint8_t TCA9548A_Address; /*!< Address of TCA9548A I2C Multiplexer or 255 if TCA9548A not being used */
	} VL53L1_Dev_t;
	typedef VL53L1_Dev_t *VL53L1_DEV;

Now I'm thoroughly terrible with pointers, but I may be able to get this up and running blind and push it into a branch for you to test.

I'd been suggesting using multiple i2c channels without fully appreciating how broken the current library setup would make that. But it's easily fixable by the looks of it.

[Gadgetoid]

Okay, made a first attempt. I'll be surprised if I haven't missed something, but fingers crossed!

See: #34

[Gadgetoid]

Discussion should probably continue at #34 (though it's been a couple of years...)