基于FreeRTOS的STM32四轴飞行器: 十六.激光测距定高功能
一.芯片介绍
激光测高芯片在飞控板下侧：
原理图如下：
型号为：VL53LX1，为国产仿制，使用I2C进行通信。
GPIO1为中断引脚，XSHUT为上拉关机引脚，如果给予低电平可以关闭芯片，高电平正常使用，每次上电初始化重启 芯片可以防止I2C时序不对。
手册解读：
I2C默认地址为0X52。
二.配置CubeMX
配置I2C2：
配置PB12引脚：
三.激光测距芯片驱动编写
移植驱动：
将原厂代码拷贝到项目。
初始化：
先拉低引脚重启芯片，接着使用API初始化芯片，之后设置距离模式长距或短距，之后设置测量速度测量频率，测量速度意思是从测量开始到结束需要的时间，测量频率是多久测一次需要大于等于测量速度，最后一步开始测量。
c
#define DEV 0x52
/**
* @description: 初始化激光测距芯片
* @return {*}
*/
void Inf_VL53LX1_Init(void)
{
/* 1. 先重启芯片 */
HAL_GPIO_WritePin(VL53LX1_SHUT_GPIO_Port, VL53LX1_SHUT_Pin, GPIO_PIN_RESET);
HAL_Delay(500);
HAL_GPIO_WritePin(VL53LX1_SHUT_GPIO_Port, VL53LX1_SHUT_Pin, GPIO_PIN_SET);
/* 2. 初始化化芯片 */
VL53L1X_SensorInit(DEV);
/* 3. 设置距离模式: 长或短 1:short 2:long*/
VL53L1X_SetDistanceMode(DEV, 2);
/* 4. 测量的速度 */
VL53L1X_SetTimingBudgetInMs(DEV, 20);
/* 5. 测量的频率 ms值必须大于等于上一个*/
VL53L1X_SetInterMeasurementInMs(DEV, 20);
/* 6. 开始测量 */
VL53L1X_StartRanging(DEV);
uint16_t sensorID;
VL53L1X_GetSensorId(DEV, &sensorID);
printf("sensorID:0x%x\r\n", sensorID);
}观察打印ID：
读取高度：
先判断是否准备好，如果准备好因为有中断机制先清除中断之后返回高度。
c
/**
* @description: 返回测到的高度
* @return {*}
*/
uint16_t Inf_VL53LX1_GetHeight(void)
{
static uint16_t height = 0;
uint8_t isDataReady;
/* 检测测距是否完成 */
VL53L1X_CheckForDataReady(DEV, &isDataReady);
if(isDataReady)
{
VL53L1X_ClearInterrupt(DEV);
/* 读取测距结果 */
VL53L1X_GetDistance(DEV, &height);
}
return height;
}获取飞机的飞行高度：
在获得后进行了测得高度的处理，判断是否发生了突变如果突变使用lastHeight。
c
/**
* @description: 获取飞机的飞行高度
* @return {*} 高度: mm
*/
uint16_t App_Flight_GetHeight(void)
{
static uint16_t lastHeight = 0;
uint16_t height = Inf_VL53LX1_GetHeight();
if (abs(height - lastHeight) > 500 || /* 如果有突变,则返回上次的值 */
abs(joyStick.PIT - 500) > 100 || /* 有水平飞行, 返回上次的值 */
abs(joyStick.ROL - 500) > 100)
{
return lastHeight;
}
height = Com_Filter_LowPass(height, lastHeight);
lastHeight = height;
return height;
}观察打印数据：
观察发现打印数据正常。
四.定高PID的计算
代码：
创建定高状态机 根据状态执行定高。
状态0：根据按键是否解锁进入状态1。
状态1：计算PID前的准备，设置期望值，之后进入状态2计算PID。
状态2：进入状态2时判断是否要解除定高，因为飞行任务执行周期为20ms，所以需要5次来计算一次PID，对Z轴速度数据进行互补滤波。
c
/**
* @description: 高度pid控制
* @param {Com_Status} isRemoteUnlocked
* @param {uint16_t} height
* @return {*}
*/
void App_Flight_PIDHeight(Com_Status isRemoteUnlocked, uint16_t height, float dt)
{
/* 定高状态机:
状态0: 检测是否定高
状态1: 当前的油门值是定高时的油门值 当前的高度: 固定的高度
状态2: 进行pid控制
*/
static uint8_t status = 0;
static uint16_t thrHold = 0;
static uint16_t heightHold = 0;
static float staticAcc = 0; /* 静态时z的加速度 */
if (isRemoteUnlocked == Com_OK && staticAcc == 0)
{
staticAcc = Common_IMU_GetNormAccZ();
}
switch (status)
{
case 0: /* 定高检测 */
{
/* pid重置 */
heightPID.result = 0;
zSpeedPID.result = 0;
if (isRemoteUnlocked == Com_OK && isFixHeight == Com_OK)
{
status = 1;
}
break;
}
case 1: /* pid计算前的准备 */
{
thrHold = joyStick.THR;
heightHold = height;
status = 2;
break;
}
case 2: /* pid计算 */
{
/* 定高时: 油门变化超过100, 或者定高的标记为0. 解除定高 */
if (abs(joyStick.THR - thrHold) > 100 || isFixHeight == Com_FAIL)
{
status = 0; /* 回到状态0 */
joyStick.isFixHeight = 0; /* 标记定高的变量置为0 */
isFixHeight = Com_FAIL;
}
else
{
/* 由于高度变动的周期20ms, 所以我们需要5次来计算一次pid */
static uint8_t cnt = 0;
cnt++;
if (cnt < 5)
return;
cnt = 0;
dt *= 5;
/* 对z的速度: 互补滤波 */
float zSpeed = 0.9 * (zSpeedPID.measure + (Common_IMU_GetNormAccZ() - staticAcc) * dt) +
0.1 * (height - heightPID.measure) / dt;
/*
串级pid
外环 高度环
内环 z方向的速度环
*/
heightPID.desire = heightHold;
heightPID.measure = height;
heightPID.dt = dt;
zSpeedPID.measure = zSpeed;
zSpeedPID.dt = dt;
Com_PID_CascadePID(&heightPID, &zSpeedPID);
}
break;
}
default:
break;
}
}设置两个定高PID 参数：
height为外环，speed为内环。
五.定高PID作用到电机上
代码：
将内环PID作用到最终的对象上，加上zPid对zPid进行限幅，注意需要在采集欧拉角后执行获取加速度的函数。
c
/**
* @description: 把定高的pid作用到motor上
* @param {Com_Status} isRemoteUnlocked
* @return {*}
*/
void App_Flight_MotorWithHeightPID(Com_Status isRemoteUnlocked)
{
int16_t zPid = LIMIT(zSpeedPID.result, -150, 150);
motorLeftTop.speed += zPid;
motorLeftBottom.speed += zPid;
motorRightTop.speed += zPid;
motorRightBottom.speed += zPid;
}