MiniFly 魔改共轴双旋翼：STM32F411 + NRF51822 双 MCU 飞控代码移植 5 大要点

MiniFly 魔改共轴双旋翼：STM32F411 + NRF51822 双 MCU 飞控代码移植 5 大要点

共轴双旋翼无人机因其独特的机械结构和飞行特性，在航拍、巡检等专业领域展现出显著优势。与常规四轴飞行器相比，共轴双旋翼通过上下旋翼的反向旋转实现力矩平衡，具有更高的空间利用率和更强的抗风性能。然而，这种结构也带来了飞控系统设计的特殊挑战，尤其是当开发者尝试将成熟的四轴飞控代码移植到共轴架构时，需要解决一系列关键问题。

本文将深入探讨基于 MiniFly 开源飞控的代码移植过程，聚焦 STM32F411 与 NRF51822 双 MCU 架构下的 5 个核心代码修改点。不同于简单的硬件适配，我们将从软件层面剖析姿态解算重构、控制逻辑优化、任务调度调整等关键技术细节，为具备嵌入式开发基础的无人机爱好者提供可直接复用的实战指南。

1. 姿态解算算法重构与传感器数据处理

共轴双旋翼的动力学特性与传统四轴存在本质差异，这要求我们从根本上重新思考姿态解算的实现方式。四轴飞行器通常依赖四个旋翼的差动推力实现姿态控制，而共轴结构则通过倾斜盘机构改变旋翼桨距周期来实现相同功能。

传感器数据预处理 是姿态解算的基础。MPU9250 提供的原始数据需要经过严格校准和滤波：

// 陀螺仪零偏校准示例代码
void GyroCalibration(void) {
    float gx_sum = 0, gy_sum = 0, gz_sum = 0;
    for(int i=0; i<500; i++) {
        MPU9250_ReadGyr(&gx, &gy, &gz);
        gx_sum += gx; gy_sum += gy; gz_sum += gz;
        delay(2);
    }
    gx_offset = gx_sum / 500;
    gy_offset = gy_sum / 500;
    gz_offset = gz_sum / 500;
}

对于共轴双旋翼，我们需要特别注意振动对传感器数据的影响。上下旋翼的高速旋转会产生特定频率的机械振动，这些振动会被加速度计和陀螺仪捕捉，导致数据异常。解决方案包括：

• 采用自适应IIR滤波器，根据飞行状态动态调整截止频率
• 在关键控制周期内进行多次采样取平均
• 设置振动阈值触发数据异常处理机制

四元数解算优化 是姿态估计的核心。共轴双旋翼的快速姿态变化要求更高频率的姿态更新：

// 基于Mahony互补滤波的快速姿态解算
void AttitudeUpdate(float gx, float gy, float gz, float ax, float ay, float az) {
    float recipNorm;
    float halfvx, halfvy, halfvz;
    float halfex, halfey, halfez;
    
    // 加速度计数据归一化
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;
    
    // 估计重力方向
    halfvx = q1q3 - q0q2;
    halfvy = q0q1 + q2q3;
    halfvz = q0q0 - 0.5f + q3q3;
    
    // 计算误差
    halfex = (ay * halfvz - az * halfvy);
    halfey = (az * halfvx - ax * halfvz);
    halfez = (ax * halfvy - ay * halfvx);
    
    // 积分误差补偿
    gyro_bias[0] += K_i * halfex * dt;
    gyro_bias[1] += K_i * halfey * dt;
    gyro_bias[2] += K_i * halfez * dt;
    
    // 补偿陀螺仪数据
    gx += gyro_bias[0] + K_p * halfex;
    gy += gyro_bias[1] + K_p * halfey;
    gz += gyro_bias[2] + K_p * halfez;
    
    // 四元数更新
    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
    
    // 积分四元数
    q0 += qDot1 * dt;
    q1 += qDot2 * dt;
    q2 += qDot3 * dt;
    q3 += qDot4 * dt;
    
    // 四元数归一化
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
}

提示：共轴双旋翼的倾斜盘机构会引入额外的机械延迟，建议将姿态解算频率提升至1kHz以上，并在控制算法中考虑这一延迟因素。

2. 电机与舵机混合控制逻辑实现

共轴双旋翼的动力系统由两个无刷电机和三个舵机（通常两个用于倾斜盘控制，一个用于尾舵）组成，这与四轴飞行器的纯电机控制有显著不同。我们需要重构整个输出控制模块。

PWM输出配置 需要重新设计。STM32F411 的定时器配置示例如下：

// TIM1 配置为电机PWM输出 (1kHz)
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;

RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);

TIM_TimeBaseStructure.TIM_Period = 999; // 1kHz PWM
TIM_TimeBaseStructure.TIM_Prescaler = 83; // 84MHz/84 = 1MHz
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);

// PWM模式配置
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = 0;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

TIM_OC1Init(TIM1, &TIM_OCInitStructure); // 上旋翼
TIM_OC2Init(TIM1, &TIM_OCInitStructure); // 下旋翼
TIM_OC3Init(TIM1, &TIM_OCInitStructure); // 备用

TIM_CtrlPWMOutputs(TIM1, ENABLE);
TIM_Cmd(TIM1, ENABLE);

// TIM3 配置为舵机PWM输出 (50Hz)
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);

TIM_TimeBaseStructure.TIM_Period = 19999; // 50Hz PWM
TIM_TimeBaseStructure.TIM_Prescaler = 83; // 84MHz/84 = 1MHz
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);

TIM_OCInitStructure.TIM_Pulse = 1500; // 中立位置

TIM_OC1Init(TIM3, &TIM_OCInitStructure); // 横滚舵机
TIM_OC2Init(TIM3, &TIM_OCInitStructure); // 俯仰舵机
TIM_OC3Init(TIM3, &TIM_OCInitStructure); // 偏航舵机

TIM_Cmd(TIM3, ENABLE);

混合控制逻辑 是共轴双旋翼的核心挑战。我们需要将传统的四轴控制量转换为电机和舵机的协同输出：

void MixerUpdate(float throttle, float roll, float pitch, float yaw) {
    // 电机基础输出
    motor_up = throttle + yaw * 0.3f;   // 上旋翼
    motor_down = throttle - yaw * 0.3f; // 下旋翼
    
    // 舵机输出转换
    servo_roll = 1500 + roll * 500;    // 横滚舵机 (500us/度)
    servo_pitch = 1500 + pitch * 500;  // 俯仰舵机
    servo_yaw = 1500 + yaw * 200;      // 偏航补偿舵机
    
    // 输出限幅
    motor_up = constrain(motor_up, 1000, 2000);
    motor_down = constrain(motor_down, 1000, 2000);
    servo_roll = constrain(servo_roll, 1000, 2000);
    servo_pitch = constrain(servo_pitch, 1000, 2000);
    servo_yaw = constrain(servo_yaw, 1000, 2000);
    
    // 更新PWM输出
    TIM_SetCompare1(TIM1, motor_up);
    TIM_SetCompare2(TIM1, motor_down);
    TIM_SetCompare1(TIM3, servo_roll);
    TIM_SetCompare2(TIM3, servo_pitch);
    TIM_SetCompare3(TIM3, servo_yaw);
}

注意：共轴双旋翼的舵机响应速度直接影响飞行稳定性，建议选择响应时间小于0.08s的高速舵机，并在软件中加入舵机轨迹平滑处理。

3. 双MCU通信协议优化与任务调度

MiniFly 采用 STM32F411 和 NRF51822 的双 MCU 架构，两者通过 UART 进行通信。在共轴双旋翼应用中，我们需要优化这一通信机制以适应更高的实时性要求。

通信协议优化 需要考虑以下方面：

1. 数据帧结构重新设计，增加舵机控制字段
2. 通信频率从原来的100Hz提升至200Hz
3. 增加通信超时检测和故障恢复机制

改进后的通信帧结构示例：

任务调度调整 是保证系统实时性的关键。共轴双旋翼对控制周期的要求更为严格，我们需要重新规划 FreeRTOS 任务的优先级和调度策略：

// 任务优先级配置示例
#define TASK_PRIORITY_STABILIZER    (tskIDLE_PRIORITY + 4)
#define TASK_PRIORITY_RADIOLINK     (tskIDLE_PRIORITY + 3)
#define TASK_PRIORITY_IMU           (tskIDLE_PRIORITY + 2)
#define TASK_PRIORITY_USBLINK       (tskIDLE_PRIORITY + 1)
#define TASK_PRIORITY_LED           (tskIDLE_PRIORITY + 0)

// 创建高优先级姿态控制任务
xTaskCreate(stabilizerTask, "stabilizer", configMINIMAL_STACK_SIZE, NULL, 
           TASK_PRIORITY_STABILIZER, NULL);

双MCU同步机制 对于系统稳定性至关重要。我们引入硬件握手信号来确保控制时序：

1. NRF51822 在发送控制数据前拉高 GPIO
2. STM32F411 检测到信号后立即处理最新数据
3. 处理完成后 STM32F411 回复确认信号
4. 超时未收到确认则触发安全保护

4. PID控制参数整定与飞行模式实现

共轴双旋翼的PID参数整定比四轴飞行器更为复杂，需要分别调整角度环、角速度环和高度环的参数。由于机械结构的差异，传统四轴的PID参数往往不能直接套用。

参数整定流程 建议按照以下顺序进行：

1. 先调整角速度环（内环），确保机体对摇杆输入的响应快速且无振荡
2. 再调整角度环（外环），保证姿态控制的准确性
3. 最后调整高度环，实现稳定的定高飞行

共轴双旋翼典型PID参数范围参考：

飞行模式实现 需要针对共轴特性进行优化。以下是几种典型模式的切换逻辑：

typedef enum {
    FLIGHT_MODE_MANUAL = 0,   // 纯手动模式
    FLIGHT_MODE_STABILIZE,    // 自稳模式
    FLIGHT_MODE_ALT_HOLD,     // 定高模式
    FLIGHT_MODE_LOITER        // 定点模式
} flight_mode_t;

void FlightModeUpdate(flight_mode_t new_mode) {
    static flight_mode_t current_mode = FLIGHT_MODE_MANUAL;
    
    // 模式切换时的过渡处理
    switch(current_mode) {
        case FLIGHT_MODE_ALT_HOLD:
            // 退出定高时重置积分项
            pid_alt.reset_integral();
            break;
        // 其他模式清理...
    }
    
    current_mode = new_mode;
    
    // 新模式的初始化
    switch(new_mode) {
        case FLIGHT_MODE_STABILIZE:
            // 启用角度控制
            attitude_control_enable(true);
            break;
        case FLIGHT_MODE_ALT_HOLD:
            // 记录当前高度为目标
            target_altitude = get_current_altitude();
            break;
        // 其他模式初始化...
    }
}

自适应PID调节 可以进一步提升飞行性能。根据飞行状态动态调整PID参数：

void AdaptivePIDTuning(float throttle, float attitude_error) {
    // 根据油门大小调整角速度环P值
    if(throttle < 0.3f) {
        pid_rate_roll.kp = BASE_KP * 0.8f; // 低油门时降低灵敏度
    } else {
        pid_rate_roll.kp = BASE_KP * (1.0f + throttle * 0.2f); // 高油门时提高响应
    }
    
    // 根据姿态误差调整角度环D值
    if(fabs(attitude_error) > 15.0f) {
        pid_angle_roll.kd = BASE_KD * 1.5f; // 大误差时增加阻尼
    } else {
        pid_angle_roll.kd = BASE_KD;
    }
}

5. 安全保护机制与故障恢复策略

共轴双旋翼的安全风险高于四轴飞行器，特别是在舵机失效或双旋翼差速过大的情况下。我们需要在软件层面实现多重保护机制。

故障检测系统 应包含以下监测点：

1. 传感器数据有效性检查（范围、变化率、一致性）
2. 舵机反馈监测（有条件时）
3. 电机电流监测
4. 通信链路质量监测
5. 电池电压监测

// 综合故障检测函数
uint8_t FailureDetection(void) {
    static uint32_t last_imu_ok_time = 0;
    uint8_t failure_flags = 0;
    
    // IMU数据异常检测
    if(fabs(accel.x) > 16.0f || fabs(accel.y) > 16.0f || fabs(accel.z) > 16.0f) {
        failure_flags |= IMU_FAILURE;
    } else {
        last_imu_ok_time = millis();
    }
    
    // 通信超时检测
    if(millis() - last_radio_time > 200) {
        failure_flags |= RADIO_FAILURE;
    }
    
    // 电池低压检测
    if(battery_voltage < 10.5f) {
        failure_flags |= LOW_BATTERY;
    }
    
    // 舵机范围检测
    if(servo_roll < 800 || servo_roll > 2200) {
        failure_flags |= SERVO_FAILURE;
    }
    
    return failure_flags;
}

分级保护策略 根据故障严重程度采取不同措施：

应急降落算法 在故障发生时确保安全着陆：

void EmergencyLanding(void) {
    // 逐步降低油门
    target_throttle -= 0.01f;
    target_throttle = fmax(target_throttle, 0.1f);
    
    // 保持水平姿态
    target_roll = 0;
    target_pitch = 0;
    
    // 缓慢减小偏航控制
    target_yaw *= 0.95f;
    
    // 触地检测
    if(baro_altitude < 0.3f && accel.z > 0.5f) {
        MotorsStop();
        set_armed(false);
    }
}

双MCU冗余设计 进一步提升可靠性。当主飞控(STM32F411)失效时，NRF51822可接管基础控制：

1. STM32F411 定期发送心跳信号给NRF51822
2. NRF51822 检测到心跳丢失后启动备份控制
3. 备份控制采用预设的简单PID算法
4. NRF51822 通过GPIO直接控制电机和舵机