背景及需求
出于小型及轻量化考虑,使用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;
}
}
}