无人机
1. 项目概述
1.1 简介
本项目是基于STM32的微型四轴无人机，控制核心采用STM32F103C8T6，姿态运动传感器选择MPU6050。无人机通过Si24R1（NRF24L01）与控制器进行2.4G无线通信，实现了即时有效地接收控制器指令，通过串级PID进行姿态控制，从而在空间中实现自由移动。
1.2 功能描述
- 姿态控制
通过MPU6050获取三轴加速度和三轴角速度，经过四元数姿态解算得到三种倾角。利用串级PID算法控制四轴无人机平衡状态。
- 实时操作系统
移植FreeRTOS实现多任务调度和管理。
- 数据显示
把测得电池电压、摇杆大小、2.4G信道数据实时显示在液晶屏幕上。
- 遥控控制
遥控板与飞控板通过2.4G信号交互，发送指令，控制四轴无人机上升下降、前进后退、左右移动。
1.3 前置知识
一、基本常识
-
结构： X型 -> 相对于十字型，更稳定
-
欧拉角： 描述物体在空间中的旋转
具体 -> 三个角度： 俯仰角、横滚角、偏航角 -
姿态解算 -> 运动传感器的原始数据,经过计算,才能得到 欧拉角 -> 我们采用四元数姿态解算
二、PID算法
-
PID算法是一种自动控制理论
-
由三部分控制 :
P：比例控制，与误差值成比例
I：积分控制，对误差做积分（累加），消除稳态误差（系统静差）
D：微分控制，控制 误差的变化率，避免误差变化率过高 -> 抵消惯性，避免震荡 -
PID公式
误差值e = 测量值 - 期望值
PID输出 = P系数 * 误差 + I系数 * 误差的累加 + D系数 * 误差的变化率
= Kp * e + Ki * 累加(e * T) + Kd *(本次e - 上次的e)/时间间隔 -
串级PID
PID1 -> PID2 -> 目标物体
外环 内环
串级PID：外环PID的输出，输入给内环PID，作为内环的期望值串级好处： 效果更好
1）更稳定： 可以控制多个物理量
2）反应更快 -
无人机姿态控制的PID方案
1）控制目标：
俯仰姿态： 俯仰角=0度， 对应轴的角速度=0
横滚姿态： 横滚角=0度， 对应轴的角速度=0
偏航姿态： 偏航角=0度， 对应轴的角速度=0每个姿态做一组串级PID，一共需要三组串级PID2）设计串级PID
设计原则：
1）反应更灵敏的放内环： 角速度
2）数据更准确的放内环： 角速度
=》 角度需要用 角速度和加速度 一起计算得到，加速度噪音大， 角度结果准确性不如角速度外环：角度 内环：角速度
中比例部分就是PID中的P, 积分就是PID中的I, 微分就是PID中的D.
图中
，是期望值，
是系统的实际输出值，
给定值与实际输出值构成控制偏差：
所以模拟 PID 控制器的控制规律为:
其中: 
为控制器的比例系数, 
为控制器的积分系数, 
为控制器的微分系数.
三、采样周期
根据香农（Shannon） 采样定律 ：为不失真地复现信号的变化， 采样频率至少应大于或等于连续信号最高频率分量的2倍。
四、 电机
8520空心杯
直流、有刷
3.7V 、 几十mA
51000 rpm（每分钟 51000转）
不需要电调
怎么控制转速： 定时器输出pwm
固定方向：2个顺时针、2个逆时针
1.4 系统总体设计
1.5 硬件选型
①. 主控芯片
使用的是ST公司的STM32F103C8T6. 主频最高可达72MHz, SRAM有20K, Flash有64K. 定时器有4个.
②电机
- 8520空心杯电机
主要用于单浆遥控飞机和四轴飞行器,由顺时针旋转和逆时针旋转2种.(注意: 一种空心杯只能有一种转动方向)
尺寸: 8.5mm 直径，20mm 长度.
工作电压：通常为3.7V（单节锂电池）
无负载转速：在额定电压下，通常为几万转每分钟（RPM）
无负载电流：通常在几十毫安（mA）左右.
重量：几克，通常不超过10克.
使用PWM可以方便的驱动电机转动
③运动传感器
- MPU6050运动传感器
传感器的数据可以通过 I2C接口读取。
包含一个3 轴陀螺仪(测量角速度)和一个3 轴加速度计，并集成了一个数字运动处理器（DMP）。
需要注意的是, 内置的DMP处理的数据不够准确,我们是直接读取原始数据,在外面经过MCU来进行四元素和欧拉角的运算
④2.4G通信
- Si24R1
通过 SPI 接口
工作频率：2.4GHz ISM 频段.
数据传输速率：最高可达 2Mbps.
支持自动重传和自动确认.
多达 126数据信道,通常在几十米到数百米范围内
1.6 软件架构
2. 功能模块
公共层
- Com_Types.h
/* 解锁指令的不同阶段 */
#define UNLOCK_0 0
#define UNLOCK_1 1
#define UNLOCK_2 2
#define UNLOCK_PROCESS 3
/* 失联的计数值 */
#define DISCONNECT_COUNT 7500
typedef struct
{
uint16_t flashTime; // 控制闪烁的间隔时间, 单位毫秒
enum
{
AlwaysOn,
AlwaysOff,
AllFlash,
Warning,
Dangerous
} status;
} sLED;
typedef struct
{
short ax; // 加速度 x轴
short ay; // 加速度 y轴
short az; // 加速度 z轴
short gx; // 角速度 x轴
short gy; // 角速度 y轴
short gz; // 角速度 z轴
} sMPU;
typedef struct
{
float pitch; // 俯仰角
float roll; // 翻滚角
float yaw; // 偏航角
} sAngle;
typedef struct
{
int16_t THR; // 油门值
int16_t YAW; // 偏航值
int16_t ROL; // 横滚值
int16_t PIT; // 俯仰值
int16_t AUX1;
int16_t AUX2;
int16_t AUX3;
int16_t AUX4;
int16_t AUX5;
int16_t AUX6;
} sReceiveRemote;
动力模块
我们的电机驱动主要是靠定时器的PWM输出.
电机一般pwm频率多少合适？
建议：10k~20k
=》过小：抖动明显，听到噪声
=》过大：稳定，但是mos管扛不住
所以我们这里配置预分频为4,重装载为1000. PWM频率就为18k
配置完后便可先启动PWM并设定值验证一下:
/* 启动4个定时器 */
/*
左前：TIM3—CH1
左后：TIM4-CH4
右前：TIM2-CH2
右后：TIM1-CH3
*/
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_4);
/* =================测试pwm控制电机===================== */
// __HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_1,50);
// __HAL_TIM_SetCompare(&htim4,TIM_CHANNEL_4,50);
// __HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,50);
// __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_3,50);
角度获取模块
我们使用I2C与之连接,配置快速400k.
mpu6050的方向:
MPU6050寄存器使用:
- 0x6B — 电源管理 1
上电后，MPU-60X0 的时钟源默认是内部振荡器。不管怎么样，强烈建议设备修改它（或者使用外部时钟源）作为陀螺仪的基准时钟，以提高稳定性。时钟源可以根据下表选择：我们这里选择陀螺仪x轴时钟
- 0x19 — 采样频率分频器
该寄存器用于 MPU-60X0的陀螺仪采样频率输出设置。
采样频率=陀螺仪输出频率/（1+SMPLRT_DIV）
当 DLPF is disabled（DLPF_CFG=0 or 7），陀螺输出频率=8kHz；当 DLPF is enabled（see 寄存器 26），陀螺仪输出频率=1KHz。
我们设置采样频率为500较合适,所以1k二分频
- 0x1A
DLPF(数字低通滤波器)由 DLPF_CFG 配置。加速度计和陀螺仪根据 DLPF_CFG 的值被
过滤。下表显示过滤情况：
根据香农采样定理： 采样频率 >= 2*带宽
===> 带宽 <= 采样频率/2
- 0x43 ~ 0x48 陀螺仪测量值(高位在前)
- 0x3B ~ 0x40 加速度计测量值(高位在前)
接口层函数:
①初始化及获取寄存器数据
- Int_MPU6050.c
/**
* @brief 根据采样率,设置低通滤波器的带宽对应的配置值
*
* @param sampleRate 目标采样率
*/
void Int_MPU6050_SetDLPF(uint16_t sampleRate)
{
/* 带宽 <= 采样频率/2 */
uint16_t bandwidth = sampleRate / 2;
uint8_t cfg = 0;
if (188 < bandwidth)
{
cfg = 1;
}
else if (98 < bandwidth)
{
cfg = 2;
}
else if (42 < bandwidth)
{
cfg = 3;
}
else if (20 < bandwidth)
{
cfg = 4;
}
else if (10 < bandwidth)
{
cfg = 5;
}
else if (5 < bandwidth)
{
cfg = 6;
}
Int_MPU6050_WriteByte(MPU_CFG_REG, cfg << 0);
}
/**
* @brief 设置陀螺仪采样率 ==> 根据目标采样率,推算需要的分频值,设置到寄存器中
*
* @param sampleRate 陀螺仪采样率
*/
void Int_MPU6050_SetGyroSampleRate(uint16_t sampleRate)
{
/*对采样率做一个限幅: 4~1000
为什么最低是4,因为分频最大 = 2^8 = 256,1k/256 = 4
为什么最高是1k,因为分频最小是1,1k/1 = 1k
*/
if (sampleRate <= 4)
{
sampleRate = 4;
}
else if (sampleRate >= 1000)
{
sampleRate = 1000;
}
/*
开启低通滤波器,输出频率=1k
采样频率=陀螺仪输出频率/（分频值 + 1）
==> 分频值 = 1k/采样率 - 1
*/
uint8_t div = 0;
div = 1000 / sampleRate - 1;
Int_MPU6050_WriteByte(MPU_SAMPLE_RATE_REG, div);
/*根据设置的采样率,设置低通滤波器的带宽*/
Int_MPU6050_SetDLPF(sampleRate);
}
// 初始化
uint8_t Int_MPU6050_Init(void)
{
/*1.复位 -> 延时200ms -> 唤醒*/
Int_MPU6050_WriteByte(MPU_PWR_MGMT1_REG, 0x80);
HAL_Delay(200);
Int_MPU6050_WriteByte(MPU_PWR_MGMT1_REG, 0x00);
/*2.设置传感器的量程*/
/*2.1 设置陀螺仪的量程 +-2000°/s*/
Int_MPU6050_WriteByte(MPU_GYRO_CFG_REG, 3 << 3);
/*2.1 设置加速度计的量程 +-2g*/
Int_MPU6050_WriteByte(MPU_ACCEL_CFG_REG, 0 << 3);
/*3. 其他功能设置: FIFO 第二IIC 中断*/
/*3.1 关闭中断*/
Int_MPU6050_WriteByte(MPU_INT_EN_REG, 0x00);
/*3.2 关闭FIFO 第二IIC*/
Int_MPU6050_WriteByte(MPU_USER_CTRL_REG, 0x00);
/*3.3 若果要使用FIFO,可以详细配置FIFO
==> ①3.2中,FIFO功能保持开启
==> ②配置3.3的寄存器,分别设置哪个数据要走FIFO
*/
Int_MPU6050_WriteByte(MPU_FIFO_EN_REG, 0x00);
// 判断外设是否正常工作 读取器件ID
uint8_t dev_id = 0;
Int_MPU6050_ReadByte(MPU_DEVICE_ID_REG, &dev_id);
/*读取到ID.需要自己拼接最低位AD0的值,这里是0,所以直接|0即可*/
if ((dev_id | 0) == MPU_IIC_ADDR)
{
/*4. 系统时钟,陀螺仪采样率,启动传感器*/
/*4.1 设置MPU系统时钟为陀螺仪x轴时钟*/
Int_MPU6050_WriteByte(MPU_PWR_MGMT1_REG, 0x01);
/*4.2 陀螺仪采样率: 采样频率=陀螺仪输出频率/（1+SMPLRT_DIV）*/
//越快越好,但是需要考虑后续处理的时间 太快处理不过来,实际上2ms一次比较合适
Int_MPU6050_SetGyroSampleRate(500);
/*4.3 启动传感器*/
Int_MPU6050_WriteByte(MPU_PWR_MGMT2_REG, 0x00);
return 0;//初始化成功
}
else
{
return 1;//初始化失败
}
}
// 读取六轴数据
/**
* @brief 读取加速度计三轴数据,并且转换成带符号类型,符号表示方向
*
* @param accel_x
* @param accel_y
* @param accel_z
*/
void Int_MPU6050_GetAccel(int16_t *accel_x, int16_t *accel_y, int16_t *accel_z)
{
/*读取加速度的数据*/
uint8_t data[6];
Int_MPU6050_ReadBytes(MPU_ACCEL_XOUTH_REG, data, 6);
/*
data[0]:加速度x轴高字节
data[1]:加速度x轴低字节
data[2]:加速度y轴高字节
data[3]:加速度y轴低字节
data[4]:加速度z轴高字节
data[5]:加速度z轴低字节
*/
*accel_x = (int16_t)data[0] << 8 | data[1];
*accel_y = (int16_t)data[2] << 8 | data[3];
*accel_z = (int16_t)data[4] << 8 | data[5];
}
/**
* @brief 读取陀螺仪三轴数据,并且转换成带符号类型,符号表示方向
*
* @param gyro_x
* @param gyro_y
* @param gyro_z
*/
void Int_MPU6050_GetGyro(int16_t *gyro_x, int16_t *gyro_y, int16_t *gyro_z)
{
/*读取加速度的数据*/
uint8_t data[6];
Int_MPU6050_ReadBytes(MPU_GYRO_XOUTH_REG, data, 6);
/*
data[0]:角速度x轴高字节
data[1]:角速度x轴低字节
data[2]:角速度y轴高字节
data[3]:角速度y轴低字节
data[4]:角速度z轴高字节
data[5]:角速度z轴低字节
*/
*gyro_x = (int16_t)data[0] << 8 | data[1];
*gyro_y = (int16_t)data[2] << 8 | data[3];
*gyro_z = (int16_t)data[4] << 8 | data[5];
}
②零偏校准、滤波
零偏校准
z轴加速度，理想16384
其他五轴，理想是0初始有偏差，需要初始化完MPU后，立即计算偏差
实现： ① 判断是否静止： 取角速度前后两次差值，变化在规定的区间内，就认为静止（要求30次，3个轴都判断） ② 取偏差的平均值： 求和/次数（用到了位运算，效率更高） ③ 赋值给保存偏差的数组，后续原始数据 - 对应的偏差值滤波
模拟信号滤波：
电容： 滤电压
电感： 滤电流
数字信号滤波：
一阶低通滤波： k*上次的值 + （1-k）*本次的值
==> 角速度： 噪音没那么大，用这个就可以
卡尔曼滤波： 原理和算法复杂，要求掌握使用及效果即可。
==> 加速度： 噪音大，需要狠一点
- App_Flight.c
/**
* @description: 读取MPU六轴数据，并进行零偏校准、滤波处理
* @return {*}
*/
void App_Flight_GetMpuData(void)
{
/* 1、读取六轴数据 */
Int_MPU6050_GetAccel(&mpu6050.ax, &mpu6050.ay, &mpu6050.az);
Int_MPU6050_GetGyro(&mpu6050.gx, &mpu6050.gy, &mpu6050.gz);
// printf("=====MPU6050六轴原始数据=====\r\n");
// printf("加速度：\r\nax=%d\r\nay=%d\r\naz=%d\r\n", mpu6050.ax, mpu6050.ay, mpu6050.az);
// printf("角速度：\r\ngx=%d\r\ngy=%d\r\ngz=%d\r\n", mpu6050.gx, mpu6050.gy, mpu6050.gz);
/* 2、进行零偏校准 */
mpu6050.ax -= mpuOffsets[0];
mpu6050.ay -= mpuOffsets[1];
mpu6050.az -= mpuOffsets[2];
mpu6050.gx -= mpuOffsets[3];
mpu6050.gy -= mpuOffsets[4];
mpu6050.gz -= mpuOffsets[5];
// printf(">>>>>>>>>>>MPU6050六轴校准数据=====\r\n");
// printf("加速度：\r\nax=%d\r\nay=%d\r\naz=%d\r\n", mpu6050.ax, mpu6050.ay, mpu6050.az);
// printf("角速度：\r\ngx=%d\r\ngy=%d\r\ngz=%d\r\n", mpu6050.gx, mpu6050.gy, mpu6050.gz);
/* 3、进行滤波处理 */
OutData[0] = (float)mpu6050.ax; // x轴加速度，滤波前的值
/* 3.1 对加速度进行 简易卡尔曼滤波 */
Com_Kalman_1(&ekf[0], mpu6050.ax); // 对x轴加速度进行 一维卡尔曼滤波
mpu6050.ax = ekf[0].out; // 赋值滤波结果
Com_Kalman_1(&ekf[1], mpu6050.ay); // 对y轴加速度进行 一维卡尔曼滤波
mpu6050.ay = ekf[1].out; // 赋值滤波结果
Com_Kalman_1(&ekf[2], mpu6050.az); // 对z轴加速度进行 一维卡尔曼滤波
mpu6050.az = ekf[2].out; // 赋值滤波结果
OutData[1] = (float)mpu6050.ax; // x轴加速度，滤波后的值
/* 3.2 对角速度进行 一阶低通滤波 */
OutData[2] = (float)mpu6050.gx; // x轴角速度，滤波前的值
/* 一阶低通滤波：k取值（0~1）
本次的值 = k * 上次的值 + （1-k） * 本次的值
*/
static short last_gyro[3] = {0}; // 0-x,1-y,2-z
mpu6050.gx = (0.85 * last_gyro[0]) + (0.15 * mpu6050.gx); // 对x轴角速度做一阶低通滤波
last_gyro[0] = mpu6050.gx; // 保存本次的值，方便下轮使用
mpu6050.gy = (0.85 * last_gyro[1]) + (0.15 * mpu6050.gy); // 对y轴角速度做一阶低通滤波
last_gyro[1] = mpu6050.gy; // 保存本次的值，方便下轮使用
mpu6050.gz = (0.85 * last_gyro[2]) + (0.15 * mpu6050.gz); // 对z轴角速度做一阶低通滤波
last_gyro[2] = mpu6050.gz; // 保存本次的值，方便下轮使用
OutData[3] = (float)mpu6050.gx; // x轴角速度，滤波后的值
/* 调用虚拟数字示波器的发送函数,注意不看的时候注释掉 */
// OutPut_Data();
}
/**
* @description: 计算MPU六轴初始偏差值
* @return {*}
*/
void App_Flight_MpuOffset(void)
{
/* 定义静止的差值范围：-5~5 */
int16_t MIN_GYRO_BIAS = -5;
int16_t MAX_GYRO_BIAS = 5;
uint8_t gyro_static_count = 30;
short gyro_last[3] = {0}; // 保存上一次陀螺仪的值，对应三个轴
short gyro_bias[3] = {0}; // 与上一次陀螺仪数据的差值，用于判断是否静止
int32_t offset_sum[6] = {0}; // 六轴偏差值的和，用于计算平均值
/* 1、判断是否处于静止 =》 角速度前后两次差值，小于阈值，认为静止，要求多次满足这个条件 */
while (gyro_static_count--)
{
/* 要求满足30次静止条件，不满足静止就循环卡住继续判断 */
do
{
HAL_Delay(2); // 对应采样率500hz，2ms读一次
App_Flight_GetMpuData(); // 获取最新数据
gyro_bias[0] = mpu6050.gx - gyro_last[0]; // 计算 角速度x轴 前后两次的变化值
gyro_bias[1] = mpu6050.gy - gyro_last[1]; // 计算 角速度y轴 前后两次的变化值
gyro_bias[2] = mpu6050.gz - gyro_last[2]; // 计算 角速度z轴 前后两次的变化值
gyro_last[0] = mpu6050.gx; // 保存本次 角速度x轴数据，方便下次使用
gyro_last[1] = mpu6050.gy; // 保存本次 角速度y轴数据，方便下次使用
gyro_last[2] = mpu6050.gz; // 保存本次 角速度z轴数据，方便下次使用
} while ( // 如果不满足静止条件，就继续循环判断，不放过
gyro_bias[0] < MIN_GYRO_BIAS || gyro_bias[0] > MAX_GYRO_BIAS || gyro_bias[1] < MIN_GYRO_BIAS || gyro_bias[1] > MAX_GYRO_BIAS || gyro_bias[2] < MIN_GYRO_BIAS || gyro_bias[2] > MAX_GYRO_BIAS);
}
/* 2、计算偏差值,求偏差的平均值 */
for (uint16_t i = 0; i < 356; i++)
{
/* 保守处理：前100次不计算，但是要读取最新数据 */
HAL_Delay(2);
App_Flight_GetMpuData();
if (i >= 100)
{
offset_sum[0] += (mpu6050.ax - 0);
offset_sum[1] += (mpu6050.ay - 0);
offset_sum[2] += (mpu6050.az - 16384);
offset_sum[3] += (mpu6050.gx - 0);
offset_sum[4] += (mpu6050.gy - 0);
offset_sum[5] += (mpu6050.gz - 0);
}
}
/* 3、保存到偏差数组mpuOffsets中 */
for (uint8_t i = 0; i < 6; i++)
{
/* 求平均值 = 和/256 ===> 除以256等价于 右移8位 */
mpuOffsets[i] = offset_sum[i] >> 8;
}
}
- Com_Filter.c
struct _1_ekf_filter ekf[3] = {
{0.02, 0, 0, 0, 0.001, 0.543},
{0.02, 0, 0, 0, 0.001, 0.543},
{0.02, 0, 0, 0, 0.001, 0.543}
};
void Com_Kalman_1(struct _1_ekf_filter *ekf,float input) //一维卡尔曼
{
ekf->Now_P = ekf->LastP + ekf->Q;
ekf->Kg = ekf->Now_P / (ekf->Now_P + ekf->R);
ekf->out = ekf->out + ekf->Kg * (input - ekf->out);
ekf->LastP = (1-ekf->Kg) * ekf->Now_P ;
}
- Com_Filter.h
struct _1_ekf_filter
{
float LastP; //上一时刻的状态方差（或协方差）
float Now_P; //当前时刻的状态方差（或协方差）
float out; //滤波器的输出值，即估计的状态
float Kg; //卡尔曼增益，用于调节预测值和测量值之间的权重
float Q; //过程噪声的方差，反映系统模型的不确定性
float R; //测量噪声的方差，反映测量过程的不确定性
};
extern struct _1_ekf_filter ekf[3];
extern void Com_Kalman_1(struct _1_ekf_filter *ekf,float input); //一维卡尔曼
x轴加速度滤波效果:(一阶卡尔曼)
x轴角速度滤波效果:(一阶低通滤波)
③ 获取欧拉角
我们用四元数解算欧拉角,所以需要移植一份四元数解算欧拉角的代码.这里直接移植稍加修改即可,便不赘述
无线通信模块2.4G — SI24R1
数据速率：2Mbps/1Mbps/250Kbps
最高 10MHz 四线 SPI 接口
芯片最多可以同时存三个有效数据包，当 FIFO 已满，接收到的数据包被自动丢掉。
ARQ 包格式:
我们自己发的数据最多只能发送32字节,我们使用了28字节
ACK 通信模式:
ACK PAYLOAD 通信模式:
收发器可同时进行 6 个发送端，1 个接收端之间的双向或单向通信。发射端如果需要接收 ACK 信号，还需要设置其接收管道 0 的地址与自身发送地址相同。
移植官方驱动:
修改:
- 判断发送完成: 官方用的是判断中断引脚IRQ,但是实测没作用,所以我们这里使用标志位判断
- 由于硬件电器性能不稳定,所以调用初始化之前先delay一次啊,我们还做了一个自检函数,delay后自检一下确定没问题才初始化
- Int_SI24R1.c
...
/**
* @description: 发送一个数据包
* @param {uint8_t} *txBuf
* @return {*}
*/
uint8_t NRF24L01_TxPacket(uint8_t *txBuf)
{
uint8_t status = 0;
/* 1、使用写Tx FIFO指令，将数据发送 */
NRF24L01_CE_LOW;
NRF24L01_Write_Buf(WR_TX_PLOAD, txBuf, TX_PLOAD_WIDTH);
NRF24L01_CE_HIGH; // 确保进入发射模式
/* 2、判断发送完成（或达到最大重发次数）：循环读取状态寄存器，并判断bit4、bit5 */
while (!(status & (TX_OK | MAX_TX)))
{
status = NRF24L01_Read_Reg(SPI_READ_REG + STATUS);
}
/* 3、 清空 接收或最大重发次数 中断标志位 */
NRF24L01_Write_Reg(SPI_WRITE_REG + STATUS, status);
/* 4、达到最大重发次数，就要主动清除Tx FIFO，否则无法继续发送 */
if (status & MAX_TX)
{
if (status & 0x01) // bit0: 如果TX FIFO满了，=1，没满=0
{
NRF24L01_Write_Reg(FLUSH_TX, 0xff);
}
}
/* 5、如果是发送成功 */
if (status & TX_OK)
{
return 0;
}
return 1; // 其他原因未成功
}
/**
* @description: 自检
* @return {*}
*/
uint8_t NRF24L01_Check()
{
uint8_t buff_w[5] = {0xA5, 0xA5, 0xA5, 0xA5, 0xA5};
uint8_t buff_r[5] = {0};
uint8_t count = 0;
/* 1、往寄存器写入几个字节 */
NRF24L01_Write_Buf(SPI_WRITE_REG + TX_ADDR, buff_w, 5);
/* 2、从寄存器读出几个字节 */
NRF24L01_Read_Buf(SPI_READ_REG + TX_ADDR, buff_r, 5);
/* 3、判断是否相同 */
for (uint8_t i = 0; i < 5; i++)
{
if (buff_r[i] == buff_w[i])
{
count++;
}
}
if (count == 5)
{
return 0; // 校验成功
}
else
{
return 1; // 校验失败
}
}
遥感数据采集和发送 — 遥控器
开启ADC1,我们使用ADC1的IN1,IN2,IN3,IN6通道获取遥感数据.配置为规则通道组的循环扫描模式,采样时间设置为: 7.5Cycle + 12.5Cycle个转化时间 -> 采集一个数据总共需要20个Cycle
ADC最高不能超过14M,我们6分频设置为12M
开启DMA1(通道1)传输数据,注意要关闭DMA中断,因为DMA中断传输一个数据会产生两个中断,如果存储数据的数组过小就会卡在启动DMA那行函数中
ADC采用12位存储右对齐,所以取值范围是:0~4095
摇杆电路:
对于遥感的原始数据范围0-4095是不方便我们直接使用的,所以我们需要做些处理:
极性&大小限幅:让遥感数据的极性符合常人,取值范围在1000-2000,不要和零打交道,航空标准都是这样取的
大小限幅&中点限幅:不能<1000也不能>2000;中点限幅是为了防止静止时微小变化或摇杆滑丝产生误差
校准:油门打到最低,然后按下按键校准,防止滑丝产生的误差
滑动窗口滤波:对于油门这种控制量不能让他突变
- App_Remote.c
extern uint16_t adc_values[4];
sRemote remote;
sOffset offsets;
sWindow thrWindow, pitWindow, rolWindow, yawWindow;
/**
* @description: 对摇杆值做大小限幅[1000,2000]
* @return {*}
*/
void App_Remote_Limit(void)
{
remote.THR = LIMIT(remote.THR, 1000, 2000);
remote.YAW = LIMIT(remote.YAW, 1000, 2000);
remote.PIT = LIMIT(remote.PIT, 1000, 2000);
remote.ROL = LIMIT(remote.ROL, 1000, 2000);
}
/**
* @description: 中点限幅
* @return {*}
*/
void App_Remote_MidLimit(void)
{
remote.PIT = MID_LIMIT(remote.PIT, 1500, 1470, 1530);
remote.ROL = MID_LIMIT(remote.ROL, 1500, 1470, 1530);
remote.YAW = MID_LIMIT(remote.YAW, 1500, 1470, 1530);
remote.THR = MID_LIMIT(remote.THR, 1500, 1470, 1530);
}
/**
* @description: 手动校准功能
* @return {*}
*/
void App_Remote_Calibration(void)
{
static uint16_t time_count = 0;
static uint8_t press_flag = 0;
static int32_t offset_sum[4] = {0};
static uint8_t sum_count = 0;
/* 1、判断按键长按、不松手只算1次 */
if (!READ_KEY_LEFT_X)
{
if ((!READ_KEY_LEFT_X) && (time_count++ > 50) && (press_flag == 0))
{
/* 说明长按了,开始执行校准 */
if (sum_count == 0)
{
/* 1、第一次进来，初始化 */
memset(offset_sum, 0, sizeof(int32_t) * 4);
sum_count++;
}
else if (sum_count < 51)
{
/* 2、计算偏差的和： 累加（当前值-期望值） */
offset_sum[0] += (remote.THR - 1000);
offset_sum[1] += (remote.PIT - 1500);
offset_sum[2] += (remote.ROL - 1500);
offset_sum[3] += (remote.YAW - 1500);
sum_count++;
}
else
{
/* 3、保存平均偏差值 */
/* 求平均值 */
offsets.THR = offset_sum[0] / 50;
offsets.PIT = offset_sum[1] / 50;
offsets.ROL = offset_sum[2] / 50;
offsets.YAW = offset_sum[3] / 50;
/* 关键点：应该在满足要求后，立即对标志位赋值！！！！！！！！ */
press_flag = 1;
}
}
}
else
{
/* 松手状态，对标志位进行清零 */
time_count = 0;
press_flag = 0;
sum_count = 0;
}
}
/**
* @description: 滑动窗口滤波
* @return {*}
*/
void App_Remote_WindowFilter(void)
{
static uint8_t index = 0;
/* 1、窗口的总和 - 旧的值 */
thrWindow.windowSum -= thrWindow.windowArray[index];
pitWindow.windowSum -= pitWindow.windowArray[index];
rolWindow.windowSum -= rolWindow.windowArray[index];
yawWindow.windowSum -= yawWindow.windowArray[index];
/* 2、将新的值放入对应的index中 */
thrWindow.windowArray[index] = remote.THR;
pitWindow.windowArray[index] = remote.PIT;
rolWindow.windowArray[index] = remote.ROL;
yawWindow.windowArray[index] = remote.YAW;
/* 3、窗口的总和 + 新的值 */
thrWindow.windowSum += thrWindow.windowArray[index];
pitWindow.windowSum += pitWindow.windowArray[index];
rolWindow.windowSum += rolWindow.windowArray[index];
yawWindow.windowSum += yawWindow.windowArray[index];
/* 4、求平均值 = 总和/窗口长度 */
remote.THR = thrWindow.windowSum / WINDOW_SIZE;
remote.PIT = pitWindow.windowSum / WINDOW_SIZE;
remote.ROL = rolWindow.windowSum / WINDOW_SIZE;
remote.YAW = yawWindow.windowSum / WINDOW_SIZE;
index++;
if (index >= WINDOW_SIZE)
{
index = 0;
}
}
/**
* @description: 处理摇杆数据
* @return {*}
*/
void App_Remote_StickScan(void)
{
/* 1、极性、取值范围、提升区间（1000,2000） */
remote.THR = 2000 - 0.25 * adc_values[0];
remote.YAW = 2000 - 0.25 * adc_values[1];
remote.PIT = 2000 - 0.25 * adc_values[2];
remote.ROL = 2000 - 0.25 * adc_values[3];
/* 查看波形：滤波前的值 */
OutData[0] = (float)remote.THR;
OutData[2] = (float)remote.PIT;
/* 2、进行滤波：避免油门突变，安全性处理 */
App_Remote_WindowFilter();
/* 查看波形：滤波后的值 */
OutData[1] = (float)remote.THR;
OutData[3] = (float)remote.PIT;
/* 3、手动触发校准：长按左上按键 */
App_Remote_Calibration();
remote.THR -= offsets.THR;
remote.PIT -= offsets.PIT;
remote.ROL -= offsets.ROL;
remote.YAW -= offsets.YAW;
/* 4、大小限幅 */
App_Remote_Limit();
/* 5、中点限幅 */
App_Remote_MidLimit();
/* 查看波形：发送到串口 */
// OutPut_Data();
}
...
油门滤波效果:
按键微调摇杆值 — 遥控器
- App_Remote.c
...
/**
* @description: 按键处理
* @return {*}
*/
void App_Remote_Key(void)
{
/* 计时变量 */
static uint8_t up_time_count = 0;
static uint8_t down_time_count = 0;
static uint8_t left_time_count = 0;
static uint8_t right_time_count = 0;
/* 按下不松手，算1次的标志位 */
static uint8_t up_flag = 0;
static uint8_t down_flag = 0;
static uint8_t left_flag = 0;
static uint8_t right_flag = 0;
/* 1、KEY_UP处理 */
if (!READ_KEY_UP)
{
if ((!READ_KEY_UP) && (up_time_count++ >= 1) && (up_flag == 0))
{
/* 按键结果，作用于偏差值上 */
offsets.PIT -= 20;
/* 长短只算1次的标志位 */
up_flag = 1;
}
}
else
{
up_time_count = 0;
up_flag = 0;
}
/* 2、KEY_DOWN处理 */
if (!READ_KEY_DOWN)
{
if ((!READ_KEY_DOWN) && (down_time_count++ >= 1) && (down_flag == 0))
{
/* 按键结果，作用于偏差值上 */
offsets.PIT += 20;
/* 长短只算1次的标志位 */
down_flag = 1;
}
}
else
{
down_time_count = 0;
down_flag = 0;
}
/* 3、KEY_LEFT处理 */
if (!READ_KEY_LEFT)
{
if ((!READ_KEY_LEFT) && (left_time_count++ >= 1) && (left_flag == 0))
{
/* 按键结果，作用于偏差值上 */
offsets.ROL += 20;
/* 长短只算1次的标志位 */
left_flag = 1;
}
}
else
{
left_time_count = 0;
left_flag = 0;
}
/* 4、KEY_RIGHT处理 */
if (!READ_KEY_RIGHT)
{
if ((!READ_KEY_RIGHT) && (right_time_count++ >= 1) && (right_flag == 0))
{
/* 按键结果，作用于偏差值上 */
offsets.ROL -= 20;
/* 长短只算1次的标志位 */
right_flag = 1;
}
}
else
{
right_time_count = 0;
right_flag = 0;
}
}
打包数据 — 遥控器
按照我们规定的协议发送:
- App_Remote.c
/**
* @description: 按照定义的通信协议格式，打包数据
* @param {uint8_t *} buff 存储数据的数组地址
* @return {*}
*/
void App_Remote_PacketData(uint8_t *buff)
{
/* 帧头（2字节）+功能字（1字节）+长度字（1字节）+ 负载数据（20字节）+ 校验和（4字节） */
uint8_t index = 0;
uint32_t check_sum = 0;
buff[index++] = 0xAA; // 帧头第一个字节
buff[index++] = 0xAF; // 帧头第二个字节
buff[index++] = 0x03; // 功能字
buff[index++] = 0x00; // 长度字
/* 有效数据 */
buff[index++] = (uint8_t)(remote.THR >> 8); // THR油门高字节
buff[index++] = (uint8_t)remote.THR; // THR油门低字节
buff[index++] = (uint8_t)(remote.YAW >> 8); // YAW油门高字节
buff[index++] = (uint8_t)remote.YAW; // YAW油门低字节
buff[index++] = (uint8_t)(remote.ROL >> 8); // ROL油门高字节
buff[index++] = (uint8_t)remote.ROL; // ROL油门低字节
buff[index++] = (uint8_t)(remote.PIT >> 8); // PIT油门高字节
buff[index++] = (uint8_t)remote.PIT; // PIT油门低字节
/* 计算有效数据的字节数，写回“长度字” */
buff[3] = index - 4;
/* 计算校验和 */
for (uint8_t i = 0; i < index; i++)
{
check_sum += buff[i];
}
/* 写入校验和 */
buff[index++] = (uint8_t)(check_sum >> 24);
buff[index++] = (uint8_t)(check_sum >> 16);
buff[index++] = (uint8_t)(check_sum >> 8);
buff[index++] = (uint8_t)check_sum;
}
解析数据 — 飞控
- App_Flight.c
...
/**
* @description: 解析校验2.4G模块接收到的数据
* @param {uint8_t *} recData 2.4G接收数据的数组地址
* @param {uint8_t} size 接收的字节数（一次）
* @return {*}
*/
void App_Flight_CheckData(uint8_t *recData, uint8_t size)
{
/* 帧头（2字节）+功能字（1字节）+长度字（1字节）+ 负载数据（20字节）+ 校验和（4字节） */
uint32_t my_sum = 0;
uint32_t check_sum = 0;
/* 1、校验帧头：校验合法性 */
if (recData[0] != 0xA1 || recData[1] != 0x77)
{
/* 帧头错误，直接return */
printf("帧头错误，收到的帧头=%02x+%02x\r\n", recData[0], recData[1]);
return;
}
/* 2、校验校验和：判断数据的正确性、完整性 */
/* 2.1 取出数据里的校验和 */
check_sum = ((uint32_t)recData[size - 4] << 24) | ((uint32_t)recData[size - 3] << 16) | ((uint32_t)recData[size - 2] << 8) | ((uint32_t)recData[size - 1]);
/* 2.2 自己重新算一遍校验和 */
for (uint8_t i = 0; i < size - 4; i++)
{
my_sum += recData[i];
}
/* 2.3 比较两者是否一致 */
if (my_sum != check_sum)
{
printf("校验和校验失败，check_sum=%d,my_sum=%d\r\n", check_sum, my_sum);
/* 校验和不一致，直接return */
return;
}
/* 3、根据功能字，对应的进行解析 */
if (recData[2] == 0x03)
{
/* 功能 0x03 : 控制数据 */
/* 摇杆数据 */
receiveRemote.THR = (int16_t)recData[4] << 8 | recData[5];
receiveRemote.YAW = (int16_t)recData[6] << 8 | recData[7];
receiveRemote.ROL = (int16_t)recData[8] << 8 | recData[9];
receiveRemote.PIT = (int16_t)recData[10] << 8 | recData[11];
/* 辅助通道数据 */
receiveRemote.AUX1 = (int16_t)recData[12] << 8 | recData[13];
/* >>>>>>>>>>>>>>>>>>>>>>>>>定高处理，标志位赋值<<<<<<<<<<<<<<<<<<<<<<<<<<<< */
isFixedHeight = receiveRemote.AUX1;
receiveRemote.AUX2 = (int16_t)recData[14] << 8 | recData[15];
receiveRemote.AUX3 = (int16_t)recData[16] << 8 | recData[17];
receiveRemote.AUX4 = (int16_t)recData[18] << 8 | recData[19];
receiveRemote.AUX5 = (int16_t)recData[20] << 8 | recData[21];
receiveRemote.AUX6 = (int16_t)recData[22] << 8 | recData[23];
}
}
OLED显示 — 遥控器
由于引脚受限,所以我们采用了软件模拟SPI的方式驱动OLED
oled显示直接移植驱动,调用即可,不用展开说,不过这里分享一个显示进度条的函数:
使用’|'来表示,粗略计算每个进度条大约需要41个竖线,1000个值÷41≈24,所以油门值变化24,进度条才变化1%
void OLED_Show_progress_bar(uint8_t temp,uint8_t chr_star,uint8_t chr_default,uint8_t x,uint8_t y,uint8_t size,uint8_t mode)
{
switch(temp)
{
case 0:OLED_Show_CH(x,y,chr_star+temp,size,size);break;
case 1:OLED_Show_CH(x,y,chr_star+temp,size,size);break;
case 2:OLED_Show_CH(x,y,chr_star+temp,size,size);break;
case 3:OLED_Show_CH(x,y,chr_star+temp,size,size);break;
case 4:OLED_Show_CH(x,y,chr_star+temp,size,size);break;
case 5:OLED_Show_CH(x,y,chr_star+temp,size,size);break;
case 6:OLED_Show_CH(x,y,chr_star+temp,size,size);break;
case 7:OLED_Show_CH(x,y,chr_star+temp,size,size);break;
case 8:OLED_Show_CH(x,y,chr_star+temp,size,size);break;
case 9:OLED_Show_CH(x,y,chr_star+temp,size,size);break;
case 10:OLED_Show_CH(x,y,chr_star+temp,size,size);break;
case 11:OLED_Show_CH(x,y,chr_star+temp,size,size);break;
case 12:OLED_Show_CH(x,y,chr_star+temp,size,size);break;
default:OLED_Show_CH(x,y,chr_default,size,size);break;
}
}
安全处理1 — 解锁指令
解锁指令： 避免一开机就转，非常危险
实现： 状态机写法 ===》 常用 switch case
指令： 油门最低 -> 油门最高1s -> 油门最低 -> 退出机制，回到第一个状态
注意：最后一步让油门最低
维护了一个标志位（其他函数可以去判断是否解锁）
- App_Flight.c
/* 解锁指令的不同阶段 */
#define UNLOCK_0 0
#define UNLOCK_1 1
#define UNLOCK_2 2
#define UNLOCK_PROCESS 3
/**
* @description: 安全处理：解锁指令处理
* @return {*}
*/
void App_Flight_Unlock(void)
{
/* 指令：油门最低 ---》 油门最高1s ---》 油门最低 */
static uint8_t status = UNLOCK_0;
static uint16_t thr_time_count = 0; // 油门保持的计时变量
switch (status)
{
case UNLOCK_0:
// printf("当前状态=UNLOCK_0\r\n");
/* 锁定状态：灯常亮 */
led.status = AlwaysOn;
/* 如果满足切换到状态2的条件，那么status=1 */
/* 判断油门是否最低，如果是==》切换到下一个状态，status=1 */
if (receiveRemote.THR <= 1020 && receiveRemote.THR >= 1000)
{
// printf("UNLOCK_0中满足油门最低....\r\n");
/* 满足油门最低的要求，切换到下一个状态 */
status = UNLOCK_1;
}
break;
case UNLOCK_1:
// printf("当前状态=UNLOCK_1\r\n");
/* 如果满足切换到状态3的条件，那么status=2 */
/* 判断油门是否最高且保持1s，如果是==》切换到下一个状态，status=2 */
if (receiveRemote.THR >= 1980)
{
/* 满足油门最高且保持1s的要求，切换到下一个状态 */
/* 该函数4ms调用1次，4ms * 250 = 1s */
if (thr_time_count++ >= 250)
{
// printf("UNLOCK_1中满足油门最高且保持1s....\r\n");
status = UNLOCK_2;
/* 达到要求，切换到下一个状态，要把计时清零 */
thr_time_count = 0;
}
}
else
{
/* 不到1秒，油门推走了，要把计时变量清零 */
thr_time_count = 0;
}
break;
case UNLOCK_2:
// printf("当前状态=UNLOCK_2\r\n");
/* 判断油门是否最低，如果是==》切换到下一个状态，status=3 */
if (receiveRemote.THR <= 1020)
{
// printf("UNLOCK_2中满足油门最低....\r\n");
status = UNLOCK_PROCESS;
/* 将解锁标志位置1 */
isUnlock = 1;
/* 正式解锁，灯控= 慢闪 */
led.status = AllFlash;
led.flashTime = 1000;
}
break;
case UNLOCK_PROCESS:
// printf("当前状态=UNLOCK_PROCESS\r\n");
/* 正式解锁 */
/* 特定条件，恢复成锁定状态 */
/* 1、如果长时间保持低油门，恢复成锁定状态 */
if (receiveRemote.THR <= 1020)
{
if (thr_time_count++ >= 7500)
{
// printf("30s低油门，恢复锁定....\r\n");
/* 长时间低油门 */
status = UNLOCK_0;
/* 解锁的标志位置0 */
isUnlock = 0;
/* 恢复锁定状态，要把计时变量清零 */
thr_time_count = 0;
}
}
else
{
/* 30s之内，不再是低油门，计时也要清零 */
thr_time_count = 0;
}
/* 2、如果其他任务需要紧急锁定，根据标志位来判断 */
if (isUnlock == 0)
{
// printf("紧急情况，恢复锁定....\r\n");
/* 恢复成锁定状态 */
status = UNLOCK_0;
}
break;
default:
break;
}
}
安全处理2 — 失联处理
失联处理
① 判断是否失联： 超过30s 2.4G模块没有收到新数据，就认为失联了
-> 维护一个计时变量
-> 如果收到新数据，变量=0；正常解析数据
-> 其他时候，变量++
② 失联具体处理：
位移控制复位： 俯仰摇杆值、横滚摇杆值、偏航的摇杆值 = 1500
油门缓慢减少： 每秒钟 减少20， 一直减到<=1000
恢复锁定，尝试重连：
-> 解锁标志位=0
->2.4G模块，重新 自检、初始化 （死马当活马医）
- App_Flight.c
/**
* @description: 遥控数据处理：包含 失联处理、解析校验、解锁指令
* @param {uint8_t} isConnect 收到数据的标志位：0-收到，1-未收到
* @return {*}
*/
void App_Flight_RemoteHandle(uint8_t isConnect)
{
static uint16_t disconnect_time_count = 0;
static uint8_t thr_time_count = 0;
/* 1、判断是否失联：30s长时间没有接收到新数据 */
disconnect_time_count++;
if (isConnect == 0)
{
/* 收到新数据，就清零计时变量 */
disconnect_time_count = 0;
App_Flight_CheckData(RX_BUFF, RX_PLOAD_WIDTH);
}
App_Flight_Unlock();
if (disconnect_time_count >= 60000)
{
/* 长时间失联，考虑计时变量越界，复位到超时的最小值 */
disconnect_time_count = DISCONNECT_COUNT;
}
/* 2、失联处理：
① 位移控制复位：俯仰、横滚、偏航 = 1500
② 油门处理： 缓慢降低到1000
③ 油门降下来后，恢复锁定，isUnlock = 0;
*/
if (disconnect_time_count >= DISCONNECT_COUNT)
{
// printf("失联中.....\r\n");
/* 失联了 */
/* 2.1 位移控制复位 */
receiveRemote.PIT = 1500;
receiveRemote.ROL = 1500;
receiveRemote.YAW = 1500;
/* 2.2 缓慢降低油门 */
if (receiveRemote.THR > 1000)
{
if (thr_time_count++ >= 250)
{
// printf(">>>>>>>>>>>>>>>>>油门降低20.....\r\n");
/* 每秒钟油门值减20 */
receiveRemote.THR -= 20;
/* 对计时变量清零 */
thr_time_count = 0;
}
}
else
{
/* 油门<=1000 */
receiveRemote.THR = LIMIT(receiveRemote.THR, 1000, 2000);
/* 2.3 恢复锁定 */
// printf("失联，恢复锁定....\r\n");
isUnlock = 0;
/* 2.4 额外处理：尝试重新初始化2.4G模块 */
while (NRF24L01_Check())
{
};
// printf("2.4G模块自检通过....\r\n");
NRF24L01_RX_Mode();
// printf("2.4G模块初始化为接收模式成功....\r\n");
}
}
}
定高 — VL53L1X
基本参数:IIC通信 400K快速模式
有效4m高度
数据刷新:50HZ
16位存储器,设备地址0x52(左移后的)
大端存储:高位在前 小端存储:低位在前 内存中存储默认小端存储,低位在前 而VL53L1X默认高位先行,高位在前,读写都是高位先行写驱动:vl53l1_platform.c
由于它是16位,高位先行,所以读写时要注意字节序
写入之前,先调整字节顺序
读取之后调整字节顺序
- 先进行官方库的移植:将图示中官方文件中的这两个文件夹下的文件全部copy到自定义的过程文件夹中
- 完成平台适配文件:
- vl53l1_platform.c
/**
* @description: 连续写入多个字节
* @param {uint16_t} dev 设备地址：！！！已经左移好的地址
* @param {uint16_t} index 寄存器地址，16位
* @param {uint8_t} *pdata 要写入的数据的地址
* @param {uint32_t} count 要写入的字节数
* @return {*}
*/
int8_t VL53L1_WriteMulti(uint16_t dev, uint16_t index, uint8_t *pdata, uint32_t count)
{
return HAL_I2C_Mem_Write(&hi2c2, dev, index, I2C_MEMADD_SIZE_16BIT, pdata, count, 1000);
}
/**
* @description: 连续读取多个字节
* @param {uint16_t} dev 设备地址：！！！已经左移好的地址
* @param {uint16_t} index 寄存器地址，16位
* @param {uint8_t} *pdata 接收读取的多个字节的数组的地址
* @param {uint32_t} count 要读取的字节数
* @return {*}
*/
int8_t VL53L1_ReadMulti(uint16_t dev, uint16_t index, uint8_t *pdata, uint32_t count)
{
return HAL_I2C_Mem_Read(&hi2c2, dev, index, I2C_MEMADD_SIZE_16BIT, pdata, count, 1000);
}
/**
* @description: 写入一个字节
* @param {uint16_t} dev
* @param {uint16_t} index
* @param {uint8_t} data 要写入的一个字节
* @return {*}
*/
int8_t VL53L1_WrByte(uint16_t dev, uint16_t index, uint8_t data)
{
return HAL_I2C_Mem_Write(&hi2c2, dev, index, I2C_MEMADD_SIZE_16BIT, &data, 1, 1000);
}
int8_t VL53L1_WrWord(uint16_t dev, uint16_t index, uint16_t data)
{
/* 写入之前，先调整字节顺序 */
REVERS_WORD(&data);
return HAL_I2C_Mem_Write(&hi2c2, dev, index, I2C_MEMADD_SIZE_16BIT, (uint8_t *)&data, 2, 1000);
}
int8_t VL53L1_WrDWord(uint16_t dev, uint16_t index, uint32_t data)
{
/* 写入之前，先调整字节顺序 */
REVERS_DWORD(&data);
return HAL_I2C_Mem_Write(&hi2c2, dev, index, I2C_MEMADD_SIZE_16BIT, (uint8_t *)&data, 4, 1000);
}
int8_t VL53L1_RdByte(uint16_t dev, uint16_t index, uint8_t *data)
{
return HAL_I2C_Mem_Read(&hi2c2, dev, index, I2C_MEMADD_SIZE_16BIT, data, 1, 1000);
}
int8_t VL53L1_RdWord(uint16_t dev, uint16_t index, uint16_t *data)
{
HAL_I2C_Mem_Read(&hi2c2, dev, index, I2C_MEMADD_SIZE_16BIT, (uint8_t *)data, 2, 1000);
/* 读取之后，调整字节顺序 */
REVERS_WORD(data);
return 0;
}
int8_t VL53L1_RdDWord(uint16_t dev, uint16_t index, uint32_t *data)
{
HAL_I2C_Mem_Read(&hi2c2, dev, index, I2C_MEMADD_SIZE_16BIT, (uint8_t *)data, 4, 1000);
/* 读取之后，调整字节顺序 */
REVERS_DWORD(data);
return 0;
}
int8_t VL53L1_WaitMs(uint16_t dev, int32_t wait_ms)
{
HAL_Delay(wait_ms);
return 0;
}
- 参考官方案例,实现初始化
VL53L1X初始化:
1.复位传感器
XSHUT -> 拉低(delay100ms) -> 拉高(delay100ms)
2.读取ID,验证IIC驱动是否正常
3.等待传感器系统初始化OK
4.配置传感器参数
4.1 设置距离模式
4.2 设置距离模式
4.3 设置单次测距持续时间
4.4 设置测距间隔
5.启动测距测试:读取id,观察输出的id和官方提供的是否一致
- Int_VL53L1X.c
#include "Int_VL53L1X.h"
/**
* @description: 初始化VL53L1X
* @return {*}
*/
uint8_t Int_VL53L1X_Init(void)
{
int status = 0;
uint8_t byteData, sensorState = 0;
uint16_t wordData;
/* 1、复位传感器：XSHUT==》 拉低 -> 延迟100ms -> 拉高 -> 延迟100ms */
HAL_GPIO_WritePin(VX_XSHUT_GPIO_Port, VX_XSHUT_Pin, GPIO_PIN_RESET);
HAL_Delay(100);
HAL_GPIO_WritePin(VX_XSHUT_GPIO_Port, VX_XSHUT_Pin, GPIO_PIN_SET);
HAL_Delay(100);
/* 2、读取id，验证iic驱动是否正常 */
status = VL53L1_RdByte(DEV, 0x010F, &byteData);
printf("VL53L1X Model_ID: %X\n", byteData); // 0xEA
status = VL53L1_RdByte(DEV, 0x0110, &byteData);
printf("VL53L1X Module_Type: %X\n", byteData); // 0xCC
status = VL53L1_RdWord(DEV, 0x010F, &wordData);
printf("VL53L1X: %X\n", wordData); // 0x10
/* 3、等待传感器系统初始化完成 */
while (sensorState == 0)
{
status = VL53L1X_BootState(DEV, &sensorState);
HAL_Delay(2);
}
printf("Chip booted\n");
/* 4、配置传感器参数 */
/* 4.1 初始化传感器 */
status = VL53L1X_SensorInit(DEV);
/* 4.2 设置距离模式： 1-短距离模式（2m）；2-长距离模式（4m） */
status = VL53L1X_SetDistanceMode(DEV, 2);
/* 4.3 设置单次测距持续的时间 */
status = VL53L1X_SetTimingBudgetInMs(DEV, 100);
/* 4.4 设置测距的间隔，注意 >= 单次持续时间 */
status = VL53L1X_SetInterMeasurementInMs(DEV, 100);
/* 5、启动测距 */
status = VL53L1X_StartRanging(DEV);
return status;
}
- 获取高度数据
这里数据准备读取官方用的是while,可能会导致程序卡在哪里,我们这里使用if判断
- Int_VL53L1X.c
/**
* @description: 获取高度数据，单位：毫米
* @return {*}
*/
uint16_t Int_VL53L1X_GetHeight(void)
{
int status = 0;
uint16_t Distance;
uint8_t dataReady;
status = VL53L1X_CheckForDataReady(DEV, &dataReady);
/* 改造：不卡循环 ===》 如果数据准备好，那就读取最新数据 */
if (dataReady == 1)
{
dataReady = 0;
status = VL53L1X_GetDistance(DEV, &Distance);
// status = VL53L1X_ClearInterrupt(DEV); //用到中断的时候，需要每次清除中断标志位
// printf("测距高度=%u\n", Distance);
}
else
{
/* 如果数据没准备好，给个0，回头交给APP层判断处理 */
Distance = 0;
}
return Distance;
}
PID
PID算法的实现
误差值 = 测量值-期望值
PID计算=kp误差值 + ki累加（误差值dt）+kd（本次误差-上次误差）/dt 串级PID实现：
外环进行PID计算
内环的期望值 = 外环的PID结果
内环进行PID计算
- Com_PID.h
typedef struct
{
float kp; // P系数
float ki; // I系数
float kd; // D系数
float measure; // 测量值
float expect; // 期望值
float integral; // 误差的积分
float last_bias; // 上次的误差值
float out; // pid计算结果
} sPID;
- Com_PID.c
#include "Com_PID.h"
/* PID计算=kp*误差值 + ki * 累加（误差*T） + kd * （本次误差-上次误差）/T */
/**
* @description: 进行PID计算
* @param {sPID} *pid pid结构体的地址
* @param {float} dt 周期，单位秒
* @return {*}
*/
void Com_PID_Update(sPID *pid, float dt)
{
float bias = 0.0f;
float div = 0.0f;
/* 1、计算误差值 */
bias = pid->measure - pid->expect;
/* 2、计算误差的积分：累加 */
pid->integral += bias * dt;
/* 3、计算误差的微分：变化率 */
div = (bias - pid->last_bias) / dt;
/* 4、进行PID计算 */
pid->out = pid->kp * bias + pid->ki * pid->integral + pid->kd * div;
/* 5、保存本次误差值 */
pid->last_bias = bias;
}
/**
* @description: 进行串级PID计算
* @param {sPID *} outer 外环PID
* @param {sPID *} inner 内环PID
* @param {float} dt 周期
* @return {*}
*/
void Com_PID_Cascade(sPID *outer, sPID *inner, float dt)
{
/* 外环的输出作为内环的期望值 */
/* 1、计算外环PID */
Com_PID_Update(outer, dt);
/* 2、外环的输出 赋值给 内环的期望值 */
inner->expect = outer->out;
/* 3、计算内环PID */
Com_PID_Update(inner, dt);
}
/**
* @description: 批量复位PID
* @param {sPID} * 保存pid结构体地址的数组的地址
* @param {uint8_t} size 数组元素个数
* @return {*}
*/
void Com_PID_Reset(sPID **pids, uint8_t size)
{
/* 参数说明：sPID **pids
sPID * 表示数组里的元素类型是 结构体地址
第二个* 表示数组的地址
*/
for (uint8_t i = 0; i < size; i++)
{
/* 只需要清理：积分值、上次误差值、输出 */
pids[i] -> integral = 0.0f;
pids[i] -> last_bias = 0.0f;
pids[i] -> out = 0.0f;
}
}
姿态控制PID
1）锁定状态
-> 判断是否解锁，如果是 -> 复位一下pid结构体的值
2）解锁状态： 进行pid处理
赋值 6个pid的测量值（三个角度、三个角速度）
调用串级PID
如果isUnlock=0,恢复状态1
- App_Flight.c
/**
* @description: 姿态控制PID处理
* @param {float} dt 周期，单位秒
* @return {*}
*/
void App_Flight_PosturePID(float dt)
{
static uint8_t status = UNLOCK_0;
switch (status)
{
case UNLOCK_0:
/* 锁定状态 */
if (isUnlock == 1)
{
status = UNLOCK_PROCESS;
/* 重置PID，避免重新解锁后，影响新的PID计算 */
Com_PID_Reset(pids, 6);
}
break;
case UNLOCK_PROCESS:
/* 锁定状态 */
/* 欧拉角测量值 */
pitch_pid.measure = angle.pitch;
roll_pid.measure = angle.roll;
yaw_pid.measure = angle.yaw;
/* 角速度测量值 */
gyroY_pid.measure = mpu6050.gy * Gyro_G;
gyroX_pid.measure = mpu6050.gx * Gyro_G;
gyroZ_pid.measure = mpu6050.gz * Gyro_G;
/* 俯仰姿态串级PID处理 */
Com_PID_Cascade(&pitch_pid, &gyroY_pid, dt);
/* 横滚姿态串级PID处理 */
Com_PID_Cascade(&roll_pid, &gyroX_pid, dt);
/* 偏航姿态串级PID处理 */
Com_PID_Cascade(&yaw_pid, &gyroZ_pid, dt);
/* 如果恢复锁定，退出PID处理 */
if (isUnlock == 0)
{
status = UNLOCK_0;
}
break;
default:
break;
}
}
定高PID
1、串级PID
外环：高度
内环：z轴速度设计原则：
①反应更快： z轴速度
②准确性： 如果用z轴加速度，本身噪音大，滤波后副作用延迟 ==》准确性不够
如果两个原则冲突，优先考虑原则1，此时考虑尽可能的满足原则2（用一些算法、数据处理）
===》 尽可能： 用z轴速度 替代 z轴加速度2、z轴速度怎么取
方案一： 利用加速度换算出速度
vt = v0+aT
当前速度= 上次的速度+ 当时加速度 * 时间间隔方案二：
（本次高度 - 上次的高度）/T最终方案： 互补滤波
方案A、方案B，各有缺点
综合两种方案 = k * 方案A + （1-k）* 方案B最终公式：
速度 = k* （上次的速度+ 当时加速度 * 时间间隔） + （1-k）*（本次高度 - 上次的高度）/TVL53L1X得到 高度 --> 串级PID（高度环 --》 速度环 ） --》 作用于油门值
互补滤波得到 z轴速度 _↗
- App_Flight.c
/**
* @description: 获取高度数据,单位换算成cm
* @return {*}
*/
float App_Flight_GetHeight(void)
{
float height = 0.0f;
static float last_height = 0.0f;
/* 1、读取高度数据 */
height = (float)Int_VL53L1X_GetHeight();
/* 2、处理返回0的情况(！！！添加，遥控控制位移时，不更新高度数据) */
if (
height == 0
|| (abs(receiveRemote.PIT - 1500) > 50)
|| (abs(receiveRemote.ROL - 1500) > 50)
|| (abs(receiveRemote.YAW - 1500) > 50)
)
{
/* 2.1 说明数据未准备好，未更新 ==》 取上次的值 */
height = last_height;
}
else
{
/* 3、障碍物处理算法(简单实现：高度变化超过一定范围，主动忽略，避免突变) */
if (abs(height - last_height) >= 200)
{
/* 必须在更新last之前处理 */
height = last_height;
}
/* 2.2 说明数据准备好了，更新了,保存到last中 */
last_height = height;
}
/* 4、换算成cm */
height = height / 10.0f;
return height;
}
/**
* @description: 定高的PID处理
* @param {float} dt 周期
* @return {*}
*/
void App_Flight_HeightPID(float dt)
{
static uint8_t status = FIXEDHEIGHT_0;
static int16_t thr_hold = 0; // 保存定高时的油门值
static float static_accz = 0; // 静止时的加速度读值（用来去皮）
static float thr_hold_time_count = 0; // 油门改变的计时变量
static float current_height = 0;
switch (status)
{
case FIXEDHEIGHT_0:
/* 非定高状态 */
/* 初始状态，获取静止时的加速度值（后续去皮处理）,锁定时认为是静止状态 */
if (isUnlock == 0)
{
static_accz = GetAccz();
}
/* 解锁 且 定高，进入定高状态 */
if (isUnlock && isFixedHeight)
{
/* 1、清理定高PID的值 */
Com_PID_Reset(&pids[6], 2);
/* 2、记录一些初始值 */
/* 2.1 当前高度作为初始的高度期望值 */
height_pid.expect = App_Flight_GetHeight();
/* 2.2 记录进入定高时的油门，方便后续判断油门是否改变 */
thr_hold = receiveRemote.THR;
/* 3、切换到定高状态 */
status = FIXEDHEIGHT_PROCESS;
/* 4、切换灯的状态:快闪 */
led.status = AllFlash;
led.flashTime = 100;
}
break;
case FIXEDHEIGHT_PROCESS:
/* 定高状态 */
/* 1、获取最新的高度值 */
current_height = App_Flight_GetHeight();
/* 2、定高时推油门的处理 */
/* 如果油门值改变超过50，且持续半秒还是这样，说明油门确实动了，类似按键的写法 */
if (abs(receiveRemote.THR - thr_hold) >= 50)
{
if (thr_hold_time_count++ >= 50)
{
/* 进入if，说明进入定高之后，油门又改变了 ==》 重新定高 */
height_pid.expect = current_height;
/* 更新记录的油门值 */
thr_hold = receiveRemote.THR;
/* 更新完毕之后，清零计时变量 */
thr_hold_time_count = 0;
}
}
else
{
/* 进入这里说明，不到半秒，油门恢复，计时变量清零 */
thr_hold_time_count = 0;
}
/* 3、PID赋值测量值 */
/* 3.1 z轴速度的测量值：互补滤波 */
/* 速度 = k* （上次的速度+ 当时加速度 * dt） + （1-k）*（本次高度 - 上次的高度）/dt */
/* 注意：
上次的速度： 此时速度的measure还没更新，存的就是上次的速度值
上次的高度： 此时高度的measure还没更新，存的就是上次的高度值
加速度值：① 去皮，② 用算法处理过的加速度值，更准确
*/
zSpeed_pid.measure = 0.98 * (zSpeed_pid.measure + (GetAccz() - static_accz) * dt) + 0.02 * ((current_height - height_pid.measure) / dt);
/* 3.2 高度测量值 */
height_pid.measure = current_height;
/* 4、进行串级PID计算 */
Com_PID_Cascade(&height_pid, &zSpeed_pid, dt);
/* 5、退出机制*/
if (isFixedHeight == 0 || isUnlock == 0)
{
status = FIXEDHEIGHT_0;
/* 恢复灯的状态 */
led.status = AllFlash;
led.flashTime = 1000;
}
break;
default:
break;
}
}
控制电机
1、控制方案
油门值 <= 90% -> 油门值最大只能到900，至少留出10%的空间给PID
PID值 >= 10%2、三个PID结果如何叠加到电机上？
俯仰姿态： 前面两个电机一组，后面两个电机一组， 这两组叠加的符号相反
横滚姿态： 左边2个电机一组，右边2个电机一组， 这两组叠加的符号相反
偏航姿态： 相同对角线为一组， 这两组 叠加的符号相反3、油门和PID叠加的最终结果，设置到 TIM的 CCR中
- App_Flight.c
/**
* @description: 控制电机：将PID结果和油门值作用于电机
* @return {*}
*/
void App_Flight_Motor(void)
{
static uint8_t status = UNLOCK_0;
int16_t thr_tmp = 0;
switch (status)
{
case UNLOCK_0:
/* 锁定状态 */
if (isUnlock == 1)
{
status = UNLOCK_PROCESS;
/* 电机复位 */
motor1 = motor2 = motor3 = motor4 = 0;
}
break;
case UNLOCK_PROCESS:
/* 解锁状态 */
/* 1、油门值处理 */
/* 1.1、处理油门值的取值，匹配 ccr的取值范围 */
thr_tmp = receiveRemote.THR - 1000;
/* >>>>>>>>>>>>>>>>>>定高PID结果作用于油门值<<<<<<<<<<<<<<<<<<< */
thr_tmp += LIMIT(zSpeed_pid.out,-200,200);
/* 1.2、限幅油门值900，至少留出10%给PID使用 */
thr_tmp = LIMIT(thr_tmp, 0, 900);
/* 1.3、将限幅后的油门值，作用于电机上 */
motor1 = motor2 = motor3 = motor4 = thr_tmp;
/* 2、PID结果处理 */
/*
motor1--左上
motor2--右上
motor3--右下
motor4--左下
俯仰PID结果 ==》 1、2一组，3、4一组，两组符号相反
横滚PID结果 ==》 1、4一组，2、3一组，两组符号相反
偏航PID结果 ==》 1、3一组，2、4一组，两组符号相反
*/
motor1 += +gyroY_pid.out + gyroX_pid.out + gyroZ_pid.out;
motor2 += +gyroY_pid.out - gyroX_pid.out - gyroZ_pid.out;
motor3 += -gyroY_pid.out - gyroX_pid.out + gyroZ_pid.out;
motor4 += -gyroY_pid.out + gyroX_pid.out - gyroZ_pid.out;
/* 如果恢复成锁定，退出控制 */
if (isUnlock == 0)
{
status = UNLOCK_0;
}
/* 为了调试安全，油门最低时，禁用PID */
// if (thr_tmp <= 10)
/* 加入定高后，安全措施要用原始值判断 */
if (receiveRemote.THR <= 1010)
{
motor1 = motor2 = motor3 = motor4 = 0;
}
break;
default:
break;
}
/* 统一作用于TIM的CCR中 */
/*
左前：TIM3—CH1 ==> motor1
右前：TIM2-CH2 ==> motor2
右后：TIM1-CH3 ==> motor3
左后：TIM4-CH4 ==> motor4
*/
__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_1, LIMIT(motor1, 0, 1000));
__HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2, LIMIT(motor2, 0, 1000));
__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_3, LIMIT(motor3, 0, 1000));
__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_4, LIMIT(motor4, 0, 1000));
}
控制飞机移动
将摇杆值转换成角度的变化范围 -> 将这个角度 作为 外环（角度环） 的期望值
-> 怎么换算成角度
-> 摇杆值：1000-2000，
-> 与默认中间位置求差值 -> -1500 -> -500~+500
-> 换算成角度 -20°~20°
-> 换算比例 0.041）往前、后飞： 俯仰角产生一个低头的角度 -> 让PID来低头
-> 低头到期望的角度之后，PID=0
-> 此时飞机按照这个角度，4个电机=油门，往前、右飞
2）往左、右飞：横滚角产生一个角度 ===》 让PID来产生横滚的角度
-> 到期望的角度之后，PID=0
-> 此时飞机按照这个角度，4个电机=油门，往左、右飞3）水平旋转： 偏航角产生一个偏航角度===》 让PID来产生偏航的角度
-> 到期望的角度之后，PID=0
-> 此时飞机按照这个角度，4个电机=油门，往顺时针、逆时针旋转
- App_Flight.c
/**
* @description: 位移控制：前、后、左、右、水平旋转
* @return {*}
*/
void App_Flight_Move(void)
{
/* 摇杆值和角度值的换算比例 ：
比如： 右摇杆上下--俯仰
打到最上面2000,2000-1500=500的差值 对应 20°角度
20/500 = 0.04
打到最下面1000,1000-1500=-500的差值 对应 -20°角度
-20/-500 = 0.04
*/
float move_ratio = 0.04;
/* 将摇杆值转换成角度的取值 ===》 将这个角度值 赋值给 PID外环（角度）的期望值 */
/* 控制前后飞：俯仰姿态 */
pitch_pid.expect = move_ratio * (receiveRemote.PIT - 1500);
/* 控制左右飞：横滚姿态 */
roll_pid.expect = move_ratio * (receiveRemote.ROL - 1500);
/* 控制水平旋转：偏航姿态(偏航的值大概=实际值/2,手动补偿一下) */
yaw_pid.expect = 2 * move_ratio * (receiveRemote.YAW - 1500);
}
电池管理 – IP5305T
按键持续时间长于 30ms，但小于 2s， 即为短按动作，短按会打开电量显示灯和升压输出。
按键持续时间长于 2s， 即为长按动作， 长按会开启或者关闭照明 LED。
小于 30ms 的按键动作不会有任何响应。
在 1s 内连续两次短按键，会关闭升压输出、电量显示和照明 LED。
3.FreeRTOS
飞控板任务划分
1、姿态控制 （2ms） 9
获取mpu数据
四元数解算欧拉角
PID计算
位移控制
作用于电机2、通信（4ms） 9
接收2.4G数据
解析校验、解锁、失联3、辅助 (50ms) 6
灯控
电池电压4、电源唤醒任务（10s） 10
默认高电平
拉低
delay100ms
拉高
绝对延时10s
- App_Task.c
#include "App_Task.h"
/* 起始任务配置 */
#define START_TASK_STACK_SIZE 128
#define START_TASK_PRIORITY 11 // 起始任务优先级最高，就不需要加临界区了
TaskHandle_t start_handle;
void App_Task_Start(void *args);
/* 姿态控制任务配置 */
#define POSTURE_TASK_STACK_SIZE 128
#define POSTURE_TASK_PRIORITY 9
TaskHandle_t posture_handle;
void App_Task_Posture(void *args);
/* 通信任务配置 */
#define COMMUNICATION_TASK_STACK_SIZE 128
#define COMMUNICATION_TASK_PRIORITY 9
TaskHandle_t communication_handle;
void App_Task_Communication(void *args);
/* 辅助任务配置 */
#define ASSIST_TASK_STACK_SIZE 128
#define ASSIST_TASK_PRIORITY 6
TaskHandle_t assist_handle;
void App_Task_Assist(void *args);
/* 电源唤醒任务配置 */
#define POWER_TASK_STACK_SIZE 128
#define POWER_TASK_PRIORITY 10 // 除了起始任务之外，优先级保持最高
TaskHandle_t power_handle;
void App_Task_Power(void *args);
/* 定高任务配置 */
#define HEIGHT_TASK_STACK_SIZE 128
#define HEIGHT_TASK_PRIORITY 9 // 不要抢风头，优先级 <= 重要任务（姿态控制、通信）
TaskHandle_t height_handle;
void App_Task_FixedHeight(void *args);
/**
* @description: 启动FreeRTOS
* @return {*}
*/
void App_Task_Init(void)
{
/* 1、创建起始任务 */
xTaskCreate(
(TaskFunction_t)App_Task_Start,
(char *)"ergou",
(configSTACK_DEPTH_TYPE)START_TASK_STACK_SIZE,
(void *)NULL,
(UBaseType_t)START_TASK_PRIORITY,
(TaskHandle_t *)&start_handle);
/* 2、启动调度器 */
vTaskStartScheduler();
}
/**
* @description: 起始任务：用于创建其他任务
* @param {void} *args
* @return {*}
*/
void App_Task_Start(void *args)
{
/* 1、创建姿态控制任务 */
xTaskCreate(
(TaskFunction_t)App_Task_Posture,
(char *)"App_Task_Posture",
(configSTACK_DEPTH_TYPE)POSTURE_TASK_STACK_SIZE,
(void *)NULL,
(UBaseType_t)POSTURE_TASK_PRIORITY,
(TaskHandle_t *)&posture_handle);
/* 2、创建通信任务 */
xTaskCreate(
(TaskFunction_t)App_Task_Communication,
(char *)"App_Task_Communication",
(configSTACK_DEPTH_TYPE)COMMUNICATION_TASK_STACK_SIZE,
(void *)NULL,
(UBaseType_t)COMMUNICATION_TASK_PRIORITY,
(TaskHandle_t *)&communication_handle);
/* 3、创建辅助任务 */
xTaskCreate(
(TaskFunction_t)App_Task_Assist,
(char *)"App_Task_Assist",
(configSTACK_DEPTH_TYPE)ASSIST_TASK_STACK_SIZE,
(void *)NULL,
(UBaseType_t)ASSIST_TASK_PRIORITY,
(TaskHandle_t *)&assist_handle);
/* 4、创建电源唤醒任务 */
xTaskCreate(
(TaskFunction_t)App_Task_Power,
(char *)"App_Task_Power",
(configSTACK_DEPTH_TYPE)POWER_TASK_STACK_SIZE,
(void *)NULL,
(UBaseType_t)POWER_TASK_PRIORITY,
(TaskHandle_t *)&power_handle);
/* 5、创建定高任务 */
xTaskCreate(
(TaskFunction_t)App_Task_FixedHeight,
(char *)"App_Task_FixedHeight",
(configSTACK_DEPTH_TYPE)HEIGHT_TASK_STACK_SIZE,
(void *)NULL,
(UBaseType_t)HEIGHT_TASK_PRIORITY,
(TaskHandle_t *)&height_handle);
/* 6、删除自己 */
vTaskDelete(NULL);
}
/**
* @description: 姿态控制任务
* @param {void} *args
* @return {*}
*/
void App_Task_Posture(void *args)
{
TickType_t last_time = xTaskGetTickCount();
while (1)
{
// printf("...posture task...\r\n");
/* 1、获取欧拉角 */
taskENTER_CRITICAL();
App_Flight_GetMpuData();
taskEXIT_CRITICAL();
App_Flight_GetAngle(0.002f);
/* 2、进行PID处理 */
App_Flight_Move();
App_Flight_PosturePID(0.002f);
/* 3、控制电机 */
App_Flight_Motor();
vTaskDelayUntil(&last_time, 2);
}
}
/**
* @description: 通信任务
* @param {void} *args
* @return {*}
*/
void App_Task_Communication(void *args)
{
TickType_t last_time = xTaskGetTickCount();
uint8_t isConnect = 1;
while (1)
{
// printf("...Communication task...\r\n");
/* 1、2.4G模块接收遥控数据 */
taskENTER_CRITICAL();
isConnect = NRF24L01_RxPacket(RX_BUFF);
taskEXIT_CRITICAL();
/* 2、遥控器数据处理 */
App_Flight_RemoteHandle(isConnect);
vTaskDelayUntil(&last_time, 4);
}
}
/**
* @description: 辅助任务
* @param {void} *args
* @return {*}
*/
void App_Task_Assist(void *args)
{
TickType_t last_time = xTaskGetTickCount();
while (1)
{
// printf("...Assist task...\r\n");
/* 灯控处理 */
App_Flight_PoiletLED();
vTaskDelayUntil(&last_time, 50);
}
}
/**
* @description: 定高处理任务
* @param {void} *args
* @return {*}
*/
void App_Task_FixedHeight(void *args)
{
TickType_t last_time = xTaskGetTickCount();
while (1)
{
App_Flight_HeightPID(0.010f);
vTaskDelayUntil(&last_time, 10);
}
}
/**
* @description: 电源唤醒任务
* @param {void} *args
* @return {*}
*/
void App_Task_Power(void *args)
{
TickType_t last_time = xTaskGetTickCount();
/* 注意：一开始延时1s，才能稳定起作用 */
vTaskDelay(1000);
while (1)
{
// printf("...Power task...\r\n");
/* 1、拉低 */
HAL_GPIO_WritePin(POWER_KEY_GPIO_Port, POWER_KEY_Pin, GPIO_PIN_RESET);
/* 2、保持100ms */
vTaskDelay(100);
/* 3、拉高 */
HAL_GPIO_WritePin(POWER_KEY_GPIO_Port, POWER_KEY_Pin, GPIO_PIN_SET);
vTaskDelayUntil(&last_time, 10 * 1000);
}
}
遥控器任务划分
1、任务一（4ms） 6
摇杆处理
封装打包数据
2.4G发送数据2、任务二（20ms） 6
微调按键
OLED显示3、电源唤醒任务（10s） 10
默认高电平
拉低
delay100ms
拉高
绝对延时10s
- App_Task.c
#include "App_Task.h"
/* 起始任务配置 */
#define START_TASK_STACK_SIZE 128
#define START_TASK_PRIORITY 11 // 起始任务优先级最高，就不需要加临界区了
TaskHandle_t start_handle;
void App_Task_Start(void *args);
/* 通信任务配置 */
#define COMMUNICATION_TASK_STACK_SIZE 128
#define COMMUNICATION_TASK_PRIORITY 6
TaskHandle_t communication_handle;
void App_Task_Communication(void *args);
/* 辅助任务配置 */
#define ASSIST_TASK_STACK_SIZE 128
#define ASSIST_TASK_PRIORITY 6
TaskHandle_t assist_handle;
void App_Task_Assist(void *args);
/* 电源唤醒任务配置 */
#define POWER_TASK_STACK_SIZE 128
#define POWER_TASK_PRIORITY 10 // 除了起始任务之外，优先级保持最高
TaskHandle_t power_handle;
void App_Task_Power(void *args);
/**
* @description: 启动FreeRTOS
* @return {*}
*/
void App_Task_Init(void)
{
/* 1、创建起始任务 */
xTaskCreate(
(TaskFunction_t)App_Task_Start,
(char *)"ergou",
(configSTACK_DEPTH_TYPE)START_TASK_STACK_SIZE,
(void *)NULL,
(UBaseType_t)START_TASK_PRIORITY,
(TaskHandle_t *)&start_handle);
/* 2、启动调度器 */
vTaskStartScheduler();
}
/**
* @description: 起始任务：用于创建其他任务
* @param {void} *args
* @return {*}
*/
void App_Task_Start(void *args)
{
/* 1、创建通信任务 */
xTaskCreate(
(TaskFunction_t)App_Task_Communication,
(char *)"App_Task_Communication",
(configSTACK_DEPTH_TYPE)COMMUNICATION_TASK_STACK_SIZE,
(void *)NULL,
(UBaseType_t)COMMUNICATION_TASK_PRIORITY,
(TaskHandle_t *)&communication_handle);
/* 2、创建辅助任务 */
xTaskCreate(
(TaskFunction_t)App_Task_Assist,
(char *)"App_Task_Assist",
(configSTACK_DEPTH_TYPE)ASSIST_TASK_STACK_SIZE,
(void *)NULL,
(UBaseType_t)ASSIST_TASK_PRIORITY,
(TaskHandle_t *)&assist_handle);
/* 3、创建电源唤醒任务 */
xTaskCreate(
(TaskFunction_t)App_Task_Power,
(char *)"App_Task_Power",
(configSTACK_DEPTH_TYPE)POWER_TASK_STACK_SIZE,
(void *)NULL,
(UBaseType_t)POWER_TASK_PRIORITY,
(TaskHandle_t *)&power_handle);
/* 4、删除自己 */
vTaskDelete(NULL);
}
/**
* @description: 通信任务
* @param {void} *args
* @return {*}
*/
void App_Task_Communication(void *args)
{
TickType_t last_time = xTaskGetTickCount();
while (1)
{
/* 1、摇杆处理 */
App_Remote_StickScan();
/* 2、打包数据 */
App_Remote_PacketData(TX_BUFF);
/* 3、通过2.4G发送数据 */
taskENTER_CRITICAL();
NRF24L01_TxPacket(TX_BUFF);
taskEXIT_CRITICAL();
vTaskDelayUntil(&last_time, 4);
}
}
/**
* @description: 辅助任务
* @param {void} *args
* @return {*}
*/
void App_Task_Assist(void *args)
{
TickType_t last_time = xTaskGetTickCount();
while (1)
{
/* 1、按键微调 */
App_Remote_Key();
/* 2、OLED显示 */
oled_show();
vTaskDelayUntil(&last_time, 50);
}
}
/**
* @description: 电源唤醒任务
* @param {void} *args
* @return {*}
*/
void App_Task_Power(void *args)
{
TickType_t last_time = xTaskGetTickCount();
/* 注意：一开始延时1s，才能稳定起作用 */
vTaskDelay(1000);
while (1)
{
/* 1、拉低 */
HAL_GPIO_WritePin(POWER_KEY_GPIO_Port, POWER_KEY_Pin, GPIO_PIN_RESET);
/* 2、保持100ms */
vTaskDelay(100);
/* 3、拉高 */
HAL_GPIO_WritePin(POWER_KEY_GPIO_Port, POWER_KEY_Pin, GPIO_PIN_SET);
vTaskDelayUntil(&last_time, 10 * 1000);
}
}
4. PID调参
姿态PID调参
1、调试 俯仰姿态 或 横滚姿态
1）先调内环P：
确定P的极性：
对 =》 减速下掉
错 =》 加速下掉
确定P的大小： 减速下掉，没有顿搓、效果明显
大 =》 下掉过程，明显震荡
小 =》 下掉过程，没有震荡，减速不明显 2）调试外环P
确定极性： 拉到一定的角度，定住（让角速度=0），松手
对 =》 往0度角回复
错 =》 角度越来越大 确定大小
大 =》 明显震荡，回复速度非常快
小 =》 轻微震荡，回复速度很慢 3）加入D抑制震荡
如果D加在外环 ===》 震荡消失，回复速度变慢
如果D加在内环 ===》 震荡消失，回复速度不会明显减慢
确定D的极性（内环的D，暂时关闭外环P）
对 =》 下掉过程抑制P的作用（减速作用变弱）
错 =》 震荡更明显，下掉过程放大P的作用（减速更狠，甚至回弹更狠）
确定D的大小： （开启外环P）
回复快、且基本没有震荡
大 =》 明显震荡，回复0度角的过程变得缓慢
小 =》 也有震荡，回复0度角的过程没那么慢
高度PID参数
1）确定内环P（z轴速度）
确定极性：上升过程中，切换到定高模式
对 =》 观察到电机明显降速
错 =》 电机越来越快确定大小： 速度很快的降下来 大： 高低来回震荡调整 小： 减速过程慢，还会往上飞一段比较长的距离 P值来回调，很难取到合适的值 ===》 加入D的2）确定外环P（高度）
确定极性： 上升过程中，切换到定高模式
对 =》 按下定高后，往上飞（在减速），回落到期望的高度
错 =》 按下定高后，往上飞（在加速），越飞越高确定大小： 回落的速度 大： 按下定高后，往上飞很短距离，快速回到期望高度，期望高度来回震荡 小： 按下定高后，往上飞很长距离，缓慢回到期望高度 很难调试到合适的（回落速度还行、没有震荡） 选择加入D
- App_Flight.c
/**
* @description: PID参数赋值
* @return {*}
*/
void App_Flight_PID_Init(void)
{
/* ========俯仰姿态============ */
/* 内环：Y轴角速度 */
gyroY_pid.kp = +3.00f; // +3.00
gyroY_pid.ki = 0.00f;
gyroY_pid.kd = +0.085f; // +0.085
/* 外环：俯仰角 */
pitch_pid.kp = -7.00f; // -7.00
pitch_pid.ki = 0.00f;
pitch_pid.kd = 0.00f;
/* ========横滚姿态============ */
/* 内环：X轴角速度 */
gyroX_pid.kp = -3.00f; // -3.00
gyroX_pid.ki = 0.00f;
gyroX_pid.kd = -0.085f; //-0.085
/* 外环：横滚角 */
roll_pid.kp = -7.00f; //-7.0
roll_pid.ki = 0.00f;
roll_pid.kd = 0.00f;
/* ========偏航姿态============ */
/* 内环：Z轴角速度 */
gyroZ_pid.kp = +2.00f; // +2.00
gyroZ_pid.ki = 0.00f;
gyroZ_pid.kd = 0.00f;
/* 外环：偏航角 */
yaw_pid.kp = -2.00f; // -2.00
yaw_pid.ki = 0.00f;
yaw_pid.kd = 0.00f;
/* ========定高处理============ */
/* 内环：Z轴速度 */
zSpeed_pid.kp = -1.20f;
zSpeed_pid.ki = 0.00f;
zSpeed_pid.kd = -0.085f;
/* 外环：高度 */
height_pid.kp = -1.20f;
height_pid.ki = 0.00f;
height_pid.kd = -0.085f;
}
成果展示
遥控器:
左上下 -> 油门
左左右 -> 偏航
右上下 -> 俯仰
右左右 -> 横滚
显示屏左上角显示当前信道
三四行通过进度条的形式显示各个角度及油门的百分比。
