四轴飞控10——滤波处理+姿态解算+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处理一次
// 一直等任务通知 直到收到通知res=1 或者 超时res=0
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方波的占空比(前两个电机一组/后两个电机一组)
智能硬件社区聚焦AI智能硬件技术生态，汇聚嵌入式AI、物联网硬件开发者，打造交流分享平台，同步全国赛事资讯、开展 OPC 核心人才招募，助力技术落地与开发者成长。
更多推荐
4
0- 0
已为社区贡献7条内容
所有评论(0)