开源Flight飞控系列 | Baseflight核心函数解析

西北望msm66g9f 2025-01-16 发布于山东

展开全文

本文是系列的第三部分，详细探讨了多旋翼飞行控制器中两个核心函数 pidMultiWii() 和 mixTable() 的工作原理，以及PWM写入函数 pwmWriteMotor() 的具体实现。pidMultiWii() 函数负责根据传感器数据和遥控器输入计算姿态调整值，以确保飞行器的稳定性和响应性。mixTable() 函数则将这些姿态调整值转换为具体的电机功率输出，并处理舵机控制信号，使飞行器能够按照预期的姿态和运动进行操作。最后，pwmWriteMotor() 函数实现了将计算出的PWM值写入对应的定时器寄存器，从而控制电机的实际转速。

pid_controller()的解析

pid_controller()是一个函数指针，指向pidMultiWii() ，pidMultiWii() 函数是 baseflight 飞行控制器中实现 PID 控制逻辑的核心部分。它负责根据传感器数据（如陀螺仪和加速度计）以及遥控器输入，计算出每个轴（滚转、俯仰、偏航）的姿态调整值 (axisPID)。下面是对该函数的详细解析。

函数结构与变量

static void pidMultiWii(void){ int axis, prop; int32_t error, errorAngle; int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm; static int16_t lastGyro[3] = { 0, 0, 0 }; static int32_t delta1[3], delta2[3]; int32_t deltaSum; int32_t delta;

axis：循环变量，用于遍历三个轴（滚转、俯仰、偏航）。

prop：比例因子，基于遥控输入的最大值来决定混合模式的比例。

error, errorAngle：误差值，分别表示基于角速度和角度的误差。

PTerm, ITerm, DTerm：PID控制中的比例、积分和微分项。

PTermACC, ITermACC, PTermGYRO, ITermGYRO：分别为基于角度和角速度的P和I项。

lastGyro[]：存储上次的陀螺仪读数，用于计算角速度变化。

delta1[], delta2[]：存储前两次的角速度变化量，用于计算 deltaSum。

比例因子计算

prop=max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]

计算滚转和俯仰轴遥控输入的最大绝对值，并将其映射到范围 [0, 500] 内。这个值将用于后续的角度模式和地平线模式下的比例混合。

遍历每个轴

for (axis = 0; axis < 3; axis++) {

对每个轴（滚转、俯仰、偏航）进行处理。

角度模式或地平线模式

if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) {    errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)mcfg.max_angle_inclination), +mcfg.max_angle_inclination) - angle[axis] + cfg.angleTrim[axis];    PTermACC = errorAngle * cfg.P8[PIDLEVEL] / 100;    PTermACC = constrain(PTermACC, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);    errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp    ITermACC = (errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12;}

依赖于加速度传感器

errorAngle：计算期望角度与实际角度之间的误差，考虑了GPS角度校正和角度修剪。

PTermACC：基于误差计算比例项，并限制其范围。

errorAngleI[] 和 ITermACC：更新并计算积分项，防止积分风-up（即积分值过大导致系统不稳定）。

非角度模式或地平线模式

if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis]; error -= gyroData[axis]; PTermGYRO = rcCommand[axis]; errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp if ((abs(gyroData[axis]) > 640) || ((axis == YAW) && (abs(rcCommand[axis]) > 100))) errorGyroI[axis] = 0; ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6;}

依赖于陀螺仪

error：计算基于角速度的误差。

PTermGYRO：直接使用遥控输入作为比例项。

errorGyroI[] 和 ITermGYRO：更新并计算积分项，同样防止积分风-up。对于高角速度或大遥控输入情况，重置积分项以避免不稳定的积累。

混合比例项和积分项

if (f.HORIZON_MODE && axis < 2) {    PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;    ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500;} else {    if (f.ANGLE_MODE && axis < 2) {        PTerm = PTermACC;        ITerm = ITermACC;    } else {        PTerm = PTermGYRO;        ITerm = ITermGYRO;    }}

根据当前飞行模式（角度模式或地平线模式），按比例混合基于角度和角速度的比例项和积分项。

计算最终的 P-term 和 D-term

PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8;delta = gyroData[axis] - lastGyro[axis];lastGyro[axis] = gyroData[axis];deltaSum = delta1[axis] + delta2[axis] + delta;delta2[axis] = delta1[axis];delta1[axis] = delta;DTerm = (deltaSum * dynD8[axis]) / 32;axisPID[axis] = PTerm + ITerm - DTerm;

PTerm：进一步调整比例项，考虑动态比例增益。

delta 和 deltaSum：计算最近三次角速度变化的总和，用于生成更稳定的微分项。

DTerm：基于累积的角速度变化计算微分项，并通过缩放操作确保数值在合理范围内。

axisPID[]：将比例、积分和微分项组合成最终的姿态调整命令。

小结

pidMultiWii() 函数实现了多旋翼飞行器的姿态控制逻辑，通过结合来自不同传感器的数据和用户输入，精确计算每个轴的姿态调整值。它根据不同的飞行模式（角度模式、地平线模式）和飞行条件，灵活调整控制策略，最终为下一级处理函数提供了axisPID内容。

这个函数不仅体现了 PID 控制算法的核心思想，还展示了如何在实际应用中针对特定需求进行优化和调整。例如，通过混合比例项和积分项，可以在不同飞行模式下提供更加适合的控制效果；而通过累积角速度变化（delta1[axis] + delta2[axis] + delta）来计算微分项，则有助于提高系统的稳定性和抗噪能力。

mixTable()的解析

mixTable() 函数在 baseflight飞行控制器中负责将姿态调整命令（axisPID）转换为具体的电机功率输出（motor[]）。它还处理舵机控制信号的生成，确保飞行器按照期望的姿态和运动进行操作。下面是对该函数的详细解析。

函数结构与变量

void mixTable(void){    int16_t maxMotor;    uint32_t i;

maxMotor：用于记录所有电机中的最大功率值，以便进行后续的归一化处理。

i：循环变量，用于遍历电机和舵机。

防止偏航跳跃

if (numberMotor > 3) { axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW]));}

如果电机数量大于3个，则限制 axisPID[YAW]的范围，以防止偏航修正过程中出现突然的大角度变化（即“偏航跳跃”），这有助于保持飞行器的稳定性。

计算非伺服混合的电机功率

if (numberMotor > 1) {    for (i = 0; i < numberMotor; i++) {        motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle +                   axisPID[PITCH] * currentMixer[i].pitch +                   axisPID[ROLL] * currentMixer[i].roll +                   -cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw;        if (f.FIXED_WING) { // vector_thrust handling            if (cfg.fw_vector_thrust) {                if (f.PASSTHRU_MODE)                    motor[i] = rcCommand[THROTTLE] - rcCommand[YAW] * (i - 0.5f);            } else { // Override mixerVectorThrust                motor[i] = rcCommand[THROTTLE];            }        }    }}

motor[i]：计算每个电机的功率输出，基于油门输入、姿态调整命令（俯仰、滚转、偏航）以及当前混合配置 (`currentMixer`)。

固定翼模式：根据配置是否启用矢量推力（fw_vector_thrust），进一步调整电机输出。如果启用了直通模式（PASSTHRU_MODE），则对每个电机应用不同的偏航修正；否则，直接使用油门输入作为电机功率。

处理飞机或舵机混合

switch (mcfg.mixerConfiguration) { case MULTITYPE_CUSTOM_PLANE: case MULTITYPE_FLYING_WING: case MULTITYPE_AIRPLANE: case MULTITYPE_BI: case MULTITYPE_TRI: case MULTITYPE_DUALCOPTER: case MULTITYPE_SINGLECOPTER: servoMixer(); break; case MULTITYPE_GIMBAL: servo[0] = (((int32_t)cfg.servoConf[0].rate * angle[PITCH]) / 50) + servoMiddle(0); servo[1] = (((int32_t)cfg.servoConf[1].rate * angle[ROLL]) / 50) + servoMiddle(1); break;}

根据混合配置类型调用相应的舵机混合函数（如 servoMixer()）或直接设置舵机位置（如云台控制）。

相机稳定（CamStab）

if (feature(FEATURE_SERVO_TILT)) {    servo[0] = servoMiddle(0);    servo[1] = servoMiddle(1);    if (rcOptions[BOXCAMSTAB]) {        if (cfg.gimbal_flags & GIMBAL_MIXTILT) {            servo[0] -= (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50;            servo[1] += (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50;        } else {            servo[0] += (int32_t)cfg.servoConf[0].rate * angle[PITCH] / 50;            servo[1] += (int32_t)cfg.servoConf[1].rate * angle[ROLL]  / 50;        }    }}

如果启用了相机稳定功能（FEATURE_SERVO_TILT），则根据姿态调整命令（俯仰、滚转）更新舵机位置，以实现相机的稳定。

约束舵机输出

for (i = 0; i < MAX_SERVOS; i++) servo[i] = constrain(servo[i], cfg.servoConf[i].min, cfg.servoConf[i].max); // limit the values

确保每个舵机的输出值在其允许范围内，避免超出极限导致损坏或其他问题。

AUX通道转发到舵机输出

if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) {    int offset = core.numServos - 4;    for (i = 0; i < 4; i++)        pwmWriteServo(i + offset, rcData[AUX1 + i]);}

将辅助通道（AUX1-AUX4）的数据直接转发到指定的舵机输出，适用于需要额外控制信号的应用场景。

归一化电机输出并约束功率

maxMotor = motor[0];for (i = 1; i < numberMotor; i++) if (motor[i] > maxMotor) maxMotor = motor[i];for (i = 0; i < numberMotor; i++) { if (maxMotor > mcfg.maxthrottle && !f.FIXED_WING) { motor[i] -= maxMotor - mcfg.maxthrottle; } if (feature(FEATURE_3D)) { if ((rcData[THROTTLE]) > mcfg.midrc) { motor[i] = constrain(motor[i], mcfg.deadband3d_high, mcfg.maxthrottle); if ((mcfg.mixerConfiguration) == MULTITYPE_TRI) { servo[5] = constrain(servo[5], cfg.servoConf[5].min, cfg.servoConf[5].max); } } else { motor[i] = constrain(motor[i], mcfg.mincommand, mcfg.deadband3d_low); if ((mcfg.mixerConfiguration) == MULTITYPE_TRI) { servo[5] = constrain(servo[5], cfg.servoConf[5].max, cfg.servoConf[5].min); } } } else { motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle); if ((rcData[THROTTLE]) < mcfg.mincheck) { if (!feature(FEATURE_MOTOR_STOP)) motor[i] = mcfg.minthrottle; else { motor[i] = mcfg.mincommand; f.MOTORS_STOPPED = 1; } } else { f.MOTORS_STOPPED = 0; } } if (!f.ARMED) { motor[i] = motor_disarmed[i]; f.MOTORS_STOPPED = 1; }}

归一化：找到所有电机的最大功率值，并将其归一化至最大允许值（mcfg.maxthrottle），以确保不会超过安全范围。

约束功率：根据不同条件（如3D飞行模式、未武装状态等）进一步约束电机功率，确保其在合理范围内工作。

特殊处理：对于某些特定的混合配置（如三轴飞行器），还需要额外处理舵机输出，以适应特定的机械设计需求。

小结

mixTable()函数是连接姿态控制和电机驱动的关键环节，它负责将 PID 控制器计算出的姿态调整命令转换为具体的电机功率输出，并处理相关的舵机控制信号。这个过程不仅考虑了基本的飞行控制需求，还涵盖了多种特殊情况和功能扩展，如防止偏航跳跃、相机稳定、辅助通道转发等，确保飞行器能够在各种条件下稳定运行。

通过这种方式，mixTable() 实现了从姿态调整到实际物理动作的桥梁作用，保证了飞行器能够精确响应用户指令和环境变化，提供了灵活且可靠的飞行体验。

pwmWriteMotor()解析

我们直接看主体的控制动力电机部分pwmWriteMotor()

pwmWriteMotor()在 drv_pwm.c 文件中，函数的具体实现如下。

输入参数

void pwmWriteMotor(uint8_t index, uint16_t value){    if (index < numMotors)        pwmWritePtr(index, value);}

uint8_t index：表示电机的索引号。它指定了要控制哪个电机。

uint16_t value`：表示要设置的PWM值，通常是一个介于0到最大占空比之间的整数值。

该条件确保提供的电机索引在合法范围内（即小于 numMotors）。如果索引超出范围，则不会执行任何操作，避免访问无效的内存地址或引起其他错误。

如果索引有效，则通过 pwmWritePtr函数指针调用实际的PWM写入函数。

pwmWritePtr是一个函数指针，在初始化时根据配置选择不同的PWM写入方法。这允许代码根据不同的硬件配置或需求灵活选择最合适的PWM生成方式。

pwmWritePtr的选择

在 pwmInit()函数中，pwmWritePtr被赋值为以下几种可能之一：

// determine motor writer functionpwmWritePtr = pwmWriteStandard;if (init->motorPwmRate > 500) pwmWritePtr = pwmWriteBrushed;else if (init->syncPWM) pwmWritePtr = pwmWriteSyncPwm;

pwmWriteStandard：标准的PWM写入方法，适用于大多数情况。

pwmWriteBrushed：用于高频率PWM（如无刷电机），当 motorPwmRate大于500Hz时使用。

pwmWriteSyncPwm：同步PWM模式，适用于需要严格同步的场景。

实际写入函数解析

以下是三种可能的PWM写入函数的具体实现：

1. pwmWriteStandard

static void pwmWriteStandard(uint8_t index, uint16_t value)   {       *motors[index]->ccr = value;   }

直接将PWM值写入对应的定时器捕获/比较寄存器 (CCR)，以设置占空比。

2. pwmWriteBrushed

static void pwmWriteBrushed(uint8_t index, uint16_t value) { *motors[index]->ccr = (value - 1000) * motors[index]->period / 1000; }

对于高频率PWM，先对输入值进行缩放和偏移处理，然后写入定时器捕获/比较寄存器。

3. pwmWriteSyncPwm

   static void pwmWriteSyncPwm(uint8_t index, uint16_t value)   {       *motors[index]->cr1 &= (uint16_t) ~(0x0001);    // disable timer       *motors[index]->cnt = 0x0000;                   // set timer counter to zero       *motors[index]->ccr = value;                    // set the pwm value       *motors[index]->cr1 |= (uint16_t) (0x0001);     // enable timer   }