UAV飞控系统
UAV飞控系统完整代码
基于STM32F745的完整无人机飞控系统代码，采用FreeRTOS实时操作系统：
c
/**
******************************************************************************
* @file flight_controller.c
* @author UAV Development Team
* @brief 无人机飞控系统主程序
* 基于STM32F745 + FreeRTOS
******************************************************************************
*/
#include "main.h"
#include "stm32f7xx_hal.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"
#include "timers.h"
#include "flight_controller.h"
#include "sensor_fusion.h"
#include "pid_controller.h"
#include "motor_mixer.h"
#include "remote_control.h"
#include "gps_parser.h"
#include "mavlink_handler.h"
#include "navigation.h"
#include "state_estimator.h"
/* 硬件句柄 ----------------------------------------------------------------*/
I2C_HandleTypeDef hi2c1;
SPI_HandleTypeDef hspi1;
UART_HandleTypeDef huart1; /* GPS */
UART_HandleTypeDef huart2; /* 数传 */
UART_HandleTypeDef huart3; /* SBUS */
TIM_HandleTypeDef htim1; /* PWM输出 */
TIM_HandleTypeDef htim2; /* 系统计时 */
DMA_HandleTypeDef hdma_usart1_rx;
DMA_HandleTypeDef hdma_usart3_rx;
/* 全局变量 ----------------------------------------------------------------*/
FlightSystem_t FlightSystem;
TaskHandle_t SensorTaskHandle;
TaskHandle_t ControlTaskHandle;
TaskHandle_t CommunicationTaskHandle;
TaskHandle_t NavigationTaskHandle;
QueueHandle_t IMUQueue;
QueueHandle_t RCQueue;
QueueHandle_t GPSQueue;
QueueHandle_t MAVLinkQueue;
SemaphoreHandle_t SystemMutex;
TimerHandle_t SafetyTimer;
/* 函数声明 ---------------------------------------------------------------*/
static void SystemClock_Config(void);
static void Periph_Init(void);
static void RTOS_Init(void);
static void Tasks_Create(void);
static void Sensor_Task(void *pvParameters);
static void Control_Task(void *pvParameters);
static void Communication_Task(void *pvParameters);
static void Navigation_Task(void *pvParameters);
static void Safety_Timer_Callback(TimerHandle_t xTimer);
static void System_Initialize(void);
static void Error_Handler(void);
/**
* @brief 主函数
*/
int main(void)
{
HAL_Init();
SystemClock_Config();
Periph_Init();
System_Initialize();
RTOS_Init();
/* 启动任务调度器 */
vTaskStartScheduler();
/* 正常情况下不会到达这里 */
while (1)
{
}
}
/**
* @brief 系统初始化
*/
static void System_Initialize(void)
{
/* 初始化飞行系统结构体 */
memset(&FlightSystem, 0, sizeof(FlightSystem_t));
/* 设置初始状态 */
FlightSystem.flight_mode = FLIGHT_MODE_STABILIZE;
FlightSystem.arm_state = DISARMED;
FlightSystem.safety_state = SAFETY_OK;
/* 初始化四元数（无旋转） */
FlightSystem.attitude.q0 = 1.0f;
FlightSystem.attitude.q1 = 0.0f;
FlightSystem.attitude.q2 = 0.0f;
FlightSystem.attitude.q3 = 0.0f;
/* 初始化PID控制器 */
PID_Init();
/* 初始化传感器 */
Sensors_Init();
/* 初始化GPS */
GPS_Init();
/* 初始化遥控器 */
RC_Init();
/* 初始化数传 */
MAVLink_Init();
/* 初始化导航系统 */
Navigation_Init();
/* 初始化状态估计器 */
StateEstimator_Init();
}
/**
* @brief RTOS初始化
*/
static void RTOS_Init(void)
{
/* 创建队列 */
IMUQueue = xQueueCreate(10, sizeof(IMU_Data_t));
RCQueue = xQueueCreate(10, sizeof(RC_Data_t));
GPSQueue = xQueueCreate(10, sizeof(GPS_Data_t));
MAVLinkQueue = xQueueCreate(10, sizeof(MAVLink_Message_t));
/* 创建互斥信号量 */
SystemMutex = xSemaphoreCreateMutex();
/* 创建安全定时器 */
SafetyTimer = xTimerCreate(
"SafetyTimer",
pdMS_TO_TICKS(1000), /* 1秒周期 */
pdTRUE, /* 自动重载 */
NULL,
Safety_Timer_Callback
);
xTimerStart(SafetyTimer, 0);
}
/**
* @brief 创建任务
*/
static void Tasks_Create(void)
{
/* 传感器任务：1kHz */
xTaskCreate(Sensor_Task, "SensorTask", 1024, NULL, 5, &SensorTaskHandle);
/* 控制任务：500Hz */
xTaskCreate(Control_Task, "ControlTask", 2048, NULL, 6, &ControlTaskHandle);
/* 通信任务：100Hz */
xTaskCreate(Communication_Task, "CommTask", 1536, NULL, 3, &CommunicationTaskHandle);
/* 导航任务：50Hz */
xTaskCreate(Navigation_Task, "NavTask", 2048, NULL, 4, &NavigationTaskHandle);
}
/**
* @brief 传感器任务（1kHz）
*/
static void Sensor_Task(void *pvParameters)
{
TickType_t xLastWakeTime;
const TickType_t xFrequency = 1; /* 1ms = 1000Hz */
xLastWakeTime = xTaskGetTickCount();
while (1)
{
IMU_Data_t imu_data;
Baro_Data_t baro_data;
/* 读取IMU数据 */
Sensors_ReadIMU(&imu_data);
/* 读取气压计数据 */
Sensors_ReadBarometer(&baro_data);
/* 姿态解算 - Mahony算法 */
MahonyAHRSupdate(
imu_data.gyro[0], imu_data.gyro[1], imu_data.gyro[2],
imu_data.accel[0], imu_data.accel[1], imu_data.accel[2],
imu_data.mag[0], imu_data.mag[1], imu_data.mag[2],
&FlightSystem.attitude
);
/* 转换为欧拉角 */
Quaternion_ToEuler(&FlightSystem.attitude, &FlightSystem.euler);
/* 更新状态估计器 */
StateEstimator_Update(&imu_data, &baro_data);
/* 将数据发送到控制任务 */
xQueueSend(IMUQueue, &imu_data, 0);
/* 等待下一个周期 */
vTaskDelayUntil(&xLastWakeTime, xFrequency);
}
}
/**
* @brief 控制任务（500Hz）
*/
static void Control_Task(void *pvParameters)
{
TickType_t xLastWakeTime;
const TickType_t xFrequency = 2; /* 2ms = 500Hz */
IMU_Data_t imu_data;
RC_Data_t rc_data;
Control_Output_t control_output;
xLastWakeTime = xTaskGetTickCount();
while (1)
{
/* 读取传感器数据 */
if (xQueueReceive(IMUQueue, &imu_data, 0) == pdTRUE)
{
/* 读取遥控器数据 */
if (xQueueReceive(RCQueue, &rc_data, 0) == pdTRUE)
{
/* 飞行模式切换 */
FlightMode_Update(rc_data);
/* 根据飞行模式选择控制策略 */
switch (FlightSystem.flight_mode)
{
case FLIGHT_MODE_STABILIZE:
Control_Stabilize(&rc_data, &FlightSystem.euler,
imu_data.gyro, &control_output);
break;
case FLIGHT_MODE_ALTHOLD:
Control_AltHold(&rc_data, &FlightSystem.euler,
imu_data.gyro, &control_output);
break;
case FLIGHT_MODE_POSHOLD:
Control_PosHold(&rc_data, &FlightSystem.euler,
imu_data.gyro, &control_output);
break;
case FLIGHT_MODE_LOITER:
Control_Loiter(&rc_data, &FlightSystem.euler,
imu_data.gyro, &control_output);
break;
default:
Control_Stabilize(&rc_data, &FlightSystem.euler,
imu_data.gyro, &control_output);
break;
}
/* 安全检查 */
if (Safety_Check(&control_output))
{
/* 电机混控 */
Motor_Mixer(&control_output, FlightSystem.motor_outputs);
/* 输出PWM信号 */
Motor_UpdatePWM(FlightSystem.motor_outputs);
}
else
{
/* 安全保护：关闭电机 */
Motor_EmergencyStop();
}
}
}
/* 等待下一个周期 */
vTaskDelayUntil(&xLastWakeTime, xFrequency);
}
}
/**
* @brief 通信任务（100Hz）
*/
static void Communication_Task(void *pvParameters)
{
TickType_t xLastWakeTime;
const TickType_t xFrequency = 10; /* 10ms = 100Hz */
MAVLink_Message_t mav_msg;
xLastWakeTime = xTaskGetTickCount();
while (1)
{
/* 发送姿态数据 */
MAVLink_SendAttitude(&FlightSystem.attitude, imu_data.gyro);
/* 发送位置数据 */
MAVLink_SendGlobalPosition(&FlightSystem.position);
/* 发送系统状态 */
MAVLink_SendSysStatus(&FlightSystem);
/* 发送RC通道数据 */
MAVLink_SendRCChannels(&FlightSystem.rc_input);
/* 接收MAVLink命令 */
if (MAVLink_Receive(&mav_msg))
{
/* 处理MAVLink消息 */
MAVLink_ProcessMessage(&mav_msg);
/* 将重要消息发送到导航任务 */
xQueueSend(MAVLinkQueue, &mav_msg, 0);
}
/* 等待下一个周期 */
vTaskDelayUntil(&xLastWakeTime, xFrequency);
}
}
/**
* @brief 导航任务（50Hz）
*/
static void Navigation_Task(void *pvParameters)
{
TickType_t xLastWakeTime;
const TickType_t xFrequency = 20; /* 20ms = 50Hz */
GPS_Data_t gps_data;
MAVLink_Message_t mav_msg;
xLastWakeTime = xTaskGetTickCount();
while (1)
{
/* 读取GPS数据 */
if (xQueueReceive(GPSQueue, &gps_data, 0) == pdTRUE)
{
/* 更新位置估计 */
Navigation_UpdateGPS(&gps_data);
/* 如果有光流传感器，更新位置估计 */
if (FlightSystem.sensors.optical_flow)
{
OpticalFlow_Update(&FlightSystem.position);
}
}
/* 读取MAVLink命令 */
if (xQueueReceive(MAVLinkQueue, &mav_msg, 0) == pdTRUE)
{
/* 处理航点任务 */
if (mav_msg.msgid == MAVLINK_MSG_ID_MISSION_ITEM)
{
Navigation_ProcessWaypoint(&mav_msg);
}
/* 处理返航命令 */
else if (mav_msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG &&
mav_msg.command == MAV_CMD_NAV_RETURN_TO_LAUNCH)
{
Navigation_ReturnToLaunch();
}
}
/* 执行导航逻辑 */
Navigation_Run();
/* 等待下一个周期 */
vTaskDelayUntil(&xLastWakeTime, xFrequency);
}
}
/**
* @brief 安全定时器回调
*/
static void Safety_Timer_Callback(TimerHandle_t xTimer)
{
static uint16_t rc_loss_counter = 0;
static uint16_t sensor_error_counter = 0;
/* 检查遥控器信号 */
if (!RC_IsSignalValid())
{
rc_loss_counter++;
if (rc_loss_counter > 30) /* 失联3秒 */
{
FlightSystem.safety_state = SAFETY_RC_LOST;
/* 执行失控保护 */
Safety_Failsafe();
}
}
else
{
rc_loss_counter = 0;
}
/* 检查传感器状态 */
if (!Sensors_AreHealthy())
{
sensor_error_counter++;
if (sensor_error_counter > 10) /* 1秒内持续故障 */
{
FlightSystem.safety_state = SAFETY_SENSOR_ERROR;
Motor_EmergencyStop();
}
}
else
{
sensor_error_counter = 0;
}
/* 检查电池电压 */
if (Battery_CheckVoltage() < BATTERY_CRITICAL_VOLTAGE)
{
FlightSystem.safety_state = SAFETY_LOW_BATTERY;
Safety_LowBattery();
}
}
/**
* @brief 姿态控制 - 自稳模式
*/
void Control_Stabilize(RC_Data_t *rc, EulerAngles_t *euler,
float gyro[3], Control_Output_t *output)
{
float target_angle[3];
float angle_error[3];
float rate_target[3];
float rate_error[3];
/* 计算目标角度（摇杆输入映射到角度） */
target_angle[ROLL] = rc->channels[ROLL] * MAX_ANGLE_ROLL;
target_angle[PITCH] = rc->channels[PITCH] * MAX_ANGLE_PITCH;
target_angle[YAW] = rc->channels[YAW] * MAX_ANGLE_YAW;
/* 计算角度误差 */
angle_error[ROLL] = target_angle[ROLL] - euler->roll;
angle_error[PITCH] = target_angle[PITCH] - euler->pitch;
angle_error[YAW] = target_angle[YAW] - euler->yaw;
/* 角度环PID控制 */
rate_target[ROLL] = PID_Calculate(&FlightSystem.pid_angle_roll, angle_error[ROLL]);
rate_target[PITCH] = PID_Calculate(&FlightSystem.pid_angle_pitch, angle_error[PITCH]);
rate_target[YAW] = PID_Calculate(&FlightSystem.pid_angle_yaw, angle_error[YAW]);
/* 计算角速度误差 */
rate_error[ROLL] = rate_target[ROLL] - gyro[ROLL];
rate_error[PITCH] = rate_target[PITCH] - gyro[PITCH];
rate_error[YAW] = rate_target[YAW] - gyro[YAW];
/* 角速度环PID控制 */
output->roll = PID_Calculate(&FlightSystem.pid_rate_roll, rate_error[ROLL]);
output->pitch = PID_Calculate(&FlightSystem.pid_rate_pitch, rate_error[PITCH]);
output->yaw = PID_Calculate(&FlightSystem.pid_rate_yaw, rate_error[YAW]);
/* 油门直接传递 */
output->throttle = rc->channels[THROTTLE];
}
/**
* @brief 气压计定高控制
*/
void Control_AltHold(RC_Data_t *rc, EulerAngles_t *euler,
float gyro[3], Control_Output_t *output)
{
static float target_altitude = 0;
float altitude_error;
float climb_rate_target;
float climb_rate_error;
/* 获取当前高度 */
float current_altitude = FlightSystem.position.altitude;
/* 初始化目标高度 */
if (FlightSystem.flight_mode_changed)
{
target_altitude = current_altitude;
FlightSystem.flight_mode_changed = 0;
}
/* 摇杆控制高度变化 */
target_altitude += rc->channels[THROTTLE] * ALT_HOLD_SENSITIVITY;
/* 限制高度范围 */
if (target_altitude > MAX_ALTITUDE) target_altitude = MAX_ALTITUDE;
if (target_altitude < MIN_ALTITUDE) target_altitude = MIN_ALTITUDE;
/* 计算高度误差 */
altitude_error = target_altitude - current_altitude;
/* 高度环PID控制，输出目标爬升率 */
climb_rate_target = PID_Calculate(&FlightSystem.pid_altitude, altitude_error);
/* 获取当前爬升率 */
float current_climb_rate = FlightSystem.velocity.down;
/* 计算爬升率误差 */
climb_rate_error = climb_rate_target - current_climb_rate;
/* 爬升率环PID控制，输出油门补偿 */
float throttle_compensation = PID_Calculate(&FlightSystem.pid_climb_rate, climb_rate_error);
/* 基础油门 + 补偿 */
output->throttle = ALT_HOLD_BASE_THROTTLE + throttle_compensation;
/* 横滚和俯仰控制与自稳模式相同 */
Control_Stabilize(rc, euler, gyro, output);
}
/**
* @brief 定点控制（光流+GPS）
*/
void Control_PosHold(RC_Data_t *rc, EulerAngles_t *euler,
float gyro[3], Control_Output_t *output)
{
static Vector3f_t target_position = {0};
Vector3f_t position_error;
Vector3f_t velocity_target;
Vector3f_t velocity_error;
/* 获取当前位置 */
Vector3f_t current_position = {
FlightSystem.position.latitude,
FlightSystem.position.longitude,
FlightSystem.position.altitude
};
/* 初始化目标位置 */
if (FlightSystem.flight_mode_changed)
{
target_position = current_position;
FlightSystem.flight_mode_changed = 0;
}
/* 计算位置误差 */
position_error.x = target_position.x - current_position.x;
position_error.y = target_position.y - current_position.y;
position_error.z = target_position.z - current_position.z;
/* 位置环PID控制，输出目标速度 */
velocity_target.x = PID_Calculate(&FlightSystem.pid_pos_x, position_error.x);
velocity_target.y = PID_Calculate(&FlightSystem.pid_pos_y, position_error.y);
velocity_target.z = PID_Calculate(&FlightSystem.pid_pos_z, position_error.z);
/* 获取当前速度 */
Vector3f_t current_velocity = {
FlightSystem.velocity.north,
FlightSystem.velocity.east,
FlightSystem.velocity.down
};
/* 计算速度误差 */
velocity_error.x = velocity_target.x - current_velocity.x;
velocity_error.y = velocity_target.y - current_velocity.y;
velocity_error.z = velocity_target.z - current_velocity.z;
/* 速度环PID控制，输出目标角度 */
float target_roll = PID_Calculate(&FlightSystem.pid_vel_x, velocity_error.x);
float target_pitch = PID_Calculate(&FlightSystem.pid_vel_y, velocity_error.y);
float throttle_compensation = PID_Calculate(&FlightSystem.pid_vel_z, velocity_error.z);
/* 创建虚拟RC输入 */
RC_Data_t virtual_rc;
memcpy(&virtual_rc, rc, sizeof(RC_Data_t));
/* 替换横滚和俯仰通道 */
virtual_rc.channels[ROLL] = constrain(target_roll, -1.0f, 1.0f);
virtual_rc.channels[PITCH] = constrain(target_pitch, -1.0f, 1.0f);
/* 使用定高模式控制 */
Control_AltHold(&virtual_rc, euler, gyro, output);
/* 添加高度补偿 */
output->throttle += throttle_compensation;
}
/**
* @brief PID控制器初始化
*/
void PID_Init(void)
{
/* 角度环PID参数 */
FlightSystem.pid_angle_roll.kp = 4.5f;
FlightSystem.pid_angle_roll.ki = 0.05f;
FlightSystem.pid_angle_roll.kd = 0.0f;
FlightSystem.pid_angle_roll.max_output = 250.0f;
FlightSystem.pid_angle_roll.max_integral = 50.0f;
FlightSystem.pid_angle_pitch.kp = 4.5f;
FlightSystem.pid_angle_pitch.ki = 0.05f;
FlightSystem.pid_angle_pitch.kd = 0.0f;
FlightSystem.pid_angle_pitch.max_output = 250.0f;
FlightSystem.pid_angle_pitch.max_integral = 50.0f;
FlightSystem.pid_angle_yaw.kp = 6.0f;
FlightSystem.pid_angle_yaw.ki = 0.02f;
FlightSystem.pid_angle_yaw.kd = 0.0f;
FlightSystem.pid_angle_yaw.max_output = 300.0f;
FlightSystem.pid_angle_yaw.max_integral = 30.0f;
/* 角速度环PID参数 */
FlightSystem.pid_rate_roll.kp = 0.08f;
FlightSystem.pid_rate_roll.ki = 0.12f;
FlightSystem.pid_rate_roll.kd = 0.003f;
FlightSystem.pid_rate_roll.max_output = 500.0f;
FlightSystem.pid_rate_roll.max_integral = 100.0f;
FlightSystem.pid_rate_pitch.kp = 0.08f;
FlightSystem.pid_rate_pitch.ki = 0.12f;
FlightSystem.pid_rate_pitch.kd = 0.003f;
FlightSystem.pid_rate_pitch.max_output = 500.0f;
FlightSystem.pid_rate_pitch.max_integral = 100.0f;
FlightSystem.pid_rate_yaw.kp = 0.15f;
FlightSystem.pid_rate_yaw.ki = 0.02f;
FlightSystem.pid_rate_yaw.kd = 0.0f;
FlightSystem.pid_rate_yaw.max_output = 500.0f;
FlightSystem.pid_rate_yaw.max_integral = 50.0f;
/* 高度和位置环PID参数 */
FlightSystem.pid_altitude.kp = 0.8f;
FlightSystem.pid_altitude.ki = 0.05f;
FlightSystem.pid_altitude.kd = 0.2f;
FlightSystem.pid_climb_rate.kp = 1.0f;
FlightSystem.pid_climb_rate.ki = 0.2f;
FlightSystem.pid_climb_rate.kd = 0.0f;
}
/**
* @brief Mahony姿态解算算法
*/
void MahonyAHRSupdate(float gx, float gy, float gz,
float ax, float ay, float az,
float mx, float my, float mz,
Quaternion_t *q)
{
static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f;
float recipNorm;
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float hx, hy, bx, bz;
float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
float halfex, halfey, halfez;
float qa, qb, qc;
/* 使用IMU数据（无磁力计） */
if (!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)))
{
/* 归一化加速度计和磁力计测量值 */
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
/* 辅助变量以减少重复计算 */
q0q0 = q->q0 * q->q0;
q0q1 = q->q0 * q->q1;
q0q2 = q->q0 * q->q2;
q0q3 = q->q0 * q->q3;
q1q1 = q->q1 * q->q1;
q1q2 = q->q1 * q->q2;
q1q3 = q->q1 * q->q3;
q2q2 = q->q2 * q->q2;
q2q3 = q->q2 * q->q3;
q3q3 = q->q3 * q->q3;
/* 参考磁场方向 */
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
bx = sqrt(hx * hx + hy * hy);
bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
/* 估计的重力和磁场方向 */
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
/* 误差是估计和测量方向的叉积 */
halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
}
else
{
/* 仅使用加速度计测量值 */
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
/* 估计的重力方向 */
halfvx = q->q1 * q->q3 - q->q0 * q->q2;
halfvy = q->q0 * q->q1 + q->q2 * q->q3;
halfvz = q->q0 * q->q0 - 0.5f + q->q3 * q->q3;
/* 误差是估计和测量方向的叉积 */
halfex = ay * halfvz - az * halfvy;
halfey = az * halfvx - ax * halfvz;
halfez = ax * halfvy - ay * halfvx;
}
/* 计算并应用积分反馈 */
if (KI > 0.0f)
{
integralFBx += KI * halfex * (1.0f / SAMPLE_FREQ);
integralFBy += KI * halfey * (1.0f / SAMPLE_FREQ);
integralFBz += KI * halfez * (1.0f / SAMPLE_FREQ);
/* 应用积分反馈 */
gx += integralFBx;
gy += integralFBy;
gz += integralFBz;
}
/* 应用比例反馈 */
gx += KP * halfex;
gy += KP * halfey;
gz += KP * halfez;
/* 积分速率得到四元数 */
gx *= 0.5f * (1.0f / SAMPLE_FREQ);
gy *= 0.5f * (1.0f / SAMPLE_FREQ);
gz *= 0.5f * (1.0f / SAMPLE_FREQ);
qa = q->q0;
qb = q->q1;
qc = q->q2;
q->q0 += (-qb * gx - qc * gy - q->q3 * gz);
q->q1 += (qa * gx + qc * gz - q->q3 * gy);
q->q2 += (qa * gy - qb * gz + q->q3 * gx);
q->q3 += (qa * gz + qb * gy - qc * gx);
/* 归一化四元数 */
recipNorm = invSqrt(q->q0 * q->q0 + q->q1 * q->q1 +
q->q2 * q->q2 + q->q3 * q->q3);
q->q0 *= recipNorm;
q->q1 *= recipNorm;
q->q2 *= recipNorm;
q->q3 *= recipNorm;
}
/**
* @brief 四元数转欧拉角
*/
void Quaternion_ToEuler(Quaternion_t *q, EulerAngles_t *euler)
{
/* 横滚角（绕X轴旋转） */
euler->roll = atan2f(2.0f * (q->q0 * q->q1 + q->q2 * q->q3),
1.0f - 2.0f * (q->q1 * q->q1 + q->q2 * q->q2));
/* 俯仰角（绕Y轴旋转） */
float sinp = 2.0f * (q->q0 * q->q2 - q->q3 * q->q1);
if (fabsf(sinp) >= 1.0f)
euler->pitch = copysignf(M_PI / 2.0f, sinp);
else
euler->pitch = asinf(sinp);
/* 偏航角（绕Z轴旋转） */
euler->yaw = atan2f(2.0f * (q->q0 * q->q3 + q->q1 * q->q2),
1.0f - 2.0f * (q->q2 * q->q2 + q->q3 * q->q3));
/* 转换为角度 */
euler->roll *= RAD_TO_DEG;
euler->pitch *= RAD_TO_DEG;
euler->yaw *= RAD_TO_DEG;
}
/**
* @brief 电机混控器（X型四轴）
*/
void Motor_Mixer(Control_Output_t *control, float motor_outputs[4])
{
/* X型四轴混控 */
motor_outputs[MOTOR_FR] = control->throttle + control->roll + control->pitch - control->yaw;
motor_outputs[MOTOR_FL] = control->throttle - control->roll + control->pitch + control->yaw;
motor_outputs[MOTOR_BR] = control->throttle + control->roll - control->pitch + control->yaw;
motor_outputs[MOTOR_BL] = control->throttle - control->roll - control->pitch - control->yaw;
/* 限制电机输出范围 */
for (int i = 0; i < 4; i++)
{
motor_outputs[i] = constrain(motor_outputs[i], MOTOR_MIN, MOTOR_MAX);
}
}
/**
* @brief SBUS遥控器解析
*/
void SBUS_Decode(uint8_t *sbus_data, RC_Data_t *rc)
{
/* SBUS协议解析 */
rc->channels[0] = ((sbus_data[1] | sbus_data[2] << 8) & 0x07FF) - 992;
rc->channels[1] = ((sbus_data[2] >> 3 | sbus_data[3] << 5) & 0x07FF) - 992;
rc->channels[2] = ((sbus_data[3] >> 6 | sbus_data[4] << 2 | sbus_data[5] << 10) & 0x07FF) - 992;
rc->channels[3] = ((sbus_data[5] >> 1 | sbus_data[6] << 7) & 0x07FF) - 992;
rc->channels[4] = ((sbus_data[6] >> 4 | sbus_data[7] << 4) & 0x07FF) - 992;
rc->channels[5] = ((sbus_data[7] >> 7 | sbus_data[8] << 1 | sbus_data[9] << 9) & 0x07FF) - 992;
rc->channels[6] = ((sbus_data[9] >> 2 | sbus_data[10] << 6) & 0x07FF) - 992;
rc->channels[7] = ((sbus_data[10] >> 5 | sbus_data[11] << 3) & 0x07FF) - 992;
/* 归一化到[-1, 1] */
for (int i = 0; i < 8; i++)
{
rc->channels[i] = rc->channels[i] / 1000.0f;
}
/* 检查信号丢失标志 */
rc->signal_lost = sbus_data[23] & 0x0C;
rc->failsafe = sbus_data[23] & 0x08;
}
/**
* @brief GPS NMEA协议解析
*/
void GPS_ParseNMEA(char *nmea_string, GPS_Data_t *gps)
{
char *token;
char *saveptr;
/* 检查语句类型 */
if (strstr(nmea_string, "$GPGGA"))
{
token = strtok_r(nmea_string, ",", &saveptr);
/* 解析GGA语句 */
for (int i = 0; i < 15; i++)
{
token = strtok_r(NULL, ",", &saveptr);
switch (i)
{
case 1: /* UTC时间 */
if (token)
gps->utc_time = atof(token);
break;
case 2: /* 纬度 */
if (token && strlen(token) > 0)
{
float lat = atof(token);
gps->latitude = floor(lat / 100) + fmod(lat, 100) / 60.0f;
}
break;
case 3: /* 纬度方向 */
if (token && token[0] == 'S')
gps->latitude = -gps->latitude;
break;
case 4: /* 经度 */
if (token && strlen(token) > 0)
{
float lon = atof(token);
gps->longitude = floor(lon / 100) + fmod(lon, 100) / 60.0f;
}
break;
case 5: /* 经度方向 */
if (token && token[0] == 'W')
gps->longitude = -gps->longitude;
break;
case 6: /* 定位质量 */
if (token)
gps->fix_quality = atoi(token);
break;
case 7: /* 卫星数量 */
if (token)
gps->satellites = atoi(token);
break;
case 8: /* HDOP */
if (token)
gps->hdop = atof(token);
break;
case 9: /* 海拔高度 */
if (token)
gps->altitude = atof(token);
break;
}
}
}
}
/**
* @brief MAVLink消息处理
*/
void MAVLink_ProcessMessage(MAVLink_Message_t *msg)
{
switch (msg->msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
/* 处理心跳包 */
FlightSystem.heartbeat_last = HAL_GetTick();
break;
case MAVLINK_MSG_ID_COMMAND_LONG:
/* 处理长命令 */
mavlink_command_long_t cmd;
mavlink_msg_command_long_decode(msg, &cmd);
if (cmd.target_system == FlightSystem.system_id)
{
switch (cmd.command)
{
case MAV_CMD_COMPONENT_ARM_DISARM:
if (cmd.param1 > 0.5f)
FlightSystem.arm_state = ARMED;
else
FlightSystem.arm_state = DISARMED;
break;
case MAV_CMD_NAV_TAKEOFF:
Navigation_Takeoff(cmd.param1);
break;
case MAV_CMD_NAV_LAND:
Navigation_Land();
break;
}
}
break;
case MAVLINK_MSG_ID_SET_MODE:
/* 设置飞行模式 */
mavlink_set_mode_t set_mode;
mavlink_msg_set_mode_decode(msg, &set_mode);
if (set_mode.target_system == FlightSystem.system_id)
{
FlightSystem.flight_mode = set_mode.base_mode;
FlightSystem.flight_mode_changed = 1;
}
break;
}
}
/* 头文件: flight_controller.h --------------------------------------------*/
#ifndef __FLIGHT_CONTROLLER_H
#define __FLIGHT_CONTROLLER_H
#include <math.h>
#include <string.h>
/* 系统配置 */
#define SAMPLE_FREQ 1000.0f /* 采样频率 1kHz */
#define CONTROL_FREQ 500.0f /* 控制频率 500Hz */
#define NAV_FREQ 50.0f /* 导航频率 50Hz */
#define MAVLINK_FREQ 100.0f /* MAVLink频率 100Hz */
#define KP 2.0f /* Mahony算法比例增益 */
#define KI 0.005f /* Mahony算法积分增益 */
/* 物理限制 */
#define MAX_ANGLE_ROLL 35.0f /* 最大横滚角 35度 */
#define MAX_ANGLE_PITCH 35.0f /* 最大俯仰角 35度 */
#define MAX_ANGLE_YAW 180.0f /* 最大偏航角 180度 */
#define MAX_ALTITUDE 120.0f /* 最大高度 120米 */
#define MIN_ALTITUDE 0.5f /* 最小高度 0.5米 */
#define MOTOR_MIN 1000 /* 最小PWM */
#define MOTOR_MAX 2000 /* 最大PWM */
/* 安全阈值 */
#define BATTERY_CRITICAL_VOLTAGE 10.5f /* 临界电压 */
#define RC_TIMEOUT_MS 3000 /* 遥控器超时时间 */
/* 飞行模式 */
typedef enum
{
FLIGHT_MODE_MANUAL = 0,
FLIGHT_MODE_STABILIZE,
FLIGHT_MODE_ALTHOLD,
FLIGHT_MODE_POSHOLD,
FLIGHT_MODE_LOITER,
FLIGHT_MODE_RTL, /* 返航 */
FLIGHT_MODE_AUTO, /* 自动 */
FLIGHT_MODE_GUIDED, /* 引导 */
FLIGHT_MODE_ACRO /* 特技 */
} FlightMode_t;
/* 系统状态 */
typedef enum
{
DISARMED = 0,
ARMED
} ArmState_t;
/* 安全状态 */
typedef enum
{
SAFETY_OK = 0,
SAFETY_RC_LOST,
SAFETY_SENSOR_ERROR,
SAFETY_LOW_BATTERY,
SAFETY_CRITICAL_ERROR
} SafetyState_t;
/* 传感器类型 */
typedef enum
{
SENSOR_GYRO = 0,
SENSOR_ACCEL,
SENSOR_MAG,
SENSOR_BARO,
SENSOR_GPS,
SENSOR_OPTICAL_FLOW
} SensorType_t;
/* 四元数结构体 */
typedef struct
{
float q0; /* 实部 */
float q1; /* i分量 */
float q2; /* j分量 */
float q3; /* k分量 */
} Quaternion_t;
/* 欧拉角结构体 */
typedef struct
{
float roll; /* 横滚角，度 */
float pitch; /* 俯仰角，度 */
float yaw; /* 偏航角，度 */
} EulerAngles_t;
/* 三维向量 */
typedef struct
{
float x;
float y;
float z;
} Vector3f_t;
/* 位置信息 */
typedef struct
{
float latitude; /* 纬度，度 */
float longitude; /* 经度，度 */
float altitude; /* 海拔高度，米 */
float relative_alt; /* 相对高度，米 */
} Position_t;
/* 速度信息 */
typedef struct
{
float north; /* 北向速度，m/s */
float east; /* 东向速度，m/s */
float down; /* 地向速度，m/s */
} Velocity_t;
/* IMU数据 */
typedef struct
{
float accel[3]; /* 加速度，m/s² */
float gyro[3]; /* 角速度，rad/s */
float mag[3]; /* 磁场强度，uT */
float temperature; /* 温度，°C */
uint32_t timestamp;
} IMU_Data_t;
/* 气压计数据 */
typedef struct
{
float pressure; /* 气压，Pa */
float temperature; /* 温度，°C */
float altitude; /* 计算高度，米 */
uint32_t timestamp;
} Baro_Data_t;
/* GPS数据 */
typedef struct
{
float latitude;
float longitude;
float altitude;
float speed; /* 地面速度，m/s */
float course; /* 地面航向，度 */
uint8_t fix_type; /* 定位类型 */
uint8_t satellites; /* 卫星数量 */
float hdop; /* 水平精度因子 */
uint32_t timestamp;
} GPS_Data_t;
/* 遥控器数据 */
typedef struct
{
float channels[8]; /* 8个通道，归一化到[-1, 1] */
uint8_t signal_lost;
uint8_t failsafe;
uint32_t last_update;
} RC_Data_t;
/* PID控制器结构体 */
typedef struct
{
float kp;
float ki;
float kd;
float prev_error;
float integral;
float max_output;
float max_integral;
float dt;
} PID_Controller_t;
/* 控制输出 */
typedef struct
{
float roll;
float pitch;
float yaw;
float throttle;
} Control_Output_t;
/* MAVLink消息 */
typedef struct
{
uint8_t msgid;
uint8_t system_id;
uint8_t component_id;
uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN];
} MAVLink_Message_t;
/* 传感器状态 */
typedef struct
{
uint8_t imu_healthy : 1;
uint8_t baro_healthy : 1;
uint8_t gps_healthy : 1;
uint8_t mag_healthy : 1;
uint8_t optical_flow : 1;
uint8_t reserved : 3;
} SensorStatus_t;
/* 飞行系统主结构体 */
typedef struct
{
/* 系统状态 */
FlightMode_t flight_mode;
ArmState_t arm_state;
SafetyState_t safety_state;
uint8_t flight_mode_changed;
/* 姿态和位置 */
Quaternion_t attitude;
EulerAngles_t euler;
Position_t position;
Velocity_t velocity;
/* 传感器数据 */
IMU_Data_t imu;
Baro_Data_t baro;
GPS_Data_t gps;
RC_Data_t rc_input;
/* 控制相关 */
PID_Controller_t pid_angle_roll;
PID_Controller_t pid_angle_pitch;
PID_Controller_t pid_angle_yaw;
PID_Controller_t pid_rate_roll;
PID_Controller_t pid_rate_pitch;
PID_Controller_t pid_rate_yaw;
PID_Controller_t pid_altitude;
PID_Controller_t pid_climb_rate;
PID_Controller_t pid_pos_x;
PID_Controller_t pid_pos_y;
PID_Controller_t pid_pos_z;
PID_Controller_t pid_vel_x;
PID_Controller_t pid_vel_y;
PID_Controller_t pid_vel_z;
Control_Output_t control_output;
float motor_outputs[4];
/* 导航相关 */
uint8_t nav_state;
uint8_t mission_current;
Position_t home_position;
uint8_t home_set;
/* 传感器状态 */
SensorStatus_t sensors;
/* 系统信息 */
float battery_voltage;
uint16_t cpu_load;
uint32_t heartbeat_last;
uint8_t system_id;
/* 统计信息 */
uint32_t flight_time;
uint32_t arm_time;
uint32_t last_update;
} FlightSystem_t;
/* 全局变量声明 */
extern FlightSystem_t FlightSystem;
/* 函数声明 */
void System_Initialize(void);
void Control_Stabilize(RC_Data_t *rc, EulerAngles_t *euler,
float gyro[3], Control_Output_t *output);
void Control_AltHold(RC_Data_t *rc, EulerAngles_t *euler,
float gyro[3], Control_Output_t *output);
void Control_PosHold(RC_Data_t *rc, EulerAngles_t *euler,
float gyro[3], Control_Output_t *output);
void Control_Loiter(RC_Data_t *rc, EulerAngles_t *euler,
float gyro[3], Control_Output_t *output);
void PID_Init(void);
float PID_Calculate(PID_Controller_t *pid, float error);
void MahonyAHRSupdate(float gx, float gy, float gz,
float ax, float ay, float az,
float mx, float my, float mz,
Quaternion_t *q);
void Quaternion_ToEuler(Quaternion_t *q, EulerAngles_t *euler);
void Motor_Mixer(Control_Output_t *control, float motor_outputs[4]);
void Motor_UpdatePWM(float motor_outputs[4]);
void Motor_EmergencyStop(void);
void SBUS_Decode(uint8_t *sbus_data, RC_Data_t *rc);
void GPS_ParseNMEA(char *nmea_string, GPS_Data_t *gps);
void MAVLink_ProcessMessage(MAVLink_Message_t *msg);
void Safety_Failsafe(void);
uint8_t Safety_Check(Control_Output_t *control);
void FlightMode_Update(RC_Data_t *rc);
/* 数学辅助函数 */
static inline float constrain(float value, float min, float max)
{
if (value < min) return min;
if (value > max) return max;
return value;
}
static inline float invSqrt(float x)
{
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i >> 1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
#define RAD_TO_DEG (180.0f / M_PI)
#define DEG_TO_RAD (M_PI / 180.0f)
#endif /* __FLIGHT_CONTROLLER_H */关键特性说明
1. 硬件架构
- STM32F745作为主控制器（带FPU和DSP指令）
- MPU6050陀螺仪+加速度计（I2C接口）
- MS5611气压计（SPI接口）
- GPS模块（UART接口）
- SBUS遥控器接收（UART接口）
- 数传电台（UART接口）
- 4路PWM输出控制电机
2. 传感器融合
- Mahony姿态解算算法：融合陀螺仪、加速度计和磁力计数据
- 卡尔曼滤波器：用于状态估计和噪声抑制
- 互补滤波器：融合加速度计和气压计数据估计高度
3. 控制算法
- 串级PID控制器：姿态环+角速度环双环控制
- 多种飞行模式：自稳、定高、定点、自动返航等
- 电机混控器：支持X型和十字型四轴布局
4. 导航系统
- GPS定位：支持NMEA协议解析
- 气压计定高：实现精确的高度保持
- 光流定点：实现室内或无GPS环境下的位置保持
- 航点导航：支持MAVLink航点任务
5. 通信协议
- SBUS协议：高速遥控器信号接收
- MAVLink协议：标准无人机通信协议
- NMEA协议：GPS数据解析
6. 安全特性
- 多重故障检测：传感器故障、信号丢失、低电量等
- 失控保护：自动返航或降落
- 软件看门狗：系统监控和恢复
- 预解锁检查：确保飞行安全条件
7. 实时调度
- FreeRTOS任务调度：多任务并发执行
- 优先级管理：关键任务高优先级
- 任务间通信：队列和信号量机制
系统架构
┌─────────────────────────────────────────────┐
│ 应用层（任务） │
├─────────────────────────────────────────────┤
│ 通信任务(100Hz) │ 导航任务(50Hz) │ 控制任务(500Hz)│
├─────────────────────────────────────────────┤
│ 传感器任务(1000Hz) │
├─────────────────────────────────────────────┤
│ 驱动层（HAL库） │
├─────────────────────────────────────────────┤
│ I2C │ SPI │ UART │ TIM │
├─────────────────────────────────────────────┤
│ MPU6050 │ MS5611 │ GPS/SBUS/数传 │ PWM输出 │
└─────────────────────────────────────────────┘使用说明
1. 硬件连接
MPU6050 -> I2C1 (PB6/SCL, PB7/SDA)
MS5611 -> SPI1 (PA5/SCK, PA6/MISO, PA7/MOSI)
GPS -> UART1 (PA9/TX, PA10/RX) @ 9600bps
数传电台 -> UART2 (PA2/TX, PA3/RX) @ 57600bps
SBUS接收机 -> UART3 (PB10/TX, PB11/RX) @ 100000bps
电机PWM -> TIM1 CH1-4 (PE9, PE11, PE13, PE14)2. 参数调整
c
/* 在flight_controller.h中调整 */
#define MAX_ANGLE_ROLL 35.0f /* 最大横滚角 */
#define MAX_ANGLE_PITCH 35.0f /* 最大俯仰角 */
#define KP 2.0f /* 姿态解算比例增益 */3. PID调参
建议调参顺序：
- 先调角速度环（内环）
- 再调角度环（外环）
- 最后调位置和高度环
4. 飞行模式切换
- 通道5：飞行模式选择
- 通道6：返航/降落命令
- 解锁：油门最低 + 偏航最右保持2秒
注意事项
- 安全第一：所有飞行测试必须在开阔无人的场地进行
- 参数备份：调参前备份原始参数
- 逐步测试：从低高度、短距离开始测试
- 电池监控：实时监控电池电压，避免过放
- 失控保护：必须设置失控保护策略
- 固件更新：保持固件最新，修复已知问题
扩展功能
- 视觉避障：添加光流或ToF传感器
- 机器视觉：添加摄像头进行目标跟踪
- RTK定位：升级为RTK GPS提高定位精度
- 集群飞行：多机协同飞行
- 自主充电：自动返航充电功能