专业版
无人机V1.0星火版
3.4k
0
0
13
简介
本项目是基于 STM32 的小型四轴无人机，相较于中大型四轴无人机，小型四轴无人机具有成本低、结构简单等优势。控制核心采用 STM32F103C8T6，姿态运动传感器选择MPU6050。
简介：本项目是基于 STM32 的小型四轴无人机，相较于中大型四轴无人机，小型四轴无人机具有成本低、结构简单等优势。控制核心采用 STM32F103C8T6，姿态运动传感器选择MPU6050。开源协议
：GPL 3.0
(未经作者授权，禁止转载)创建时间：2025-06-30 15:04:01更新时间：2026-01-03 00:37:09
描述
项目简介
本项目是基于 STM32 的小型四轴无人机，相较于中大型四轴无人机，小型四轴无人机具有成本低、结构简单等优势。控制核心采用STM32F103C8T6，姿态运动传感器选择MPU6050。
项目功能
可以实现姿态控制，实时操作系统，数据显示，遥控控制等
项目参数
- 控制核心采用 STM32F103C8T6，姿态运动传感器选择 MPU6050。
- 无人机通过 Si24R1（NRF24L01）与控制器进行 2.4G 无线通信，实现了即时有效地接收控制器指令，通过串级 PID 进行姿态控制，从而在空间中实现自由移动。
原理解析（硬件说明）
本项目由飞控板和遥控板两部分组成，其中主控板包括电源部分、辅助模块部分、主控部分、电机驱动部分，遥控板包括主控部分，电源部分，摇杆按键部分，本项目主要是通过遥控板控制飞控板进行飞行，以及姿态控制等
电源电路：
飞控板和遥控板电源电路选择了IP5305T的充电芯片进行供电和给电池充电，还采用了AMS1117-3.3的芯片进行降压，给主芯片供电。
飞控主控电路：
使用的是ST公司的STM32F103C8T6. 主频最高可达72MHz, SRAM有20K, Flash有64K. 定时器有4个.
LED指示电路
ADC电池电压采集电路和MCU滤波电路
MPU-6050电路
选用的是MPU6050运动传感器.
包含一个 3 轴陀螺仪和一个 3 轴加速度计，并集成了一个数字运动处理器（DMP）。这种传感器广泛应用于无人机、机器人、智能手机和可穿戴设备中，用于姿态检测和运动跟踪。
传感器的数据可以通过 I2C 接口读取。
MPU6050 是一种功能强大的 6 轴运动传感器，广泛应用于姿态控制和运动检测。其内置的 DMP 使得复杂的运动算法处理更加高效，适合多种嵌入式应用。
2.4G收发模块
Si24R1 是一款高性能、低成本的 2.4GHz 射频收发芯片，适用于多种无线通信应用。它的低功耗、高数据速率和多通道支持，使其成为无线传感器网络、智能家居、遥控玩具和计算机外设等领域的理想选择。通过 SPI 接口，可以方便地与各种微控制器进行通信，实现无线数据传输。
晶振电路
电机电路
使用的是8520空心杯电机. 主要用于单浆遥控飞机和四轴飞行器，具有扭力大重量轻，飞行时间长等优点. 由顺时针旋转和逆时针旋转2种.(注意: 一种空心杯只能有一种转动方向)
尺寸: 8.5mm 直径，20mm 长度.
工作电压：通常为3.7V（单节锂电池）
无负载转速：在额定电压下，通常为几万转每分钟（RPM）
无负载电流：通常在几十毫安（mA）左右.
重量：几克，通常不超过10克.
通常用于驱动小型四旋翼无人机的螺旋桨，提供高转速和高功率密度。
8520空心杯电机因其高效、轻便、快速响应等特点，广泛应用于各种微型设备中。其空心杯设计使得电机在高性能需求的微型应用中表现出色。尽管其制造成本较高，但在需要高性能和精密控制的场景下，其优势是无可替代的。
使用PWM可以方便的驱动电机转动
辅助模块
激光测距模块
摇杆按键电路
OLED显示电路
软件架构
本项目使用的HAL库
软件代码
- 公共配置模块
- 滤波模块
#include "Com_Filter.h"
#define ALPHA 0.15 /* 一阶低通滤波 指数加权系数 */
/**
* @description: 一阶低通滤波
* 是一种常用的滤波器，用于去除高频噪声或高频成分，保留信号中的低频成分。
* 在单片机应用中，一种简单且常见的低通滤波器是一阶无限脉冲响应（IIR）低通滤波器，
* 通常实现为指数加权移动平均滤波器。
* @param {int16_t} newValue 需要滤波的值
* @param {int16_t} preFilteredValue 上一次滤波过的值
* @return {*}
*/
int16_t Com_Filter_LowPass(int16_t newValue, int16_t preFilteredValue)
{
return ALPHA * newValue + (1 - ALPHA) * preFilteredValue;
}
/* 卡尔曼滤波 https://www.mwrf.net/tech/basic/2023/30081.html */
/* 卡尔曼滤波参数 */
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 Com_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;
}
- PID算法
#include "Com_PID.h"
void Com_PID_Calculate(PID_Struct *pid,float dt)
{
// 计算角度误差
float error = pid->measure - pid->desire;
// 计算积分部分
pid->integral += error * dt;
// 计算微分部分
float der = (error - pid->last_error) / dt;
pid->last_error = error;
pid->output = pid->kp * error + pid->ki * pid->integral + pid->kd * der;
}
void Com_PID_Cascade(PID_Struct *outter, PID_Struct *inner, float dt)
{
// 需要先计算外环
Com_PID_Calculate(outter, dt);
// 外环的输出值等于内环的目标值
inner->desire = outter->output;
// 在计算内环
Com_PID_Calculate(inner, dt);
}
- 姿态解算
#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 double normAccz; /* z轴上的加速度 */
/**
* @description: 根据mpu的6轴数据, 获取表征姿态的欧拉角
* @param {Gyro_Acc_Struct} *gyroacc mpu的6轴数据
* @param {EulerAngle_Struct} *EulerAngle 计算后得到的欧拉角
* @param {float} dt 采样周期 (单位s)
* @return {*}
*/
void Com_IMU_GetEulerAngle(Gyro_Acc_Struct *gyroacc,
EulerAngle_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(gyroacc->acc.x) +
squa(gyroacc->acc.y) +
squa(gyroacc->acc.z));
Acc.x = gyroacc->acc.x * NormQuat;
Acc.y = gyroacc->acc.y * NormQuat;
Acc.z = gyroacc->acc.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 = gyroacc->gyro.x * Gyro_Gr + KpDef * AccGravity.x + GyroIntegError.x; // 弧度制
Gyro.y = gyroacc->gyro.y * Gyro_Gr + KpDef * AccGravity.y + GyroIntegError.y;
Gyro.z = gyroacc->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 = gyroacc->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 = gyroacc->acc.x * vecxZ + gyroacc->acc.y * vecyZ + gyroacc->acc.z * veczZ; /*Z轴垂直方向上的加速度，此值涵盖了倾斜时在Z轴角速度的向量和，不是单纯重力感应得出的值*/
}
/**
* @description: 获取Z轴上的加速度 (如果已经倾斜,会考虑z轴上加速度的合成)
* @return {*}
*/
float Com_IMU_GetNormAccZ(void)
{
return normAccz;
}
/* ======================欧拉角计算================================== */
/* ========================结束===================================== */
- 应用层模块
- 飞控模块
#include "App_Flight.h"
extern TaskHandle_t comm_task_handle;
/* 定义电机结构体 */
Motor_Struct motor_left_top = {&htim3,TIM_CHANNEL_1,0};
Motor_Struct motor_right_top = {&htim2,TIM_CHANNEL_2,0};
Motor_Struct motor_right_bottom = {&htim1,TIM_CHANNEL_3,0};
Motor_Struct motor_left_bottom = {&htim4,TIM_CHANNEL_4,0};
Gyro_Struct last_gyro;
// PID计算参数
// 俯仰角
// 一共需要调节6个参数 => 不需要使用积分部分 => 只使用4个参数
// 首先要条件内环参数 => 内环参数是可以单独使用的
// 内环条件完成实现的效果 => 尽量保持角速度为0 => 表现为偏转过程会停顿
// 再调节外环 => 只有调节完成外环的参数 => 飞机倾斜之后才会恢复
PID_Struct pid_pitch = {.kp = -7.0f,.ki = 0.0f,.kd = 0.0f};
PID_Struct pid_gyro_y = {.kp = 3.0f,.ki = 0.0f,.kd = 0.2f};
// 横滚角
PID_Struct pid_roll = {.kp = -7.0f,.ki = 0.0f,.kd = 0.0f};
PID_Struct pid_gyro_x = {.kp = -3.0f,.ki = 0.0f,.kd = -0.2f};
// 偏航角
// 内环调节完成之后 => 手拨动飞机 会尽量停止旋转 但是不会回转
// 外环调节完成之后 => 手拨动飞机 反向旋转 恢复 偏转
PID_Struct pid_yaw = {.kp = -2.0f,.ki = 0.0f,.kd = 0.0f};
PID_Struct pid_gyro_z = {.kp = -1.5f,.ki = 0.0f,.kd = 0.0f};
// 高度
// 使用串级PID计算：外环：高度 内环：z轴的速度
// 先调节内环Kp => 判断极性 => -1
// 判断大小 => 限制了档位的上限 -1.2
// 在调节外环Kp => 判断极性 => -1
PID_Struct pid_height = {.kp = -3.0f,.ki = 0.0f,.kd = 0.0f};
PID_Struct pid_z_speed = {.kp = -1.2f,.ki = 0.0f,.kd = -0.1f};
void App_Flight_Set_Speed(Flight_Status status, Remote_Struct *remote_data)
{
switch (status)
{
case IDLE:
// 加锁静止状态 => 电机转速恒为零
motor_left_top.speed = 0;
motor_right_top.speed = 0;
motor_right_bottom.speed = 0;
motor_left_bottom.speed = 0;
break;
case NORMAL:
// 遥控杆向前推 俯仰角变大 => 飞机向前飞 => 后面的电机转的快
// 遥控杆向右推 横滚角变大 => 飞机向右飞 => 左边的电机转的快
// 旋转摇杆向右推 偏航角变大 => 顺时针旋转 2 4 电机转的快
// 方向摇杆直接控制电机转速 力度太大 => 需要等比例缩小
// motor_left_bottom.speed = remote_->THR +((remote_->PIT - 500) + (remote_->ROL - 500) + (remote_->YAW - 500)) / 10;
// motor_right_bottom.speed = remote_->THR +((remote_->PIT - 500) - (remote_->ROL - 500) - (remote_->YAW - 500)) / 10;
// motor_left_top.speed = remote_->THR + (( 500 -remote_->PIT) + (remote_->ROL - 500) - (remote_->YAW - 500))/10;
// motor_right_top.speed = remote_->THR + (( 500 -remote_->PIT ) - (remote_->ROL - 500) + (remote_->YAW - 500))/10;
// 将PID计算结果作用于电机转速
motor_left_top.speed = LIMIT(LIMIT(remote_data->THR,0,800) + (pid_gyro_y.output) + (pid_gyro_x.output) + (pid_gyro_z.output),0,1000);
motor_right_top.speed = LIMIT(LIMIT(remote_data->THR,0,800) + (pid_gyro_y.output) - (pid_gyro_x.output) - (pid_gyro_z.output),0,1000);
motor_left_bottom.speed = LIMIT(LIMIT(remote_data->THR,0,800) - (pid_gyro_y.output) + (pid_gyro_x.output) - (pid_gyro_z.output),0,1000);
motor_right_bottom.speed = LIMIT(LIMIT(remote_data->THR,0,800) - (pid_gyro_y.output) - (pid_gyro_x.output) + (pid_gyro_z.output),0,1000);
break;
case FIX_HIGHT:
motor_left_top.speed = LIMIT(LIMIT(remote_data->THR,0,800) + (pid_gyro_y.output) + (pid_gyro_x.output) + (pid_gyro_z.output) + LIMIT(pid_z_speed.output,-150,150),0,1000);
motor_right_top.speed = LIMIT(LIMIT(remote_data->THR,0,800) + (pid_gyro_y.output) - (pid_gyro_x.output) - (pid_gyro_z.output)+ LIMIT(pid_z_speed.output,-150,150),0,1000);
motor_left_bottom.speed = LIMIT(LIMIT(remote_data->THR,0,800) - (pid_gyro_y.output) + (pid_gyro_x.output) - (pid_gyro_z.output)+ LIMIT(pid_z_speed.output,-150,150),0,1000);
motor_right_bottom.speed = LIMIT(LIMIT(remote_data->THR,0,800) - (pid_gyro_y.output) - (pid_gyro_x.output) + (pid_gyro_z.output)+ LIMIT(pid_z_speed.output,-150,150),0,1000);
break;
case FAULT:
if (motor_left_top.speed > 0)
{
motor_left_top.speed -= 10;
if (motor_left_top.speed < 0)
{
motor_left_top.speed = 0;
}
}
if (motor_right_top.speed > 0)
{
motor_right_top.speed -= 10;
if (motor_right_top.speed < 0)
{
motor_right_top.speed = 0;
}
}
if (motor_right_bottom.speed > 0)
{
motor_right_bottom.speed -= 10;
if (motor_right_bottom.speed < 0)
{
motor_right_bottom.speed = 0;
}
}
if (motor_left_bottom.speed > 0)
{
motor_left_bottom.speed -= 10;
if (motor_left_bottom.speed < 0)
{
motor_left_bottom.speed = 0;
}
}
// 电机转速全部都为0
if (motor_left_top.speed == 0 && motor_right_top.speed == 0 && motor_right_bottom.speed == 0 && motor_left_bottom.speed == 0)
{
// 故障处理完成
xTaskNotifyGive(comm_task_handle);
}
break;
default:
break;
}
// 安全停机 TODO
// 判断极性
// if (motor_left_top.speed > 100)
// {
// motor_left_top.speed = 100;
// }
// if (motor_right_top.speed > 100)
// {
// motor_right_top.speed = 100;
// }
// if (motor_right_bottom.speed > 100)
// {
// motor_right_bottom.speed = 100;
// }
// if (motor_left_bottom.speed > 100)
// {
// motor_left_bottom.speed = 100;
// }
// 判断大小
if (remote_data->THR < 30)
{
motor_left_top.speed = 0;
motor_right_top.speed = 0;
motor_right_bottom.speed = 0;
motor_left_bottom.speed = 0;
}
// 设置电机转速
Int_Motor_Set_Speed(&motor_left_top);
Int_Motor_Set_Speed(&motor_right_top);
Int_Motor_Set_Speed(&motor_right_bottom);
Int_Motor_Set_Speed(&motor_left_bottom);
}
void App_Flight_Get_Gyro_Acc(Gyro_Acc_Struct *gyro_acc)
{
taskENTER_CRITICAL();
Int_MPU6050_Read(gyro_acc);
taskEXIT_CRITICAL();
// 对角速度 使用一阶低通滤波
gyro_acc->gyro.x = Com_Filter_LowPass(gyro_acc->gyro.x, last_gyro.x);
gyro_acc->gyro.y = Com_Filter_LowPass(gyro_acc->gyro.y, last_gyro.y);
gyro_acc->gyro.z = Com_Filter_LowPass(gyro_acc->gyro.z, last_gyro.z);
last_gyro = gyro_acc->gyro;
// 对加速度 使用卡尔曼滤波
gyro_acc->acc.x = Com_Filter_KalmanFilter (&kfs[0],gyro_acc->acc.x);
gyro_acc->acc.y = Com_Filter_KalmanFilter (&kfs[1],gyro_acc->acc.y);
gyro_acc->acc.z = Com_Filter_KalmanFilter (&kfs[2],gyro_acc->acc.z);
}
void App_Flight_Get_EulerAngle(Gyro_Acc_Struct *gyro_acc, EulerAngle_Struct *euler_angle, float dt)
{
// 直接使用移植的方法获取欧拉角
Com_IMU_GetEulerAngle(gyro_acc, euler_angle, dt);
debug_printfln("euler: %f, %f, %f",euler_angle->pitch,euler_angle->roll,euler_angle->yaw);
}
void App_Flight_PID_Calculate(Gyro_Acc_Struct *gyro_acc, EulerAngle_Struct *euler_angle,Remote_Struct *remote_data,float dt)
{
// 对于PID计算的F(x) =>
// 目标角度 测量角度 测量角速度是自变量 需要用户传递
// kp ki kd 是常量系数 => 需要提前定义
// 因变量 最终内环PID的output
// 俯仰角
// (remote_data->PIT-500) * 0.02 限定摇杆控制俯仰角的最大值为10°
pid_pitch.desire = (remote_data->PIT-500) * 0.02;
pid_pitch.measure = euler_angle->pitch;
pid_gyro_y.measure = gyro_acc->gyro.y * Gyro_G;
Com_PID_Cascade(&pid_pitch, &pid_gyro_y, dt);
// 横滚角
pid_roll.desire = (remote_data->ROL-500) * 0.02;
pid_roll.measure = euler_angle->roll;
pid_gyro_x.measure = gyro_acc->gyro.x * Gyro_G;
Com_PID_Cascade(&pid_roll, &pid_gyro_x, dt);
// 偏航角
pid_yaw.desire = (remote_data->YAW-500) * 0.02;
pid_yaw.measure = euler_angle->yaw;
pid_gyro_z.measure = gyro_acc->gyro.z * Gyro_G;
Com_PID_Cascade(&pid_yaw, &pid_gyro_z, dt);
}
uint16_t last_flight_height = 0;
float last_speed_z = 0.0;
float static_acc_z = 0.0;
void App_Flight_Fix_Height_PID(uint16_t flight_height,uint16_t fix_height,float dt)
{
// 传递参数 => 外环目标值和测量值 内环测量值
// 外环目标值： fix_height 测量值：flight_height
pid_height.desire = fix_height;
pid_height.measure = flight_height;
// 内环的测量值：当前的z轴速度
// 高度微分
float speed_z1 = (flight_height - last_flight_height) / dt;
last_flight_height = flight_height;
// 加速度积分 v1 =v0 + a*t
float acc_z = Com_IMU_GetNormAccZ() - static_acc_z;
;
float speed_z2 = last_speed_z + acc_z * dt;
// 混合算法
pid_z_speed.measure = 0.02 * speed_z1 + 0.98 * speed_z2;
last_speed_z = pid_z_speed.measure;
// 进行PID计算
Com_PID_Cascade(&pid_height, &pid_z_speed, dt);
}
- 通讯模块
#include "App_Comm_Rx_Data.h"
uint8_t rx_data_buff[TX_PLOAD_WIDTH];
uint16_t count = 0;
uint32_t last_tick = 0;
uint32_t max_start_tick = 0;
uint32_t min_start_tick = 0;
Com_Status App_Comm_Rx_Data(Remote_Struct *remote_data)
{
// 1.接收原始数据
uint8_t is_received = Int_SI24R1_RxPacket(rx_data_buff);
// 判断数据接收是否成功
if (is_received != 0)
{
return COM_ERROR;
}
// 2.进行唯一性校验
if (rx_data_buff[0] != FRAME0 || rx_data_buff[1] != FRAME1 || rx_data_buff[2] != FRAME2)
{
return COM_ERROR;
}
// 3.进行校验和判断
uint32_t check_sum = 0;
for(uint8_t i = 0; i < 13; i++)
{
check_sum += rx_data_buff[i];
}
// 拼接后四位
uint32_t receive_check_sum = rx_data_buff[13] | (rx_data_buff[14] << 8) | (rx_data_buff[15] << 16) | (rx_data_buff[16] << 24);
if (check_sum != receive_check_sum)
{
return COM_ERROR;
}
// 4.解析数据
remote_data-> THR = (rx_data_buff[3]) | (rx_data_buff[4] << 8);
remote_data-> YAW = (rx_data_buff[5]) | (rx_data_buff[6] << 8);
remote_data-> PIT = (rx_data_buff[7]) | (rx_data_buff[8] << 8);
remote_data-> ROL = (rx_data_buff[9]) | (rx_data_buff[10] << 8);
remote_data->FIX_HEIGHT = rx_data_buff[11];
remote_data->SHUT_DOWN = rx_data_buff[12];
debug_printfln(": %d,%d,%d,%d",remote_data-> THR,remote_data-> YAW,remote_data-> PIT,remote_data-> ROL);
return COM_OK;
}
/**
* (1) 收到一条正确的数据 就是连接成功
* (2) 多次连续收不到正确的数据 才是连接失败
*/
void App_Comm_Rx_Handle_Connect(Com_Status isReceived,Remote_Status *remote_status)
{
// if (isReceived == COM_OK)
// {
// *remote_status = CONNECTED;
// count = 0;
// }
// else if (isReceived == COM_ERROR)
// {
// count++;
// if (count > 100)
// {
// count = 100;
// *remote_status = DISCONNECTED;
// }
// }
// 处理连接状态第二种写法
if (isReceived == COM_OK)
{
*remote_status = CONNECTED;
last_tick = xTaskGetTickCount();
}
else if (isReceived == COM_ERROR)
{
// 单位是ms
if (xTaskGetTickCount() - last_tick > 600)
{
*remote_status = DISCONNECTED;
}
}
}
// 判断解锁逻辑
// 油门拉最高 1s => 拉最低1s
// 解锁状态最终一定是停止的 => 解锁完成的一瞬间 油门一定是0
/* 油门状态机 */
Com_Status App_Comm_Unlock(Remote_Struct *remote_data,THR_Status *thr_status)
{
switch (*thr_status)
{
case FREE:
if (remote_data->THR >= 900)
{
*thr_status = MAX;
// 进入MAX的时间
max_start_tick = xTaskGetTickCount();
}
break;
case MAX:
if (remote_data->THR < 900)
{
// 退出MAX的时间
if (xTaskGetTickCount() - max_start_tick > 1000)
{
*thr_status = LEAVE_MAX;
}
else
{
*thr_status = FREE;
}
}
break;
case LEAVE_MAX:
if (remote_data->THR <= 100)
{
*thr_status = MIN;
min_start_tick = xTaskGetTickCount();
}
break;
case MIN:
// 先判断解锁
if (xTaskGetTickCount() - min_start_tick <= 1000)
{
if (remote_data->THR > 100)
{
// 解锁失败
*thr_status = FREE;
}
else
{
// 解锁成功
*thr_status = UNLOCK;
}
}
break;
default:
break;
}
if (*thr_status == UNLOCK)
{
return COM_OK;
}
return COM_ERROR;
}
extern uint16_t flight_height;
extern uint16_t fix_height;
extern float static_acc_z;
/**
* 标准状态机函数 => 前置要求 整个方法在高速的循环调用
* (1) Flight_Status => 状态机本体
* (2) 不同的状态切换条件
*/
void App_Comm_Rx_Handle_Flight(Remote_Status remote_status,Remote_Struct *remote_data,Flight_Status *flight_status,THR_Status *thr_status)
{
switch (*flight_status)
{
case IDLE:
// 每个分支 只需要编写指出去的箭头
// 在连接成功的情况：触发解锁条件 => 进入NORMAL状态
if (remote_status == CONNECTED)
{
if (App_Comm_Unlock(remote_data,thr_status) == COM_OK)
{
*flight_status = NORMAL;
static_acc_z = Com_IMU_GetNormAccZ();
}
}
break;
case NORMAL:
if (remote_status == CONNECTED)
{
if (remote_data->FIX_HEIGHT == 1)
{
// 进入定高
*flight_status = FIX_HIGHT;
// 记录当前高度
fix_height = flight_height;
}
}
else
{
// 失联进入故障
*flight_status = FAULT;
}
break;
case FIX_HIGHT:
if (remote_status == CONNECTED)
{
if (remote_data->FIX_HEIGHT == 1)
{
// 进入普通
*flight_status = NORMAL;
}
}
else
{
// 失联进入故障
*flight_status = FAULT;
}
break;
case FAULT:
// 等待降落的逻辑 => 降落的逻辑在飞控任务中
// TODO 本身是Comm_task => 等待任务通知
ulTaskNotifyTake(pdTRUE,portMAX_DELAY);
// 最终进入到加锁的空闲状态
*flight_status = IDLE;
break;
}
}
- 任务调度模块
#include "FreeRTOS_task.h"
//遥控状态
Remote_Status remote_status = DISCONNECTED;
//飞行状态
Flight_Status flight_status = IDLE;
// 油门状态
THR_Status thr_status = FREE;
// 陀螺仪数据
Gyro_Acc_Struct gyro_acc;
// 欧拉角数据
EulerAngle_Struct euler_angle;
// 遥控数据
Remote_Struct remote_data;
// 飞行高度
uint16_t flight_height = 0;
uint16_t fix_height = 0;
/* 定义LED灯结构体 */
Led_Struct led_left_top = {LED1_GPIO_Port,LED1_Pin};
Led_Struct led_right_top = {LED2_GPIO_Port,LED2_Pin};
Led_Struct led_right_bottom = {LED3_GPIO_Port,LED3_Pin};
Led_Struct led_left_bottom = {LED4_GPIO_Port,LED4_Pin};
/* 电源任务 */
void power_task(void *pvParameters);
#define POWER_TASK_PRIORITY 3
#define POWER_TASK_STACK_SIZE 128
//电源芯片24s-40s低电流就会关机 => 设置24s以内的值即可
#define POWER_TASK_PERIOD 10000
TaskHandle_t power_task_handle;
/* 飞控任务 */
void flight_task(void *pvParameters);
#define FLIGHT_TASK_PRIORITY 4
#define FLIGHT_TASK_STACK_SIZE 128
#define FLIGHT_TASK_PERIOD 6
TaskHandle_t flight_task_handle;
/* LED灯控任务 */
void led_task(void *pvParameters);
#define LED_TASK_PRIORITY 3
#define LED_TASK_STACK_SIZE 128
#define LED_TASK_PERIOD 50
TaskHandle_t led_task_handle;
/* 通讯任务 */
void comm_task(void *pvParameters);
#define COMM_TASK_PRIORITY 4
#define COMM_TASK_STACK_SIZE 128
#define COMM_TASK_PERIOD 100
TaskHandle_t comm_task_handle;
void FreeRTOS_Start(void)
{
// 先初始化MPU6050 => 使用的是I2C需要时序
Int_MPU6050_Init();
Int_VL53L1X_Init();
/* 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(comm_task, "comm_task", COMM_TASK_STACK_SIZE, NULL, COMM_TASK_PRIORITY, &comm_task_handle);
//启动调度器
vTaskStartScheduler();
}
/**
* 电源任务
* 1.定时启动一次 避免进入低功耗模式（关机）
* 2.如果收到关机信号 => 执行关机
*/
void power_task(void *pvParameters)
{
//获取当前时间
TickType_t tick_count = xTaskGetTickCount();
while (1)
{
// 10s一个周期 => 如果收到关机信号 => 执行关机
// 如果没收到关机信号 => 执行开机
/**
* 任务间同步可以使用两个方法
* (1) 二值信号量
* (2) 任务通知 => 更简单 更效率
*/
// 0 => 收到任务通知 => 1
// 收到通知返回1
// 没收到返回0
uint32_t notify_result = ulTaskNotifyTake(pdTRUE, POWER_TASK_PERIOD);
if (notify_result == 0)
{
Int_IP5305T_Start();
}
else
{
Int_IP5305T_Stop();
}
}
}
/**
* 飞控任务
* 1.Todo 根据参数计算电机转速
* 2.设置电机转速
*/
void flight_task(void *pvParameters)
{
//初始化电机
Int_Motor_Init();
//获取当前时间
TickType_t tick_count = xTaskGetTickCount();
while (1)
{
//Todo 添加PID控制理论
// 1.获取陀螺仪数据
App_Flight_Get_Gyro_Acc(&gyro_acc);
// 2.姿态解算 获取欧拉角
App_Flight_Get_EulerAngle(&gyro_acc, &euler_angle,FLIGHT_TASK_PERIOD/1000.0);
// 3.使用PID计算
App_Flight_PID_Calculate(&gyro_acc, &euler_angle, &remote_data,FLIGHT_TASK_PERIOD/1000.0);
// 4.获取高度
Int_VL53L1X_Get_Distance(&flight_height);
// 5.进行高度PID计算
App_Flight_Fix_Height_PID(flight_height,fix_height,FLIGHT_TASK_PERIOD/1000.0);
// 直接根据状态设置电机转速
App_Flight_Set_Speed(flight_status, &remote_data);
vTaskDelayUntil(&tick_count, FLIGHT_TASK_PERIOD);
}
}
/*灯控任务*/
void led_task(void *pvParameters)
{
//获取当前时间
TickType_t tick_count = xTaskGetTickCount();
while (1)
{
//判断遥控状态 => 决定前两个灯的情况
switch (remote_status)
{
case DISCONNECTED:
//前两个灯灭
Int_LED_TurnOff(&led_left_top);
Int_LED_TurnOff(&led_right_top);
break;
case CONNECTED:
//前两个灯亮
Int_LED_TurnOn(&led_left_top);
Int_LED_TurnOn(&led_right_top);
break;
default:
break;
}
//判断飞行状态 => 决定后两个灯的情况
//uint8_t count = 0;
switch (flight_status)
{
case IDLE:
// 慢闪 => 100ms运行一次
// count++;
// if(count == 5)
// {
// //翻转引脚
// HAL_GPIO_TogglePin(led_left_bottom.port, led_left_bottom.pin);
// HAL_GPIO_TogglePin(led_right_bottom.port, led_right_bottom.pin);
// }
//调用闪烁方法
Int_LED_Blink(&led_left_bottom,500);
Int_LED_Blink(&led_right_bottom,500);
break;
case NORMAL:
// 快闪 => 翻转引脚
// HAL_GPIO_TogglePin(led_left_bottom.port, led_left_bottom.pin);
// HAL_GPIO_TogglePin(led_right_bottom.port, led_right_bottom.pin);
Int_LED_Blink(&led_left_bottom,100);
Int_LED_Blink(&led_right_bottom,100);
break;
case FIX_HIGHT:
/* 后两个灯常亮 */
Int_LED_TurnOn(&led_left_bottom);
Int_LED_TurnOn(&led_right_bottom);
break;
case FAULT:
/* 后两个灯常灭*/
Int_LED_TurnOff(&led_left_bottom);
Int_LED_TurnOff(&led_right_bottom);
break;
default:
break;
}
//50ms
vTaskDelayUntil(&tick_count, LED_TASK_PERIOD);
}
}
/* 通讯任务 */
void comm_task(void *pvParameters)
{
//初始化SI24R1
Int_SI24R1_Init();
//获取当前时间
TickType_t tick_count = xTaskGetTickCount();
while (1)
{
// 接收遥控数据
Com_Status isReceived = App_Comm_Rx_Data(&remote_data);
// 判断连接状态
App_Comm_Rx_Handle_Connect(isReceived,&remote_status);
// 判断关机逻辑 => 收到关机信号之后，直接关机即可
if (isReceived == COM_OK)
{
if (remote_data.SHUT_DOWN == 1)
{
//当前这一次收到了关机信号
// 给电源任务发送一个任务通知
xTaskNotifyGive(power_task_handle);
}
}
// 判断飞行状态 => 整个方法都在高速的调用 循环不断
App_Comm_Rx_Handle_Flight(remote_status,&remote_data,&flight_status,&thr_status);
vTaskDelayUntil(&tick_count,COMM_TASK_PERIOD);
}
}
- 硬件接口层
- 电源充电
#include "Int_IP5305T.h"
static void Int_IP5305T_Pressed(void)
{
//POWER_KEY按下为0 => 大于30ms小于2s
HAL_GPIO_WritePin(POWER_KEY_GPIO_Port, POWER_KEY_Pin, GPIO_PIN_RESET);
//调用vTaskDelay延时
vTaskDelay(80);
//释放引脚
HAL_GPIO_WritePin(POWER_KEY_GPIO_Port, POWER_KEY_Pin, GPIO_PIN_SET);
}
void Int_IP5305T_Start(void)
{
Int_IP5305T_Pressed();
}
void Int_IP5305T_Stop(void)
{
Int_IP5305T_Pressed();
vTaskDelay(300);
Int_IP5305T_Pressed();
}
- 电机驱动
#include "Int_motor.h"
void Int_Motor_Init(void)
{
//启动定时器
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_4);
}
/**
* 仅用于测试螺旋桨旋转
* 档位0-1000
*/
// void Int_Motor_Start(void)
// {
// __HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_1, 100);
// __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2, 100);
// __HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_3, 100);
// __HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_4, 100);
// }
void Int_Motor_Set_Speed(Motor_Struct *motor)
{
__HAL_TIM_SetCompare(motor->htim, motor->channel, motor->speed);
}
- LED驱动
#include "Int_LED.h"
void Int_LED_TurnOn(Led_Struct *led)
{
HAL_GPIO_WritePin(led->port, led->pin, GPIO_PIN_RESET);
}
void Int_LED_TurnOff(Led_Struct *led)
{
HAL_GPIO_WritePin(led->port, led->pin, GPIO_PIN_SET);
}
void Int_LED_Blink(Led_Struct *led,uint16_t period)
{
// 查看当前的时间
TickType_t current_tick = xTaskGetTickCount();
if(current_tick - led->last_tick >= period)
{
// 翻转一次引脚
HAL_GPIO_TogglePin(led->port, led->pin);
led->last_tick = current_tick;
}
}
- MPU6050驱动
#include "Int_MPU6050.h"
Gyro_Acc_Struct gyro_acc_bias;
void Int_MPU6050_WriteByte(uint8_t reg, uint8_t data)
{
HAL_I2C_Mem_Write(&hi2c1,MPU6050_ADDR_W,reg, I2C_MEMADD_SIZE_8BIT, &data, 1, 1000);
}
void Int_MPU6050_WriteBytes(uint8_t reg, uint8_t *data, uint8_t len)
{
HAL_I2C_Mem_Write(&hi2c1,MPU6050_ADDR_W,reg, I2C_MEMADD_SIZE_8BIT, data, len, 1000);
}
void Int_MPU6050_ReadByte(uint8_t reg, uint8_t *data)
{
HAL_I2C_Mem_Read(&hi2c1,MPU6050_ADDR_R,reg, I2C_MEMADD_SIZE_8BIT, data, 1, 1000);
}
void Int_MPU6050_ReadBytes(uint8_t reg, uint8_t *data, uint8_t len)
{
HAL_I2C_Mem_Read(&hi2c1,MPU6050_ADDR_R,reg, I2C_MEMADD_SIZE_8BIT, data, len, 1000);
}
void Int_MPU6050_Calculate(void);
void Int_MPU6050_Init(void)
{
// 1.复位 -> 延迟 -> 唤醒
Int_MPU6050_WriteByte(MPU_PWR_MGMT1_REG, 0x80);
HAL_Delay(200);
Int_MPU6050_WriteByte(MPU_PWR_MGMT1_REG, 0x00);
// 配置MPU6050
// 2.1 配置陀螺仪量程
Int_MPU6050_WriteByte(MPU_GYRO_CFG_REG, 3 << 3);
// 2.2 配置加速度计量程
Int_MPU6050_WriteByte(MPU_ACCEL_CFG_REG, 0);
// 2.3 配置采样频率
Int_MPU6050_WriteByte(MPU_SAMPLE_RATE_REG, 0x01);
// 2.4 设置低通滤波器
Int_MPU6050_WriteByte(MPU_CFG_REG, 2);
// 2.5 关闭中断 关闭i2c扩展 关闭FIFO
Int_MPU6050_WriteByte(MPU_INT_EN_REG, 0);
Int_MPU6050_WriteByte(MPU_USER_CTRL_REG, 0x00);
// 2.6 测试读取地址寄存器
uint8_t test_addr;
Int_MPU6050_ReadByte(MPU_DEVICE_ID_REG, &test_addr);
//debug_printf("MPU6050_ID: %d\n", test_addr);
// 2.7 配置时钟
Int_MPU6050_WriteByte(MPU_PWR_MGMT1_REG, 0x01);
// 2.8 启动传感器
Int_MPU6050_WriteByte(MPU_PWR_MGMT2_REG, 0x00);
Int_MPU6050_Calculate();
}
void Int_MPU6050_Read_Gyro(Gyro_Struct *gyro)
{
uint8_t data[6] = {0};
Int_MPU6050_ReadBytes(MPU_GYRO_XOUTH_REG, data, 6);
gyro->x = (data[0] << 8 | data[1]);
gyro->y = (data[2] << 8 | data[3]);
gyro->z = (data[4] << 8 | data[5]);
}
void Int_MPU6050_Read_Acc(Acc_Struct *acc)
{
uint8_t data[6] = {0};
Int_MPU6050_ReadBytes(MPU_ACCEL_XOUTH_REG, data, 6);
acc->x = (data[0] << 8 | data[1]);
acc->y = (data[2] << 8 | data[3]);
acc->z = (data[4] << 8 | data[5]);
}
void Int_MPU6050_Read(Gyro_Acc_Struct *gyro_acc)
{
Int_MPU6050_Read_Gyro(&gyro_acc->gyro);
Int_MPU6050_Read_Acc(&gyro_acc->acc);
// 使用校准值
gyro_acc->gyro.x -= gyro_acc_bias.gyro.x;
gyro_acc->gyro.y -= gyro_acc_bias.gyro.y;
gyro_acc->gyro.z -= gyro_acc_bias.gyro.z;
gyro_acc->acc.x -= gyro_acc_bias.acc.x;
gyro_acc->acc.y -= gyro_acc_bias.acc.y;
gyro_acc->acc.z -= gyro_acc_bias.acc.z;
}
void Int_MPU6050_Calculate(void)
{
// 1. 判断当前飞机是否停稳
// 连续100次 角速度的值和上一次的测量没有小的浮空
uint8_t count = 0;
Gyro_Struct last_gyro;
Gyro_Struct cur_gyro;
Int_MPU6050_Read_Gyro(&last_gyro);
HAL_Delay(2);
while (count < 100)
{
Int_MPU6050_Read_Gyro(&cur_gyro);
if (ABS(cur_gyro.x - last_gyro.x) < 100 && ABS(cur_gyro.y - last_gyro.y) < 100 && ABS(cur_gyro.z - last_gyro.z) < 100)
{
count++;
}
else
{
// 如果中间发送不稳定 => 重新校准
count = 0;
}
// c语言结构体 = 结构体 效果是让后面的结构体的属性挨个赋值给前面的结构体
last_gyro = cur_gyro;
}
// 2. 读取当前的值当做偏移量 => 读200次取平均值
Gyro_Acc_Struct gyro_acc_temp;
int32_t gyro_acc_sum[6] = {0};
for (uint8_t i = 0; i < 200; i++)
{
Int_MPU6050_Read(&gyro_acc_temp);
gyro_acc_sum[0] += gyro_acc_temp.gyro.x;
gyro_acc_sum[1] += gyro_acc_temp.gyro.y;
gyro_acc_sum[2] += gyro_acc_temp.gyro.z;
gyro_acc_sum[3] += gyro_acc_temp.acc.x;
gyro_acc_sum[4] += gyro_acc_temp.acc.y;
gyro_acc_sum[5] += gyro_acc_temp.acc.z - 16384;
HAL_Delay(2);
}
gyro_acc_bias.gyro.x = gyro_acc_sum[0] /= 200;
gyro_acc_bias.gyro.y = gyro_acc_sum[1] /= 200;
gyro_acc_bias.gyro.z = gyro_acc_sum[2] /= 200;
gyro_acc_bias.acc.x = gyro_acc_sum[3] /= 200;
gyro_acc_bias.acc.y = gyro_acc_sum[4] /= 200;
gyro_acc_bias.acc.z = gyro_acc_sum[5] /= 200;
}
- SI24R1模块
#include "Int_SI24R1.h"
// 地址 => 接收方和发送方保持一致
uint8_t TX_ADDRESS[TX_ADR_WIDTH] = {0x0A,0x01,0x07,0x0E,0x01}; // 定义一个静态发送地址
static uint8_t SPI_RW(uint8_t byte)
{
uint8_t result = 0;
//进入临界区
taskENTER_CRITICAL();
HAL_SPI_TransmitReceive(&hspi1, &byte, &result, 1, 1000);
//退出临界区
taskEXIT_CRITICAL();
return result;
}
/********************************************************
函数功能：写寄存器的值（单字节）
入口参数：reg:寄存器映射地址（格式：WRITE_REG｜reg）
value:寄存器的值
返回 值：状态寄存器的值
*********************************************************/
uint8_t Int_SI24R1_Write_Byte(uint8_t cmd, uint8_t value)
{
uint8_t status;
SI24R1_CS_L;
status = SPI_RW(cmd);
SPI_RW(value);
SI24R1_CS_H;
return(status);
}
/********************************************************
函数功能：写寄存器的值（多字节）
入口参数：reg:寄存器映射地址（格式：WRITE_REG｜reg）
pBuf:写数据首地址
bytes:写数据字节数
返回 值：状态寄存器的值
*********************************************************/
uint8_t Int_SI24R1_Write_Bytes(uint8_t cmd, const uint8_t *pBuf, uint8_t bytes)
{
uint8_t status,byte_ctr;
SI24R1_CS_L;
status = SPI_RW(cmd);
for(byte_ctr=0; byte_ctr 102ms
vTaskDelay(200);
//自我校验
while(Int_SI24R1_Self_Check() != COM_OK)
{
vTaskDelay(50);
}
debug_printfln("SI24R1 Init OK");
//初始化RX模式
Int_SI24R1_RX_Mode();
debug_printfln("SI24R1 RX Mode OK");
}
实物图
设计图
未生成预览图，请在编辑器重新保存一次BOM
暂无BOM
克隆工程添加到专辑
0
0
分享
0侵权投诉
知识产权声明&复刻说明
本项目为开源硬件项目，其相关的知识产权归创作者所有。创作者在本平台上传该硬件项目仅供平台用户用于学习交流及研究，不包括任何商业性使用，请勿用于商业售卖或其他盈利性的用途；如您认为本项目涉嫌侵犯了您的相关权益，请点击上方“侵权投诉”按钮，我们将按照嘉立创《侵权投诉与申诉规则》进行处理。
请在进行项目复刻时自行验证电路的可行性，并自行辨别该项目是否对您适用。您对复刻项目的任何后果负责，无论何种情况，本平台将不对您在复刻项目时，遇到的任何因开源项目电路设计问题所导致的直接、间接等损害负责。
