目录一、Keil二、VSCode①com_filter.h②com_filter.c③App_flight.h④App_flight.c⑤Com_imu.h⑥Com_imu.c⑦Com_pid.h⑧Com_pid.c⑨App_freertos_task.c三、注意四、概念(1)PID公式解读(2)串级PID一、Keil(1)在Common文件夹下创建com_filter/imu/pid.c/.h文件(2)添加.c/.h文件二、VSCode①com_filter.h#ifndef __COMMON_FILTER_H #define __COMMON_FILTER_H #include Com_debug.h /* 卡尔曼滤波器结构体 */ typedef struct { float LastP; // 上一时刻的状态方差或协方差 float Now_P; // 当前时刻的状态方差或协方差 float out; // 滤波器的输出值即估计的状态 float Kg; // 卡尔曼增益用于调节预测值和测量值之间的权重 float Q; // 过程噪声的方差反映系统模型的不确定性 float R; // 测量噪声的方差反映测量过程的不确定性 } KalmanFilter_Struct; extern KalmanFilter_Struct kfs[3]; int16_t Common_Filter_LowPass(int16_t newValue, int16_t preFilteredValue); double Common_Filter_KalmanFilter(KalmanFilter_Struct *kf, double input); #endif②com_filter.c#include Com_filter.h // 新值的系数 值越小 低通滤波的效果越强 #define ALPHA 0.15 /* 一阶低通滤波 指数加权系数 */ /** * description: 一阶低通滤波 * 是一种常用的滤波器用于去除高频噪声或高频成分保留信号中的低频成分。 * 在单片机应用中一种简单且常见的低通滤波器是一阶无限脉冲响应IIR低通滤波器 * 通常实现为指数加权移动平均滤波器。 * param {int16_t} newValue 需要滤波的值 * param {int16_t} preFilteredValue 上一次滤波过的值 * return {*} */ int16_t Common_Filter_LowPass(int16_t newValue, int16_t preFilteredValue) { return ALPHA * newValue (1 - ALPHA) * preFilteredValue; } /* 卡尔曼滤波 https://www.mwrf.net/tech/basic/2023/30081.html https://www.kalmanfilter.net/CN/default_cn.aspx*/ /* 卡尔曼滤波参数 */ KalmanFilter_Struct kfs[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}}; double Common_Filter_KalmanFilter(KalmanFilter_Struct *kf, double input) { kf-Now_P kf-LastP kf-Q; kf-Kg kf-Now_P / (kf-Now_P kf-R); kf-out kf-out kf-Kg * (input - kf-out); kf-LastP (1 - kf-Kg) * kf-Now_P; return kf-out; }③App_flight.h#ifndef __APP_FLIGHT__ #define __APP_FLIGHT__ #include math.h #include Com_debug.h #include Com_filter.h #include Com_imu.h #include Com_pid.h #include Int_motor.h #include Int_mpu6050.h /** * brief 飞控任务初始化 MPU6050初始化 启动电机 * */ void App_flight_init(void); /** * brief 根据陀螺仪测量的数据 计算出欧拉角 * */ void App_flight_get_euler_angle(void); /** * brief 根据欧拉角 计算出PID的目标值 * */ void App_flight_pid_process(void); /** * brief 根据PID的输出值 控制电机 * */ void App_flight_control_motor(void); #endif // __APP_FLIGHT__④App_flight.c#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; // 电机结构体 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 -7.00, .ki 0.00, .kd 0.00}; // Y轴角速度结构体 对应俯仰角的内环 // 极性问题 参数的正负可以调节 作用于电机的时候 正负 PID_Struct gyro_y_pid {.kp 3.00, .ki 0.00, .kd 0.50}; // 横滚角PID结构体 PID_Struct roll_pid {.kp -7.00, .ki 0.00, .kd 0.00}; // X轴角速度结构体 对应横滚角角的内环 PID_Struct gyro_x_pid {.kp 3.00, .ki 0.00, .kd 0.50}; /** * brief 飞控任务初始化 MPU6050初始化 启动电机 * */ void App_flight_init(void) { Int_MPU6050_Init(); // 启动电机 Int_motor_start(left_top_motor); Int_motor_start(left_bottom_motor); Int_motor_start(right_top_motor); Int_motor_start(right_bottom_motor); } /** * brief 根据陀螺仪测量的数据 计算出欧拉角 * */ void App_flight_get_euler_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) { // 俯仰角 // 1. 需要赋值目标值和测量值 // 外环的目标角度 如果是平稳飞行 值为0 如果需要遥控飞行 目标角度就是遥控器的值 // 数值转换 remote_data.pit(0-1000,500为中间点) 控制范围在±10° pitch_pid.desire (remote_data.pit - 500) / 50.0; // 内环的测量值 就是当前的俯仰角 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); // 先观察内环 角速度控制 目标位角速度为0 // debug_printf(:%.2f,%.2f\n, gyro_y_pid.err, gyro_y_pid.output); // 横滚角 // 1. 需要赋值目标值和测量值 // 外环的目标角度 如果是平稳飞行 值为0 如果需要遥控飞行 目标角度就是遥控器的值 roll_pid.desire (remote_data.rol - 500) / 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); } /** * 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: // 俯仰角 向前飞有角速度 正误差 需要一个向后飞的反馈效果 前两个电机转的快 后两个转的慢 left_top_motor.speed remote_data.thr gyro_y_pid.output - gyro_x_pid.output; left_bottom_motor.speed remote_data.thr - gyro_y_pid.output - gyro_x_pid.output; right_top_motor.speed remote_data.thr gyro_y_pid.output gyro_x_pid.output; right_bottom_motor.speed remote_data.thr - gyro_y_pid.output gyro_x_pid.output; break; case FIX_HEIGHT: break; case FAIL: break; default: break; } // 限制电机速度的上限值 left_top_motor.speed Com_limit(left_top_motor.speed, 600, 0); left_bottom_motor.speed Com_limit(left_bottom_motor.speed, 600, 0); right_top_motor.speed Com_limit(right_top_motor.speed, 600, 0); right_bottom_motor.speed Com_limit(right_bottom_motor.speed, 600, 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); }⑤Com_imu.h#ifndef __COMMON_IMU_H #define __COMMON_IMU_H #include Com_debug.h #include Com_Config.h #include math.h /* 表示四元数的结构体 */ typedef struct { float q0; float q1; float q2; float q3; } Quaternion_Struct; extern float RtA; extern float Gyro_G; extern float Gyro_Gr; void Common_IMU_GetEulerAngle(Gyro_Accel_Struct *gyroAccel, Euler_struct *eulerAngle, float dt); float Common_IMU_GetNormAccZ(void); #endif⑥Com_imu.c#include Com_IMU.h /* 欧拉角计算 */ /* 开始 */ /* 计算欧拉角用到的3个参数 */ float RtA 57.2957795f; // 弧度-度 // 陀螺仪初始化量程-2000度/秒于 1/(65536 / 4000) 0.03051756*2 // float Gyro_G 0.03051756f * 2; float Gyro_G 4000.0 / 65536; // 度/s // 度每秒,转换弧度每秒则 2*0.03051756 * 0.0174533f 0.0005326*2 // float Gyro_Gr 0.0005326f * 2; float Gyro_Gr 4000.0 / 65536 / 180 * 3.1415926; // 弧度/s #define squa(Sq) (((float)Sq) * ((float)Sq)) /* 计算平方 */ /** * description: 快速计算 1/sqrt(num) * param {float} number */ static float Q_rsqrt(float number) { long i; float x2, y; const float threehalfs 1.5F; x2 number * 0.5F; y number; i *(long *)y; i 0x5f3759df - (i 1); y *(float *)i; y y * (threehalfs - (x2 * y * y)); // 1st iteration 第一次牛顿迭代 return y; } static float normAccz; /* z轴上的加速度 */ /** * description: 根据mpu的6轴数据, 获取表征姿态的欧拉角 * param {GyroAccel_Struct} *gyroAccel mpu的6轴数据 * param {EulerAngle_Struct} *EulerAngle 计算后得到的欧拉角 * param {float} dt 采样周期 (单位s) * return {*} */ void Common_IMU_GetEulerAngle(Gyro_Accel_Struct *gyroAccel, Euler_struct *eulerAngle, float dt) { volatile struct V { float x; float y; float z; } Gravity, Acc, Gyro, AccGravity; static struct V GyroIntegError {0}; static float KpDef 0.8f; static float KiDef 0.0003f; static Quaternion_Struct NumQ {1, 0, 0, 0}; float q0_t, q1_t, q2_t, q3_t; // float NormAcc; float NormQuat; float HalfTime dt * 0.5f; // 提取等效旋转矩阵中的重力分量 Gravity.x 2 * (NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2); Gravity.y 2 * (NumQ.q0 * NumQ.q1 NumQ.q2 * NumQ.q3); Gravity.z 1 - 2 * (NumQ.q1 * NumQ.q1 NumQ.q2 * NumQ.q2); // 加速度归一化 NormQuat Q_rsqrt(squa(gyroAccel-accel.accel_x) squa(gyroAccel-accel.accel_y) squa(gyroAccel-accel.accel_z)); Acc.x gyroAccel-accel.accel_x * NormQuat; Acc.y gyroAccel-accel.accel_y * NormQuat; Acc.z gyroAccel-accel.accel_z * NormQuat; // 向量差乘得出的值 AccGravity.x (Acc.y * Gravity.z - Acc.z * Gravity.y); AccGravity.y (Acc.z * Gravity.x - Acc.x * Gravity.z); AccGravity.z (Acc.x * Gravity.y - Acc.y * Gravity.x); // 再做加速度积分补偿角速度的补偿值 GyroIntegError.x AccGravity.x * KiDef; GyroIntegError.y AccGravity.y * KiDef; GyroIntegError.z AccGravity.z * KiDef; // 角速度融合加速度积分补偿值 Gyro.x gyroAccel-gyro.gyro_x * Gyro_Gr KpDef * AccGravity.x GyroIntegError.x; // 弧度制 Gyro.y gyroAccel-gyro.gyro_y * Gyro_Gr KpDef * AccGravity.y GyroIntegError.y; Gyro.z gyroAccel-gyro.gyro_z * Gyro_Gr KpDef * AccGravity.z GyroIntegError.z; // 一阶龙格库塔法, 更新四元数 q0_t (-NumQ.q1 * Gyro.x - NumQ.q2 * Gyro.y - NumQ.q3 * Gyro.z) * HalfTime; q1_t (NumQ.q0 * Gyro.x - NumQ.q3 * Gyro.y NumQ.q2 * Gyro.z) * HalfTime; q2_t (NumQ.q3 * Gyro.x NumQ.q0 * Gyro.y - NumQ.q1 * Gyro.z) * HalfTime; q3_t (-NumQ.q2 * Gyro.x NumQ.q1 * Gyro.y NumQ.q0 * Gyro.z) * HalfTime; NumQ.q0 q0_t; NumQ.q1 q1_t; NumQ.q2 q2_t; NumQ.q3 q3_t; // 四元数归一化 NormQuat Q_rsqrt(squa(NumQ.q0) squa(NumQ.q1) squa(NumQ.q2) squa(NumQ.q3)); NumQ.q0 * NormQuat; NumQ.q1 * NormQuat; NumQ.q2 * NormQuat; NumQ.q3 * NormQuat; /*机体坐标系下的Z方向向量*/ float vecxZ 2 * NumQ.q0 * NumQ.q2 - 2 * NumQ.q1 * NumQ.q3; /*矩阵(3,1)项*/ float vecyZ 2 * NumQ.q2 * NumQ.q3 2 * NumQ.q0 * NumQ.q1; /*矩阵(3,2)项*/ float veczZ 1 - 2 * NumQ.q1 * NumQ.q1 - 2 * NumQ.q2 * NumQ.q2; /*矩阵(3,3)项*/ float yaw_G gyroAccel-gyro.gyro_z * Gyro_G; // 将Z轴角速度陀螺仪值 转换为Z角度/秒 Gyro_G陀螺仪初始化量程-2000度每秒于1 / (65536 / 4000) 0.03051756*2 if ((yaw_G 0.5f) || (yaw_G -0.5)) // 数据太小可以认为是干扰不是偏航动作 { eulerAngle-yaw yaw_G * dt; // 角速度积分成偏航角 } eulerAngle-pitch asin(vecxZ) * RtA; // 俯仰角 eulerAngle-roll atan2f(vecyZ, veczZ) * RtA; // 横滚角 normAccz gyroAccel-accel.accel_x * vecxZ gyroAccel-accel.accel_y * vecyZ gyroAccel-accel.accel_z * veczZ; /*Z轴垂直方向上的加速度此值涵盖了倾斜时在Z轴角速度的向量和不是单纯重力感应得出的值*/ } /** * description: 获取Z轴上的加速度 (如果已经倾斜,会考虑z轴上加速度的合成) * return {*} */ float Common_IMU_GetNormAccZ(void) { return normAccz; } /* 欧拉角计算 */ /* 结束 */⑦Com_pid.h#ifndef __COM_PID__ #define __COM_PID__ #include main.h #define PID_PERIOD 0.006 // PID结构体 如果CPU性能够强 推荐使用double // kp,ki,kd需要在初始化时确定 目标值和测量值 需要在计算时传递 typedef struct { float kp; // 比例部分 值越大响应速度越快 float ki; // 积分部分 解决稳态误差 无人机控制中 积分项一般不使用 float kd; // 微分部分 值越大 抑制效果越好 解决过调震荡 float err; // 误差值 float desire; // 目标值 float measure; // 测量值 float last_err; // 上一次的误差 float integral; // 积分累积 float output; // 输出结果 } PID_Struct; // 单次PID计算 void Com_PID_Calc(PID_Struct *pid); // 串级PID计算 void Com_PID_Calc_Chain(PID_Struct *out_pid, PID_Struct *in_pid);\ /** * brief 限制数值在正常的范围内 * * param speed * param max_speed * param min_speed * return int16_t */ int16_t Com_limit(int16_t speed, int16_t max_speed,int16_t min_speed); #endif // __COM_PID__⑧Com_pid.c#include Com_pid.h // 单次PID计算 void Com_PID_Calc(PID_Struct *pid) { // 1. 目标和测量 计算误差值 pid-err pid-measure - pid-desire; // 2. 计算积分误差 pid-integral pid-err; if (pid-last_err 0) { pid-last_err pid-err; } // 3. 计算微分误差 float der pid-err - pid-last_err; // 4. 计算输出 pid-output pid-kp * pid-err (pid-ki * pid-integral * PID_PERIOD) (pid-kd * der / PID_PERIOD); // 5. 保存上一次误差 pid-last_err pid-err; } // 串级PID计算 void Com_PID_Calc_Chain(PID_Struct *out_pid, PID_Struct *in_pid) { // 1.先计算外环 Com_PID_Calc(out_pid); // 2.将外环的输出值作为内环的目标值 in_pid-desire out_pid-output; // 3. 计算内环 Com_PID_Calc(in_pid); } /** * brief 限制数值在正常的范围内 * * param speed * param max_speed * param min_speed * return int16_t */ int16_t Com_limit(int16_t speed, int16_t max_speed, int16_t min_speed) { if (speed max_speed) { return max_speed; } else if (speed min_speed) { return min_speed; } return speed; }⑨App_freertos_task.c#include App_freeRTOS_Task.h // STM32F103C8T6 SRAM 20k 分配12K给操作系统 // 内存管理 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_DISCONNECTED; // 表示当前的飞行状态 Flight_State flight_state IDLE; // 扩展获取接收的遥控数据 Remote_Data remote_data {.thr 0, .yaw 500, .pit 500, .rol 500, .fix_height 0, .shutdown 0}; // 电源管理任务 void power_task(void *args); // 最小推荐填写128 128*4 512B #define POWER_TASK_STACK_SIZE 128 // 任务优先级 数值越小 优先级越小 最大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 6 /** * brief 启动freeRTOS操作系统 * */ 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(); } void power_task(void *args) { // 获取当前的基准时间 TickType_t xLastWakeTime xTaskGetTickCount(); while (1) { // // 每10s执行一次 启动电源 避免自动关机 // vTaskDelayUntil(xLastWakeTime, POWER_TASK_PERIOD); // // 启动电源 // Int_IP5305T_start(); // 使用直接任务通知的接收方法实现10s处理一次 // 一直等任务通知 直到收到通知res1 或者 超时res0 uint32_t res ulTaskNotifyTake(pdTRUE, POWER_TASK_PERIOD); if (res ! 0) { // 收到关机通知 Int_IP5305T_shutdown(); } else { // 不需要关机 正常执行启动 Int_IP5305T_start(); } } } void flight_task(void *args) { // 获取当前的基准时间 TickType_t xLastWakeTime xTaskGetTickCount(); // 一定要先对MPU6050执行初始化操作 之后才能读取数据 App_flight_init(); while (1) { // 1. 获根据MPU6050测量的数据 姿态解算得到欧拉角 App_flight_get_euler_angle(); // 2. 根据当前的欧拉角 进行PID计算控制 App_flight_pid_process(); // 3. 根据PID计算的结果 对电机进行控制 App_flight_control_motor(); vTaskDelayUntil(xLastWakeTime, FLIGHT_TASK_PERIOD); } } void led_task(void *args) { // 获取当前的基准时间 TickType_t xLastWakeTime xTaskGetTickCount(); uint8_t count 0; while (1) { count; // 前两个灯表示连接状态 // 1. 判断当前连接状态 if (remote_state REMOTE_CONNECTED) { // 点亮前两个灯 Int_led_turn_on(left_top_led); Int_led_turn_on(right_top_led); } else if (remote_state REMOTE_DISCONNECTED) { // 关掉前两个灯 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(xLastWakeTime, LED_TASK_PERIOD); } } void com_task(void *args) { // 获取当前的基准时间 TickType_t xLastWakeTime xTaskGetTickCount(); while (1) { // 1. 接收数据 uint8_t res App_receive_data(); // 2. 根据接收数据的返回值 处理当前飞机的连接状态 App_process_connect_state(res); // 3. 处理关机命令 if (remote_data.shutdown 1) { // 使用Int_IP5305T_shutdown 关机 功能可以实现 但是项目结构比较奇怪 // Int_IP5305T_shutdown(); // 使用freeRTOS直接任务通知 通知电源任务 执行关机 xTaskNotifyGive(power_task_handle); } // 4. 处理飞机的飞行状态 App_process_flight_state(); // 6ms执行一次 接收数据的时间间隔应该等于发送数据的时间间隔 vTaskDelayUntil(xLastWakeTime, COM_TASK_PERIOD); } }三、注意①低通滤波和卡尔曼滤波移植的代码。(获取完MPU6050的加速度和角速度后进行滤波处理)②四元数姿态解算移植的代码。(根据互补解算的角速度积分(只能得出偏航角)/加速度解算(得到俯仰角和横滚角)或者四元数姿态解算来得出当前欧拉角)四、概念(1)PID公式解读①比例部分与当前误差成正比该部分的作用是减少误差②积分部分与历史误差的积分成正比该部分的作用是消除稳态误差③微分部分与当前的误差变化率成正比该部分的作用是减少超调现象(2)串级PID①外环是角度控制目标值平稳飞行 角度为0(需要控制前后飞行)测量值是通过姿态解算得到当前的俯仰角②内环是角速度控制外环的输出值是内环的目标值测量值是MPU6050测量得到的角速度输出值控制 电机的转速➡PWM方波的占空比(前两个电机一组/后两个电机一组)