[项目]基于FreeRTOS的STM32四轴飞行器: 十六.激光测距定高功能

基于FreeRTOS的STM32四轴飞行器: 十六.激光测距定高功能

一.芯片介绍
二.配置CubeMX
三.激光测距芯片驱动编写
四.定高PID的计算
五.定高PID作用到电机上

一.芯片介绍

激光测高芯片在飞控板下侧：
在这里插入图片描述
原理图如下：
型号为：VL53LX1，为国产仿制，使用I2C进行通信。

GPIO1为中断引脚，XSHUT为上拉关机引脚，如果给予低电平可以关闭芯片，高电平正常使用，每次上电初始化重启芯片可以防止I2C时序不对。
在这里插入图片描述
手册解读：
I2C默认地址为0X52。

二.配置CubeMX

配置I2C2：
在这里插入图片描述
配置PB12引脚：

三.激光测距芯片驱动编写

移植驱动：
将原厂代码拷贝到项目。
在这里插入图片描述
初始化：
先拉低引脚重启芯片，接着使用API初始化芯片，之后设置距离模式长距或短距，之后设置测量速度测量频率，测量速度意思是从测量开始到结束需要的时间，测量频率是多久测一次需要大于等于测量速度，最后一步开始测量。

#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：
在这里插入图片描述
读取高度：
先判断是否准备好，如果准备好因为有中断机制先清除中断之后返回高度。

/*** @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。

/*** @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轴速度数据进行互补滤波。

/*** @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进行限幅，注意需要在采集欧拉角后执行获取加速度的函数。

/*** @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;
}