四轴飞行器
·
整体概述，freeRTOS中的各项内容概述
本项目根据尚硅谷的四轴飞行器进行复刻，采用freeRTOS操作系统配合HAL库进行构建。
包含任务包括电源管理任务，LED灯控任务，飞行任务，通讯任务等。
void app_freertos_start(void)
{
//1.创建电源管理任务
xTaskCreate(power_task, "power_task", power_task_stack_size, NULL, power_task_priority, &power_task_handle);
//2.创建飞行控制任务
xTaskCreate(flight_task, "flight_task",flight_task_stack_size, NULL, flight_task_priority, &flight_task_handle);
//3.创建LED任务
xTaskCreate(led_task, "led_task",led_task_stack_size, NULL, led_task_priority, &led_task_handle);
//4.创建通讯任务
xTaskCreate(com_task, "com_task",com_task_stack_size, NULL, com_task_priority, &com_task_handle);
//5.启动调度器
vTaskStartScheduler();
}在freeRTOS的.c文件下，包括以下前置内容
设置任务周期，任务大小，优先级，结构体等内容
#include "App_freeRTOS_Task.h"
//stm32f103c6t6 => sram 20k => 分配12k给freertos使用 => 12*1024=12288B
//内存管理 =>C语言的结构体通常保持在堆中 不会自动垃圾回收 => 始终使用一个同一个结构体 不断循环使用
//LED结构体
LED_Struct left_top_led={.port=LED1_GPIO_Port,.pin=LED1_Pin};
LED_Struct right_top_led={.port=LED2_GPIO_Port,.pin=LED2_Pin};
LED_Struct right_bottom_led={.port=LED3_GPIO_Port,.pin=LED3_Pin};
LED_Struct left_bottom_led={.port=LED4_GPIO_Port,.pin=LED4_Pin};
//表示当前连接状态
Remote_State remote_state=remote_connect;
//表示当前飞行状态
Flight_State flight_state=IDLE;
//扩展获取接收的遥控数据
Remote_Data remote_data={.thr = 0 ,.yaw = 500,.pit = 500,.rol = 500, .fix_height = 0, .shutdown = 0};
//按下定高按键时飞行的高度
uint16_t fix_height=0;
//电源管理任务
void power_task(void *args);
//最小推荐填写128 =>128*4=512B
#define power_task_stack_size 128
//任务优先级 0~4 => 0最低 4最高 不推荐使用最小优先级0
#define power_task_priority 4
TaskHandle_t power_task_handle;
#define power_task_period 10000
//飞行控制任务
void flight_task(void *args);
#define flight_task_stack_size 128
#define flight_task_priority 3
TaskHandle_t flight_task_handle;
#define flight_task_period 6
//LED任务
void led_task(void *args);
#define led_task_stack_size 128
#define led_task_priority 1
TaskHandle_t led_task_handle;
#define led_task_period 100
//通讯任务
void com_task(void *args);
#define com_task_stack_size 128
#define com_task_priority 2
TaskHandle_t com_task_handle;
//任务周期
#define com_task_period 61.电源管理任务
项目采用IP5305T作为电源管理芯片，芯片短时间内无操作内容会自动关机。为了避免自动关机，10s执行一次电平变化，防止芯片进入休眠状态。
void power_task(void *args)
{
//获取当前的基准时间
TickType_t last_wake_time = xTaskGetTickCount();
while(1)
{
//每10秒执行一次 => 启动电源避免自动关机
//vTaskDelayUntil(&last_wake_time,power_task_period);
//启动电源
//Int_IP5305T_start();
//使用直接任务通知的接收方法实现10s处理一次
//一直等人通知 直到收到通知res=1 或者 收到超时res=0
uint32_t res = ulTaskNotifyTake(pdTRUE,power_task_period);
if(res != 0)
{
//收到关机通知
Int_IP5305T_shutdown();
}
else
{
//没收到关机通知 正常执行启动
Int_IP5305T_start();
}
}
}电源控制函数的相关内容
IP5305T 电源管理芯片 的驱动接口，负责控制无人机电源的启动和关机。
1. Int_IP5305T_start(void) —— 启动电源/防自动关机
c
HAL_GPIO_WritePin(POWER_KEY_GPIO_Port, POWER_KEY_Pin, GPIO_PIN_RESET); // 拉低引脚（模拟按键按下） vTaskDelay(100); // 延时100ms HAL_GPIO_WritePin(POWER_KEY_GPIO_Port, POWER_KEY_Pin, GPIO_PIN_SET); // 拉高引脚（模拟按键释放）
| 步骤 | 操作 | 作用 |
|---|---|---|
| 1 | 引脚拉低 | 模拟电源按键按下，激活/唤醒 IP5305T |
| 2 | 延时 100ms | 保证芯片检测到有效按键信号 |
| 3 | 引脚拉高 | 释放按键，结束触发 |
2. Int_IP5305T_shutdown(void) —— 软件关机
c
// 第一次按键 HAL_GPIO_WritePin(..., GPIO_PIN_RESET); // 按下 vTaskDelay(100); // 保持100ms HAL_GPIO_WritePin(..., GPIO_PIN_SET); // 释放 vTaskDelay(200); // 间隔200ms // 第二次按键 HAL_GPIO_WritePin(..., GPIO_PIN_RESET); // 再次按下 vTaskDelay(100); // 保持100ms HAL_GPIO_WritePin(..., GPIO_PIN_SET); // 释放
| 步骤 | 操作 | 作用 |
|---|---|---|
| 1 | 第一次按键（按下→延时100ms→释放） | 模拟第一次短按 |
| 2 | 延时 200ms | 间隔，满足"1秒内两次按键"的关机条件 |
| 3 | 第二次按键（按下→延时100ms→释放） | 模拟第二次短按，触发关机 |
2.灯控任务
1.freeRTOS中的使用
LED 状态指示任务，通过 4 个 LED 向用户直观显示无人机的遥控连接状态和飞行状态。任务周期为 100ms，每次循环计数一次。
一、LED 分配方案
| LED 编号 | 变量名 | 指示内容 |
|---|---|---|
| 前两个 | left_top_led、right_top_led |
遥控连接状态 |
| 后两个 | left_bottom_led、right_bottom_led |
飞行状态 |
二、状态与显示逻辑
1. 遥控连接状态（前两个 LED）
| 状态 | 显示效果 |
|---|---|
remote_connect |
常亮 |
remote_disconnect |
常灭 |
2. 飞行状态（后两个 LED）
| 状态 | 显示效果 | 实现方式 |
|---|---|---|
IDLE（空闲） |
慢闪：亮 500ms → 灭 500ms | 每 5 个周期（500ms）翻转一次 |
normal（正常飞行） |
快闪：亮 200ms → 灭 200ms | 每 2 个周期（200ms）翻转一次 |
FIX_HEIGHT（定高模式） |
常亮 | 直接点亮 |
FAIL（故障） |
常灭 | 直接熄灭 |
三、相关代码
freeRTOS中led的代码使用
void led_task(void *args)
{
//获取当前的基准时间
TickType_t last_wake_time = xTaskGetTickCount();
uint8_t count=0;
while(1)
{
count++;
//前两个灯表示连接状态
//1.判断当前连接状态
if(remote_state==remote_connect)
{
//点亮前两个LED
Int_led_turn_on(&left_top_led);
Int_led_turn_on(&right_top_led);
}
else if(remote_state==remote_disconnect)
{
//关掉前两个LED
Int_led_turn_off(&left_top_led);
Int_led_turn_off(&right_top_led);
}
//后两个灯表示飞行状态
//2.判断当前飞行状态
if(flight_state==IDLE)
{
//灯慢闪 =>500ms亮 500ms灭
if(count%5==0)
{
//循环5次 一次是100ms 5次等于500ms
Int_led_toggle(&left_bottom_led);
Int_led_toggle(&right_bottom_led);
}
}
else if(flight_state==normal)
{
//灯快闪 =>200ms亮 200ms灭
if(count%2==0)
{
//循环2次 一次是100ms 2次等于200ms
Int_led_toggle(&left_bottom_led);
Int_led_toggle(&right_bottom_led);
}
}
else if(flight_state==FIX_HEIGHT)
{
//后两个灯长亮
Int_led_turn_on(&left_bottom_led);
Int_led_turn_on(&right_bottom_led);
}
else if(flight_state==FAIL)
{
//后两个灯灭
Int_led_turn_off(&left_bottom_led);
Int_led_turn_off(&right_bottom_led);
}
//将count计数重置
if(count==10)
{
count=0;
}
vTaskDelayUntil(&last_wake_time,led_task_period);
}
}
LED的驱动代码
#include "Int_led.h"
/**
* @brief 打开LED灯
*
* @param led
*/
void Int_led_turn_on(LED_Struct *led)
{
//直接修改引脚电平为低电平 开灯
HAL_GPIO_WritePin(led->port,led->pin,GPIO_PIN_RESET);
}
/**
* @brief 关闭LED灯
*
* @param led
*/
void Int_led_turn_off(LED_Struct *led)
{
//修改引脚为高电平 关灯
HAL_GPIO_WritePin(led->port,led->pin,GPIO_PIN_SET);
}
/**
* @brief 翻转LED灯
*
* @param led
*/
void Int_led_toggle(LED_Struct *led)
{
HAL_GPIO_TogglePin(led->port,led->pin);
}
3.飞行任务构建
一，MPU6050驱动的编写
#include "Int_MPU6050.h"
//保存偏移量的值
int32_t acc_x_offset = 0;
int32_t acc_y_offset = 0;
int32_t acc_z_offset = 0;
int32_t gyro_x_offset = 0;
int32_t gyro_y_offset = 0;
int32_t gyro_z_offset = 0;
/**
* @brief 写寄存器
*
* @param reg 寄存器地址
* @param data 寄存器的值
*/
void Int_MPU6050_Write_Reg(uint8_t reg,uint8_t data)
{
//hal库有固定的I2C读写函数
//1.句柄(hi2c1) 2.从设备地址(0x68) 3.寄存器地址 4.寄存器地址的位数
//5.写入的数据地址 6.写入的字节长度 7.超时时间
HAL_I2C_Mem_Write(&hi2c1,MPU6050_ADDR_WRITE,reg,I2C_MEMADD_SIZE_8BIT,&data,1,1000);
}
void Int_MPU6050_Read_Reg(uint8_t reg,uint8_t *data)
{
//1.句柄(hi2c1) 2.从设备地址(0x68) 3.寄存器地址 4.寄存器地址的位数
//5.读取的数据地址 6.读取的字节长度 7.超时时间
HAL_I2C_Mem_Read(&hi2c1,MPU6050_ADDR_READ,reg,I2C_MEMADD_SIZE_8BIT,data,1,1000);
}
/**
* @brief 在初始化MPU6050完成之后 对MPU6050进行零偏校准
*
*/
void Int_MPU6050_calculate_offset(void)
{
//1.等待飞机停放平稳
//判断飞机是否停放平稳的标准: 前后两次加速度的值差小于200
accel_struct current_accel = {0};
accel_struct last_accel = {0};
uint8_t count = 0;
Int_MPU6050_Get_Accel(&last_accel);
while (count < 100)
{
Int_MPU6050_Get_Accel(¤t_accel);
//判断飞机是否平稳 选用的参数过小 会造成一直无法判断为平稳
if(abs(current_accel.accel_x - last_accel.accel_x )< 400 && abs(current_accel.accel_y - last_accel.accel_y )< 400 && abs(current_accel.accel_z - last_accel.accel_z )< 400)
{
count++;
}
else
{
count = 0;
}
last_accel = current_accel;
vTaskDelay(6);
}
//2.飞机已经平稳 开始进行零偏校准
Gyro_Accel_Struct gyro_accel_data = {0};
int32_t acc_x_sum = 0;
int32_t acc_y_sum = 0;
int32_t acc_z_sum = 0;
int32_t gyro_x_sum = 0;
int32_t gyro_y_sum = 0;
int32_t gyro_z_sum = 0;
for(uint8_t i = 0;i < 100;i++)
{
//重新读取加速度和角速度
Int_MPU6050_Get_Data(&gyro_accel_data);
acc_x_sum += (gyro_accel_data.accel.accel_x - 0);
acc_y_sum += (gyro_accel_data.accel.accel_y - 0);
//Z轴加速度的初始值应该就是1g => 量程是±2g => 16384
acc_z_sum += (gyro_accel_data.accel.accel_z - 16384);
//角速度的偏移量
gyro_x_sum += (gyro_accel_data.gyro.gyro_x - 0);
gyro_y_sum += (gyro_accel_data.gyro.gyro_y - 0);
gyro_z_sum += (gyro_accel_data.gyro.gyro_z - 0);
//每次测量数据需要添加延迟 多次测量取平均值才有意义
vTaskDelay(6);
}
acc_x_offset = acc_x_sum / 100;
acc_y_offset = acc_y_sum / 100;
acc_z_offset = acc_z_sum / 100;
gyro_x_offset = gyro_x_sum / 100;
gyro_y_offset = gyro_y_sum / 100;
gyro_z_offset = gyro_z_sum / 100;
}
/**
* @brief 初始化MPU6050芯片
*
*/
void Int_MPU6050_Init(void)
{
//1.重启芯片 重置所有寄存器的值 => 写电源管理寄存器1 => DEVICE_RESET
Int_MPU6050_Write_Reg(0x6B,0x80);
uint8_t data=0;
//重置完成之后 0x6B寄存器的值位0x40 表示当前为低功耗模式
while(data != 0x40)
{
Int_MPU6050_Read_Reg(0x6B,&data);
}
//唤醒MPU6050 进入到正常工作状态
Int_MPU6050_Write_Reg(0x6B,0x00);
//2.选择合适的量程 => 在够用的范围内 选择的越小越好 =>精度高
//2.1填写角速度量程为±2000°/s
Int_MPU6050_Write_Reg(0x1B,3 << 3);
//2.2填写加速度量程为±2g
Int_MPU6050_Write_Reg(0x1C,0x00);
//3.关闭中断使能
Int_MPU6050_Write_Reg(0x38,0x00);
//4.用户配置寄存器 不使用FITO 以及拓展
Int_MPU6050_Write_Reg(0x6A,0x00);
//5.设置采样频率 => 陀螺仪监控三轴加速度和三轴速度 => 默认频率 1000HZ
//基本逻辑 => 不能降太低 采样率必须大于后续数据的使用率 否则失真 香农定理 采样率是2倍使用频率
//设置采样分频为2 填写的值为2-1
Int_MPU6050_Write_Reg(0x19,0x01);
//6.设置低通滤波的值为184KMz 188HZ => 1
Int_MPU6050_Write_Reg(0x1A,1);
//7.配置使用的系统时钟为添加PLL的
Int_MPU6050_Write_Reg(0x6B,0x01);
//8.使能加速度传感器和角速度传感器
Int_MPU6050_Write_Reg(0x6C,0x00);
//9.进行零偏校准
Int_MPU6050_calculate_offset();
}
/**
* @brief 读取三轴角速度 => 需要进行零偏校准 => 本身抖动不严重
*
* @param gyro
*/
void Int_MPU6050_Get_Gyro(Gyro_struct *gyro)
{
//存储角速度的寄存器地址从0x43开始 高八位在前 xyz的顺序
uint8_t hight = 0;
uint8_t low = 0;
//x轴
Int_MPU6050_Read_Reg(MPU_GYRO_XOUTH_REG,&hight);
Int_MPU6050_Read_Reg(MPU_GYRO_XOUTL_REG,&low);
gyro->gyro_x =(hight << 8 | low) - gyro_x_offset;
//y轴
Int_MPU6050_Read_Reg(MPU_GYRO_YOUTH_REG,&hight);
Int_MPU6050_Read_Reg(MPU_GYRO_YOUTL_REG,&low);
gyro->gyro_y =(hight << 8 | low) - gyro_y_offset;
//z轴
Int_MPU6050_Read_Reg(MPU_GYRO_ZOUTH_REG,&hight);
Int_MPU6050_Read_Reg(MPU_GYRO_ZOUTL_REG,&low);
gyro->gyro_z =(hight << 8 | low) - gyro_z_offset;
}
/**
* @brief 读取三轴加速度 抖动比较严重 需要零偏校准 Z轴值不为0
*
* @param accel
*/
void Int_MPU6050_Get_Accel(accel_struct *accel)
{
uint8_t hight = 0;
uint8_t low = 0;
//x轴
Int_MPU6050_Read_Reg(MPU_ACCEL_XOUTH_REG,&hight);
Int_MPU6050_Read_Reg(MPU_ACCEL_XOUTL_REG,&low);
accel->accel_x = (hight << 8 | low) - acc_x_offset;
//y轴
Int_MPU6050_Read_Reg(MPU_ACCEL_YOUTH_REG,&hight);
Int_MPU6050_Read_Reg(MPU_ACCEL_YOUTL_REG,&low);
accel->accel_y = (hight << 8 | low) - acc_y_offset;
//z轴
Int_MPU6050_Read_Reg(MPU_ACCEL_ZOUTH_REG,&hight);
Int_MPU6050_Read_Reg(MPU_ACCEL_ZOUTL_REG,&low);
accel->accel_z = (hight << 8 | low) - acc_z_offset;
}
/**
* @brief 获取所有的六轴数据
*
* @param data
*/
void Int_MPU6050_Get_Data(Gyro_Accel_Struct *data)
{
Int_MPU6050_Get_Gyro(&data->gyro);
Int_MPU6050_Get_Accel(&data->accel);
}
二，电机驱动
#include "Int_motor.h"
/**
* @brief 传入的参数其实是ccr比较值 最大为1000 默认为200
*
* @param speed
*/
void Int_motor_set_speed(Motor_Struct *motor)
{
if(motor->speed>1000)
{
debug_printf("speed is too fast\r\n");
return;
}
__HAL_TIM_SET_COMPARE(motor->tim, motor->channel, motor->speed);
}
/**
* @brief
*
* @param motor
*/
void Int_motor_start(Motor_Struct *motor)
{
__HAL_TIM_SET_COMPARE(motor->tim, motor->channel,0);
HAL_TIM_PWM_Start(motor->tim, motor->channel);
}
三，PID的构建
#include "APP_flight.h"
Gyro_Accel_Struct gyro_accel_data = {0};
Euler_struct euler_angle = {0};
Gyro_struct last_gyro = {0};
float gyro_z_sum = 0;
extern Remote_Data remote_data;
extern Flight_State flight_state;
extern TaskHandle_t com_task_handle;
//电机结构体
Motor_Struct left_top_motor={.tim=&htim3,.channel=TIM_CHANNEL_1,.speed=0};
Motor_Struct left_bottom_motor={.tim=&htim4,.channel=TIM_CHANNEL_4,.speed=0};
Motor_Struct right_top_motor={.tim=&htim2,.channel=TIM_CHANNEL_2,.speed=0};
Motor_Struct right_bottom_motor={.tim=&htim1,.channel=TIM_CHANNEL_3,.speed=0};
//PID的调参 先调节内环再调节外环
//俯仰角PID结构体 => 后续需要进行专业的PID调参
PID_Struct pitch_pid = {.kp = -6.50,.ki = 0.00,.kd = 0.00};
//Y轴角速度结构体 => 对应俯仰角的内环
//极性的问题 => 参数的正负可以调节 => 作用于电机的时候 可以调节
PID_Struct gyro_y_pid = {.kp = 2.90,.ki = 0.00,.kd = 0.40};
//横滚角PID结构体
PID_Struct roll_pid = {.kp = -6.50,.ki = 0.00,.kd = 0.00};
//x轴角速度结构体 => 对应横滚角的内环
//极性的问题 => 参数的正负可以调节 => 作用于电机的时候 可以调节
PID_Struct gyro_x_pid = {.kp = 2.90,.ki = 0.00,.kd = 0.40};
//偏航角PID结构体
//偏航角属于辅助功能 => 只需保证飞机在空中不旋转即可
PID_Struct yaw_pid = {.kp = -3.00,.ki = 0.00,.kd = 0.00};
//z轴角速度结构体 => 对应偏航角的内环
//极性的问题 => 参数的正负可以调节 => 作用于电机的时候 可以调节
PID_Struct gyro_z_pid = {.kp = -4.50,.ki = 0.00,.kd = 0.10};
//定高结构体
PID_Struct height_pid = {.kp = -0.60,.ki = 0.00,.kd = -0.20};
//记录下的定高飞行高度
extern uint16_t fix_height;
/**
* @brief 飞控任务初始化 MPU6050初始化 启动电机
*
*/
void APP_flight_init(void)
{
Int_MPU6050_Init();
//启动电机
Int_motor_start(&left_top_motor);
Int_motor_start(&right_top_motor);
Int_motor_start(&left_bottom_motor);
Int_motor_start(&right_bottom_motor);
//初始化激光测距仪
Int_VL53L1X_Init();
}
/**
* @brief 根据陀螺仪测量的数据 计算出欧拉角
*
*/
void App_flight_get_eular_angle(void)
{
//1.使用MPU6050的硬件接口 得到六轴数据
Int_MPU6050_Get_Data(&gyro_accel_data);
//2.对角速度进行低通滤波 => 后续对采集数据的使用是及时性比较高的
//对准确性要求没那么严格 但一定要计算迅速
//output = 加权系数 * last_output + (1-加权系数) * 本次的测量值;
gyro_accel_data.gyro.gyro_x = Common_Filter_LowPass(gyro_accel_data.gyro.gyro_x, last_gyro.gyro_x);
gyro_accel_data.gyro.gyro_y = Common_Filter_LowPass(gyro_accel_data.gyro.gyro_y, last_gyro.gyro_y);
gyro_accel_data.gyro.gyro_z = Common_Filter_LowPass(gyro_accel_data.gyro.gyro_z, last_gyro.gyro_z);
last_gyro.gyro_x = gyro_accel_data.gyro.gyro_x;
last_gyro.gyro_y = gyro_accel_data.gyro.gyro_y;
last_gyro.gyro_z = gyro_accel_data.gyro.gyro_z;
//先打印角速度
// debug_printf(":%d,%d,%d\n",gyro_accel_data.gyro.gyro_x,gyro_accel_data.gyro.gyro_y,gyro_accel_data.gyro.gyro_z);
//3.对波动变化较大的加速度 使用更高级的滤波方式 => 卡尔曼滤波
gyro_accel_data.accel.accel_x = Common_Filter_KalmanFilter(&kfs[0],gyro_accel_data.accel.accel_x);
gyro_accel_data.accel.accel_y = Common_Filter_KalmanFilter(&kfs[1],gyro_accel_data.accel.accel_y);
gyro_accel_data.accel.accel_z = Common_Filter_KalmanFilter(&kfs[2],gyro_accel_data.accel.accel_z);
//打印加速度
// debug_printf(":%d,%d,%d\n",gyro_accel_data.accel.accel_x,gyro_accel_data.accel.accel_y,gyro_accel_data.accel.accel_z);
//4.通过加速度和角速度来计算当前飞机切斜的角度 => 姿态解算
//使用互补解算计算欧拉角 => 优先使用加速度解算 => 俯仰角和横滚角能够使用
// euler_angle.pitch = atan2(gyro_accel_data.accel.accel_x * 1.0,gyro_accel_data.accel.accel_z) / 3.14159 * 180;
// euler_angle.roll = atan2(gyro_accel_data.accel.accel_y * 1.0,gyro_accel_data.accel.accel_z) / 3.14159 * 180;
//偏航角 => 只能使用角速度积分
//16位ADC的值转换为°/s => 量程是±2000° /s
// gyro_z_sum += (gyro_accel_data.gyro.gyro_z * 2000.0 / 32768.0 ) * 0.006;
// euler_angle.yaw = gyro_z_sum;
//也可以使用移植的四元数姿态解算
Common_IMU_GetEulerAngle(&gyro_accel_data, &euler_angle,0.006);
//俯仰角 横滚角 偏航角
// debug_printf(":%.2f,%.2f,%.2f\n",euler_angle.pitch,euler_angle.roll,euler_angle.yaw);
}
/**
* @brief 根据欧拉角 计算出PID的目标值
*
*/
void APP_flight_pid_process(void)
{
// 新增：遥控器死区处理（防止微小偏移）
int rol = remote_data.rol - 500;
int pit = remote_data.pit - 500;
int yaw = remote_data.yaw - 500;
if(rol > -5 && rol < 5) rol = 0;
if(pit > -5 && pit < 5) pit = 0;
if(yaw > -5 && yaw < 5) yaw = 0;
// 俯仰角
pitch_pid.desire = pit / 50.0; // 这里的 pit 已经是处理过的
// ... 后面代码不变，但把 remote_data.rol 全部替换成处理后的变量 rol
//1.俯仰角
//需要赋值目标值和测量值
//外环的目标角度 => 如果是平稳飞行 => 值为0 => 如果需要遥控飞行 => 目标角度就是遥控器的值
//数值转换 => remote_data.pit(0-1000,500为中间点) 控制范围在±10°
//内环的测量值 => 就是当前的俯仰角
pitch_pid.measure = euler_angle.pitch;
//外环的测量值 => 当前的角速度 => 单位要保持一致
gyro_y_pid.measure = (gyro_accel_data.gyro.gyro_y * 2000.0 / 32768.0);
//2.进行PID计算
Com_PID_Calc_Chain(&pitch_pid,&gyro_y_pid);
//横滚角PID计算
//需要赋值目标值和测量值
//外环的目标角度
roll_pid.desire = rol /50.0;
//内环的测量值 => 就是当前的横滚角
roll_pid.measure = euler_angle.roll;
//外环的测量值
gyro_x_pid.measure = (gyro_accel_data.gyro.gyro_x * 2000.0 / 32768.0);
//2.进行PID计算
Com_PID_Calc_Chain(&roll_pid,&gyro_x_pid);
//偏航角
//需要赋值目标值和测量值
//外环的目标角度
yaw_pid.desire = yaw /50.0;
//内环的测量值 => 就是当前的偏航角
yaw_pid.measure = euler_angle.yaw;
//外环的测量值
gyro_z_pid.measure = (gyro_accel_data.gyro.gyro_z * 2000.0 / 32768.0);
//2.进行PID计算
Com_PID_Calc_Chain(&yaw_pid,&gyro_z_pid);
}
/**
* @brief 根据PID的输出值 控制电机
*
*/
void APP_flight_control_motor(void)
{
//1.首先判断当前飞机的飞行状态
switch (flight_state)
{
case IDLE:
//一旦进入加锁 => 需要将电机速度设置为0
left_top_motor.speed = 0;
left_bottom_motor.speed = 0;
right_top_motor.speed = 0;
right_bottom_motor.speed = 0;
break;
case normal:
//俯仰角 => 向前飞有角速度 => 正误差 => 需要一个向后飞的反馈效果 => 前两个电机转的快 后两个慢
//不同重要程度的PID控制结果 可以进行适当的限制
left_top_motor.speed = remote_data.thr + gyro_y_pid.output - gyro_x_pid.output + gyro_z_pid.output;
left_bottom_motor.speed = remote_data.thr - gyro_y_pid.output - gyro_x_pid.output - gyro_z_pid.output;
right_top_motor.speed = remote_data.thr + gyro_y_pid.output + gyro_x_pid.output - gyro_z_pid.output;
right_bottom_motor.speed = remote_data.thr - gyro_y_pid.output + gyro_x_pid.output + gyro_z_pid.output;
break;
case FIX_HEIGHT:
//只有定高状态才需要进行定高的PID计算 => 定高也需要平稳飞行
left_top_motor.speed = remote_data.thr + gyro_y_pid.output - gyro_x_pid.output + gyro_z_pid.output +height_pid.output;
left_bottom_motor.speed = remote_data.thr - gyro_y_pid.output - gyro_x_pid.output - gyro_z_pid.output +height_pid.output;
right_top_motor.speed = remote_data.thr + gyro_y_pid.output + gyro_x_pid.output - gyro_z_pid.output + height_pid.output;
right_bottom_motor.speed = remote_data.thr - gyro_y_pid.output + gyro_x_pid.output + gyro_z_pid.output + height_pid.output;
break;
case FAIL:
//进行故障处理 -> 一直处理 到满满足条件 修改状态为IDLE
//6ms => 降低速度2点
left_top_motor.speed -= 2;
left_bottom_motor.speed -= 2;
right_top_motor.speed -= 2;
right_bottom_motor.speed -= 2;
if(left_top_motor.speed <= 0 && left_bottom_motor.speed <= 0 && right_top_motor.speed <= 0 && right_bottom_motor.speed <= 0)
{
//故障处理完成 电机转速都已经降低为0
xTaskNotifyGive(com_task_handle);
}
break;
default:
break;
}
//限制电机速度的上限值
//可以通过提供速度上限 让飞行更平稳
left_top_motor.speed = Com_limit(left_top_motor.speed,800,0);
left_bottom_motor.speed = Com_limit(left_bottom_motor.speed,800,0);
right_top_motor.speed = Com_limit(right_top_motor.speed,800,0);
right_bottom_motor.speed = Com_limit(right_bottom_motor.speed,800,0);
//安全限制 => 当油门设置为<50时 => 强制将速度设置为0
if(remote_data.thr < 50)
{
left_top_motor.speed = 0;
left_bottom_motor.speed = 0;
right_top_motor.speed = 0;
right_bottom_motor.speed = 0;
}
//2.设置电机速度
Int_motor_set_speed(&left_top_motor);
Int_motor_set_speed(&left_bottom_motor);
Int_motor_set_speed(&right_top_motor);
Int_motor_set_speed(&right_bottom_motor);
}
/**
* @brief 进入定高模式的PID处理
*
*/
void APP_flight_fix_height_pid_process(void)
{
//24ms一次
//1.填写目标值和测量值
//目标值 => 当前的测量值(进入定高功能时的高度)
height_pid.desire = fix_height;
//测量值 => 当前的测量值(当前激光测定的距离)
height_pid.measure = Int_VL53L1X_GetDistance();
//2.进行单环PID计算
Com_PID_Calc(&height_pid);
}
4.通讯任务
一，Si24R1的驱动
#include "Int_SI24R1.h"
//定义一个静态发送地址 =>发送地址与接收地址相同
uint8_t TX_ADDRESS[TX_ADR_WIDTH] = {0x0A,0x01,0x06,0x0E,0x05}; // 定义一个静态发送地址
//SPI读写一个字节 =>写入的字节是传入的参数 读取的字节是返回值
static uint8_t SPI_RW(uint8_t byte)
{
uint8_t rx_data=0;
HAL_SPI_TransmitReceive(&hspi1,&byte,&rx_data,1,1000);
return rx_data;
}
/********************************************************
函数功能：写寄存器的值（单字节）
入口参数:reg:寄存器映射地址(格式:SI24R1_WRITE_REG|reg)
value:寄存器的值
返回 值：状态寄存器的值
*********************************************************/
uint8_t Int_SI24R1_Write_Reg(uint8_t reg, uint8_t value)
{
uint8_t status;
CS_LOW;
status = SPI_RW(reg);
SPI_RW(value);
CS_HIGH;
return(status);
}
/********************************************************
函数功能：写寄存器的值（多字节）
入口参数:reg:寄存器映射地址(格式:SI24R1_WRITE_REG|reg)
pBuf:写数据首地址
bytes:写数据字节数
返回 值：状态寄存器的值
*********************************************************/
uint8_t Int_SI24R1_Write_Buf(uint8_t reg, const uint8_t *pBuf, uint8_t size)
{
uint8_t status,byte_ctr;
CS_LOW;
status = SPI_RW(reg);
for(byte_ctr=0; byte_ctr<size; byte_ctr++)
{
SPI_RW(*pBuf++);
}
CS_HIGH;
return(status);
}
/********************************************************
函数功能：读取寄存器的值（单字节）
入口参数:reg:寄存器映射地址(格式:SI24R1_READ_REG|reg)
返回 值：寄存器值
*********************************************************/
uint8_t Int_SI24R1_Read_Reg(uint8_t reg)
{
uint8_t value;
CS_LOW;
SPI_RW(reg);
value = SPI_RW(0);
CS_HIGH;
return(value);
}
/********************************************************
函数功能：读取寄存器的值（多字节）
入口参数:reg:寄存器映射地址(格式:SI24R1_READ_REG|reg)
pBuf:接收缓冲区的首地址
size:读取字节数
返回 值：状态寄存器的值
*********************************************************/
uint8_t Int_SI24R1_Read_Buf(uint8_t reg, uint8_t *pBuf, uint8_t size)
{
uint8_t status,byte_ctr;
CS_LOW;
status = SPI_RW(reg);
for(byte_ctr=0;byte_ctr<size;byte_ctr++)
{
pBuf[byte_ctr] = SPI_RW(0); //读取数据，低字节在前
}
CS_HIGH;
return(status);
}
/********************************************************
函数功能:SI24R1接收模式初始化
入口参数：无
返回 值：无
*********************************************************/
void Int_SI24R1_RX_Mode(void)
{
CE_LOW;
Int_SI24R1_Write_Buf(SI24R1_WRITE_REG + RX_ADDR_P0, TX_ADDRESS, TX_ADR_WIDTH); // 接收设备接收通道0使用和发送设备相同的发送地址
Int_SI24R1_Write_Reg(SI24R1_WRITE_REG + EN_AA, 0x01); // 使能接收通道0自动应答
Int_SI24R1_Write_Reg(SI24R1_WRITE_REG + EN_RXADDR, 0x01); // 使能接收通道0
Int_SI24R1_Write_Reg(SI24R1_WRITE_REG + RF_CH, 40); // 选择射频通道0x40 视频教程修改了 下同
Int_SI24R1_Write_Reg(SI24R1_WRITE_REG + RX_PW_P0, TX_PLOAD_WIDTH); // 接收通道0选择和发送通道相同有效数据宽度
Int_SI24R1_Write_Reg(SI24R1_WRITE_REG + RF_SETUP, 0x06); // 数据传输率1Mbps,发射功率4dBm
Int_SI24R1_Write_Reg(SI24R1_WRITE_REG + CONFIG, 0x0f); // CRC使能,16位CRC校验,上电,接收模式
Int_SI24R1_Write_Reg(SI24R1_WRITE_REG + STATUS, 0xff); //清除所有的中断标志位
CE_HIGH; // 拉高CE启动接收设备
}
/********************************************************
函数功能:SI24R1发送模式初始化
入口参数：无
返回 值：无
*********************************************************/
void Int_SI24R1_TX_Mode(void)
{
CE_LOW;
Int_SI24R1_Write_Buf(SI24R1_WRITE_REG + TX_ADDR, TX_ADDRESS, TX_ADR_WIDTH); // 写入发送地址
Int_SI24R1_Write_Buf(SI24R1_WRITE_REG + RX_ADDR_P0, TX_ADDRESS, TX_ADR_WIDTH); // 为了应答接收设备,接收通道0地址和发送地址相同
Int_SI24R1_Write_Reg(SI24R1_WRITE_REG + EN_AA, 0x01); // 使能接收通道0自动应答
Int_SI24R1_Write_Reg(SI24R1_WRITE_REG + EN_RXADDR, 0x01); // 使能接收通道0
Int_SI24R1_Write_Reg(SI24R1_WRITE_REG + SETUP_RETR, 0x0a); // 自动重发延时等待250us+86us,自动重发10次
Int_SI24R1_Write_Reg(SI24R1_WRITE_REG + RF_CH, 40); // 选择射频通道0x40
Int_SI24R1_Write_Reg(SI24R1_WRITE_REG + RF_SETUP, 0x06); // 数据传输率1Mbps,发射功率4dBm
Int_SI24R1_Write_Reg(SI24R1_WRITE_REG + CONFIG, 0x0e); // CRC使能,16位CRC校验,上电
CE_HIGH;
}
/********************************************************
函数功能:读取接收数据 硬件直接接收数据保存到 FIFO队列中 => 通过状态标志位判断队列中是否有数据
入口参数:rxbuf:接收数据存放首地址
返回 值:0:接收到数据
1:没有接收到数据
*********************************************************/
uint8_t Int_SI24R1_RxPacket(uint8_t *rxbuf)
{
uint8_t state;
//将读取的值 原封不动的写回状态寄存器 =>因为状态寄存器中的标志位设计为写1清楚
state = Int_SI24R1_Read_Reg(STATUS); //读取状态寄存器的值
Int_SI24R1_Write_Reg(SI24R1_WRITE_REG+STATUS,state); //清除RX_DS中断标志
if(state & RX_DR) //接收到数据
{
Int_SI24R1_Read_Buf(RD_RX_PLOAD,rxbuf,TX_PLOAD_WIDTH); //读取数据
Int_SI24R1_Write_Reg(FLUSH_RX,0xff); //清除RX FIFO寄存器
return 0;
}
return 1; //没收到任何数据
}
/********************************************************
函数功能：发送一个数据包
入口参数:txbuf:要发送的数据
返回 值:0:发送成功 1:发送失败
*********************************************************/
uint8_t Int_SI24R1_TxPacket(uint8_t *txbuf)
{
uint8_t state;
CE_LOW; //CE拉低,使能SI24R1配置
Int_SI24R1_Write_Buf(WR_TX_PLOAD, txbuf, TX_PLOAD_WIDTH); //写数据到TX FIFO,32个字节
CE_HIGH; //CE置高,使能发送
//没有使用中断判断是否发送完成 =>使用轮询读取状态标志位
// while(IRQ == 1); //等待发送完成
state = Int_SI24R1_Read_Reg(STATUS); //读取状态寄存器的值
while (((state & TX_DS)==0)&&((state & MAX_RT)==0))
{
state = Int_SI24R1_Read_Reg(STATUS);
vTaskDelay(1);
}
Int_SI24R1_Write_Reg(SI24R1_WRITE_REG+STATUS, state); //清除TX_DS或MAX_RT中断标志
if(state&MAX_RT) //达到最大重发次数
{
Int_SI24R1_Write_Reg(FLUSH_TX,0xff); //清除TX FIFO寄存器
return 1;
}
if(state&TX_DS) //发送完成
{
return 0;
}
return 1; //发送失败
}
uint8_t si24r1_rx_buff[5]={0};
/**
* @brief 硬件接口层SI24R1的检查
*
* @return uint8_t 0:检查通过 1:检查失败
*/
uint8_t Int_SI24R1_Check(void)
{
HAL_Delay(200);
//1.测试SPI通讯能够正常读写寄存器
//1.0 SI24R1芯片需要先读取一次 保证spi正常之后再写
Int_SI24R1_Read_Buf(SI24R1_READ_REG+TX_ADDR,si24r1_rx_buff,TX_ADR_WIDTH);
//1.1写入发送地址
Int_SI24R1_Write_Buf(SI24R1_WRITE_REG + TX_ADDR, TX_ADDRESS, TX_ADR_WIDTH); // 写入发送地址
//1.2读取同样的数据
Int_SI24R1_Read_Buf(SI24R1_READ_REG+TX_ADDR,si24r1_rx_buff,TX_ADR_WIDTH);
for(uint8_t i=0;i<TX_ADR_WIDTH;i++)
{
if(si24r1_rx_buff[i]!=TX_ADDRESS[i])
{
return 1;
}
}
return 0;
}
/**
* @brief 硬件接口层SI24R1的初始化
*
*/
void Int_SI24R1_Init(void)
{
//上电延时200ms >100ms
HAL_Delay(200);
//校验检测
while (Int_SI24R1_Check()==1)
{
//每两次检测间隔10ms
HAL_Delay(10);
}
//设置默认状态为接收模式 => 每次发送数据的时候 切换到发送状态
Int_SI24R1_RX_Mode();
debug_printf("si24r1 init ok\r\n");
}
二，应用接收层
#include "APP_receive_data.h"
extern Remote_Data remote_data;
uint8_t rx_buff[TX_PLOAD_WIDTH]={0};
//遥控连接状态
extern Remote_State remote_state;
//飞行状态
extern Flight_State flight_state;
//油门解锁状态值
Thr_state thr_state = FREE;
//MAX状态的进入时间
uint32_t max_enter_time = 0;
//MIN状态的进入时间
uint32_t min_enter_time = 0;
//重试次数
uint8_t retry_count=0;
//按下定高之后的飞行高度
extern uint16_t fix_height;
/**
* @brief 接收遥控器发送的遥控数据 => 解析为结构体
*
* @return uint8_t 0:校验通过 是正常的数据 1:没收到数据 或者 校验失败
*/
uint8_t APP_receive_data(void)
{
memset(rx_buff,0,TX_PLOAD_WIDTH);
Int_SI24R1_RxPacket(rx_buff);
if(strlen((char *)rx_buff)==0)
{
return 1;
}
//1.帧头校验
if(rx_buff[0]!=frame_head_check_1||rx_buff[1]!=frame_head_check_2||rx_buff[2]!=frame_head_check_3)
{
return 1;
}
//2.校验和验证
uint32_t sum=0;
uint32_t sum_receive=0;
for(uint8_t i=0;i<13;i++)
{
sum+=rx_buff[i];
}
//计算接收到的校验和
sum_receive= rx_buff[13]<<24|rx_buff[14]<<16|rx_buff[15]<<8|rx_buff[16];
if(sum!=sum_receive)
{
return 1;
}
//3.解析遥控数据
remote_data.thr= (rx_buff[3]<<8)|rx_buff[4];
remote_data.yaw= (rx_buff[5]<<8)|rx_buff[6];
remote_data.pit= (rx_buff[7]<<8)|rx_buff[8];
remote_data.rol= (rx_buff[9]<<8)|rx_buff[10];
remote_data.shutdown=rx_buff[11];
remote_data.fix_height=rx_buff[12];
//debug_printf(":%d,%d,%d,%d,%d,%d\n",remote_data.thr,remote_data.yaw,remote_data.pit,remote_data.rol,remote_data.shutdown,remote_data.fix_height);
return 0;
}
/**
* @brief 处理连接状态
*
* @param res 上一次接收数据的返回值
*/
void APP_process_connect_state(uint8_t res)
{
if(res == 0)
{
//接收成功 重置重试次数
remote_state = remote_connect;
retry_count =0;
}
else if(res ==1)
{
//接收失败增加校验次数 超出最大次数 判定为失败
retry_count++;
if(retry_count >= MAX_RETRY_TIMES)
{
remote_state = remote_disconnect;
retry_count =0;
}
}
}
/**
* @brief 处理解锁逻辑
*
* @return uint8_t 0:解锁成功 1:解锁失败
*/
static uint8_t APP_process_unlock(void)
{
//1.考虑安全问题 => 解锁完成的最终状态应该是油门为0
switch (thr_state)
{
case FREE:
if(remote_data.thr >= 900)
{
//2.进入MAX状态
thr_state = MAX;
//freeRTOS中以毫秒为单位的时间
max_enter_time = xTaskGetTickCount();
}
break;
case MAX:
//3.持续的时间应该是离开的时间减去进入的时间
if (remote_data.thr < 900)
{
if(xTaskGetTickCount() - max_enter_time >= 1000)
{
//4.油门保持最高状态超过了一秒 进入 leave max状态
thr_state = LEAVE_MAX;
}
else
{
//5.油门保持最高状态时间小于1s 退回到free 重新解锁
thr_state = FREE;
}
}
break;
case LEAVE_MAX:
if (remote_data.thr <= 100)
{
//6.油门回到0 进入min状态
thr_state = MIN;
min_enter_time = xTaskGetTickCount();
}
break;
case MIN:
//7.每次判断当前已经保持了多久
if (xTaskGetTickCount() - min_enter_time <= 1000)
{
//还不够1s
if (remote_data.thr > 100)
{
thr_state = FREE;
}
}
else
{
//已经保持够一秒 => 解锁完成
thr_state = UNLOCK;
}
break;
case UNLOCK:
/* code */
break;
default:
break;
}
if(thr_state == UNLOCK)
{
return 0;
}
return 1;
}
/**
* @brief 处理飞机的飞行状态
*
*/
void APP_process_flight_state(void)
{
//使用状态机逻辑实现
//1.使用轮询调用判断当前所属的状态
switch (flight_state)
{
case IDLE:
//2.只需要编写指向其它状态的代码即可
if (APP_process_unlock() == 0)
{
flight_state = normal;
//每次解锁成功 需要将解锁的状态重置
thr_state = FREE;
}
break;
case normal:
//3.判断进入定高
if(remote_data.fix_height == 1)
{
flight_state = FIX_HEIGHT;
remote_data.fix_height = 0;
//记录下当前目标的高度
fix_height = Int_VL53L1X_GetDistance();
}
//4.判断进入失联状态
if (remote_state == remote_disconnect)
{
flight_state = FAIL;
}
break;
case FIX_HEIGHT:
//5.取消定高
if(remote_data.fix_height == 1)
{
flight_state = normal;
remote_data.fix_height = 0;
}
//6.判断故障
if (remote_state == remote_disconnect)
{
flight_state = FAIL;
}
break;
case FAIL:
//7.处理失联故障 缓慢停止电机
//TODO
//等待故障处理完成 => 一直等待处理完成不会出现超时
ulTaskNotifyTake(pdTRUE,portMAX_DELAY);
flight_state = IDLE;
break;
default:
break;
}
}
openEuler 是由开放原子开源基金会孵化的全场景开源操作系统项目，面向数字基础设施四大核心场景（服务器、云计算、边缘计算、嵌入式），全面支持 ARM、x86、RISC-V、loongArch、PowerPC、SW-64 等多样性计算架构
更多推荐
11
0- 0
已为社区贡献1条内容
所有评论(0)