PX4|基于pca9534实现多路vl53激光数据的获取

背景及需求

出于小型及轻量化考虑，使用vl53l1x作为单向激光为无人机提供距离信息，实现小型无人机在规则管体内的定高及定位。
vl53l1x激光传感器基于iic进行通信，且初始地址均相同，而px4需要获取到所有8路激光的数据，选取可进行8路io口输出的iic扩展芯片进行设计，这里选用pca9534

解读芯片手册

根据手册，该芯片通信地址为00100A_2A_1A_0,选取A_2A_1A_0为0，则地址为0x20

向指定寄存器写入数据后，可使得P_7-P_0口产生对应输出，SDA及SCL为iic通信引脚，这里不适用终端引脚

vl53l0x使用

修改地址

由于8路vl53l1x初始地址一致，在使用时需要将其修改为不同的8个地址以供px4通过iic进行寻址取值。
根据使用手册，上电后Xshut存在上升沿后会进入固件的boot阶段，此时可对传感器的寻址地址进行修改，此后保持Xshut的高电平传感器即可进入工作模式。

故在初始化时，先对vl53的寻址地址进行修改

	set_device_address(PCA9534_BASEADDR);
	writeRegister(PCA9534_OUT_REG,0x01);
	set_device_address(VL53L1X_BASEADDR_DEFAULT);
	VL53L1_WrByte(VL53L1_I2C_SLAVE__DEVICE_ADDRESS,0x30);

上电后初始化阶段，将P_0引脚的所接vl53的Xshut置1，指定vl53原默认地址VL53L1X_BASEADDR_DEFAULT（0x29），写入地址为0x30，完成对该传感器地址的修改

读取数据

在初始化时，需要建立不同朝向传感器的地址与方向的映射关系，以在解析数据时可以根据不同的地址获取到不同方向的距离信息，实现准确的定位。
px4中已经内置了针对vl53传感器的距离获取功能，在初始化，仅需指定地址与方向的关系便可以进行距离信息的存储，最终会保存在一个数组中。在获取时，可通过循环放数组，判断所解析的信息方向，获取正确方向的距离信息。

	for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) {
		if (_distance_sensor_subs[i].copy(&distance_sensor)) {	
			// only use the first instace which has the correct orientation
			if (distance_sensor.orientation == distance_sensor_s::ROTATION_UPWARD_FACING) {

				dis_up = distance_sensor.current_distance;

			}
		}

		if (_distance_sensor_subs[i].copy(&distance_sensor)) {
			// only use the first instace which has the correct orientation
			if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {

				dis_down = distance_sensor.current_distance;
			}
		}
	}

总结

硬件部分

打板实现iic扩展以及多路传感器初始化，供后续根据地址对不同朝向的传感器进行寻址获取数据。

PX4代码部分

初始化

以下文件中加入初始化部分，启动地址修改模块，完成地址与方向的映射并启动数据获取模块
\PX4-Autopilot\ROMFS\px4fmu_common\init.d\rc.mc_apps

pca9534 start -X -b 2
# 右前
vl53l1x start -X  -a 0x30 -R 1
#左前
vl53l1x start -X  -a 0x31 -R 6
# 右后
vl53l1x start -X  -a 0x32 -R 4
#上
vl53l1x start -X  -a 0x33 -R 24
#左
vl53l1x start -X  -a 0x34 -R 2
# 左后
vl53l1x start -X  -a 0x35 -R 0
#下
vl53l1x start -X  -a 0x36 -R 25
#右
vl53l1x start -X  -a 0x37 -R 7

修改地址

在pca9534 模块中修改各路传感器地址

	writeRegister(PCA9534_CONFIGURATION_REG,0x00);

	writeRegister(PCA9534_OUT_REG,0x01);
	set_device_address(VL53L1X_BASEADDR_DEFAULT);
	VL53L1_WrByte(VL53L1_I2C_SLAVE__DEVICE_ADDRESS,0x30);

	set_device_address(PCA9534_BASEADDR);
	writeRegister(PCA9534_OUT_REG,0x03);
	set_device_address(VL53L1X_BASEADDR_DEFAULT);
	VL53L1_WrByte(VL53L1_I2C_SLAVE__DEVICE_ADDRESS,0x31);

	set_device_address(PCA9534_BASEADDR);
	writeRegister(PCA9534_OUT_REG,0x07);
	set_device_address(VL53L1X_BASEADDR_DEFAULT);
	VL53L1_WrByte(VL53L1_I2C_SLAVE__DEVICE_ADDRESS,0x32);

	set_device_address(PCA9534_BASEADDR);
	writeRegister(PCA9534_OUT_REG,0x0F);
	set_device_address(VL53L1X_BASEADDR_DEFAULT);
	VL53L1_WrByte(VL53L1_I2C_SLAVE__DEVICE_ADDRESS,0x33);

	set_device_address(PCA9534_BASEADDR);
	writeRegister(PCA9534_OUT_REG,0x1F);
	set_device_address(VL53L1X_BASEADDR_DEFAULT);
	VL53L1_WrByte(VL53L1_I2C_SLAVE__DEVICE_ADDRESS,0x34);

	set_device_address(PCA9534_BASEADDR);
	writeRegister(PCA9534_OUT_REG,0x3F);
	set_device_address(VL53L1X_BASEADDR_DEFAULT);
	VL53L1_WrByte(VL53L1_I2C_SLAVE__DEVICE_ADDRESS,0x35);

	set_device_address(PCA9534_BASEADDR);
	writeRegister(PCA9534_OUT_REG,0x7F);
	set_device_address(VL53L1X_BASEADDR_DEFAULT);
	VL53L1_WrByte(VL53L1_I2C_SLAVE__DEVICE_ADDRESS,0x36);

	set_device_address(PCA9534_BASEADDR);
	writeRegister(PCA9534_OUT_REG,0xFF);
	set_device_address(VL53L1X_BASEADDR_DEFAULT);
	VL53L1_WrByte(VL53L1_I2C_SLAVE__DEVICE_ADDRESS,0x37);

获取数据

在需要用到距离信息的模块中订阅mavlink消息，完成传感器数据的解析并写入自定义变量

	for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) {
		if (_distance_sensor_subs[i].copy(&distance_sensor)) {	
			// only use the first instace which has the correct orientation
			if (distance_sensor.orientation == distance_sensor_s::ROTATION_UPWARD_FACING) {

				dis_up = distance_sensor.current_distance;

			}
		}

		if (_distance_sensor_subs[i].copy(&distance_sensor)) {
			// only use the first instace which has the correct orientation
			if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {

				dis_down = distance_sensor.current_distance;
			}
		}
	}