開源Flight飛控系列 | Baseflight核心函數(shù)解析

【開源Flight飛控系列】這個(gè)系列是解析開源飛行控制系統(tǒng) - Flight系列項(xiàng)目的文章，喜歡的話記得關(guān)注我們，這樣就能第一時(shí)間收到更新通知啦。從Baseflight開始到Cleanflight，以及兩個(gè)基于Cleanflight發(fā)展而來、的Betaflight和iNavFlight。

本文是系列的第三部分，詳細(xì)探討了多旋翼飛行控制器中兩個(gè)核心函數(shù) pidMultiWii() 和 mixTable() 的工作原理，以及PWM寫入函數(shù) pwmWriteMotor() 的具體實(shí)現(xiàn)。

pidMultiWii() 函數(shù)負(fù)責(zé)根據(jù)傳感器數(shù)據(jù)和遙控器輸入計(jì)算姿態(tài)調(diào)整值，以確保飛行器的穩(wěn)定性和響應(yīng)性。mixTable() 函數(shù)則將這些姿態(tài)調(diào)整值轉(zhuǎn)換為具體的電機(jī)功率輸出，并處理舵機(jī)控制信號，使飛行器能夠按照預(yù)期的姿態(tài)和運(yùn)動進(jìn)行操作。最后，pwmWriteMotor() 函數(shù)實(shí)現(xiàn)了將計(jì)算出的PWM值寫入對應(yīng)的定時(shí)器寄存器，從而控制電機(jī)的實(shí)際轉(zhuǎn)速。

01、pid_controller()的解析

pid_controller()是一個(gè)函數(shù)指針，指向pidMultiWii() ，pidMultiWii() 函數(shù)是 baseflight 飛行控制器中實(shí)現(xiàn) PID 控制邏輯的核心部分。它負(fù)責(zé)根據(jù)傳感器數(shù)據(jù)（如陀螺儀和加速度計(jì)）以及遙控器輸入，計(jì)算出每個(gè)軸（滾轉(zhuǎn)、俯仰、偏航）的姿態(tài)調(diào)整值 (axisPID)。下面是對該函數(shù)的詳細(xì)解析。

函數(shù)結(jié)構(gòu)與變量

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：循環(huán)變量，用于遍歷三個(gè)軸（滾轉(zhuǎn)、俯仰、偏航）。

prop：比例因子，基于遙控輸入的最大值來決定混合模式的比例。

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

PTerm, ITerm, DTerm：PID控制中的比例、積分和微分項(xiàng)。

PTermACC, ITermACC, PTermGYRO, ITermGYRO：分別為基于角度和角速度的P和I項(xiàng)。

lastGyro[]：存儲上次的陀螺儀讀數(shù)，用于計(jì)算角速度變化。

delta1[], delta2[]：存儲前兩次的角速度變化量，用于計(jì)算 deltaSum。

比例因子計(jì)算

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

計(jì)算滾轉(zhuǎn)和俯仰軸遙控輸入的最大絕對值，并將其映射到范圍 [0, 500] 內(nèi)。這個(gè)值將用于后續(xù)的角度模式和地平線模式下的比例混合。

遍歷每個(gè)軸

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

對每個(gè)軸（滾轉(zhuǎn)、俯仰、偏航）進(jìn)行處理。

角度模式或地平線模式

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：計(jì)算期望角度與實(shí)際角度之間的誤差，考慮了GPS角度校正和角度修剪。

PTermACC：基于誤差計(jì)算比例項(xiàng)，并限制其范圍。

errorAngleI[] 和 ITermACC：更新并計(jì)算積分項(xiàng)，防止積分風(fēng)-up（即積分值過大導(dǎo)致系統(tǒng)不穩(wěn)定）。

非角度模式或地平線模式

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：計(jì)算基于角速度的誤差。

PTermGYRO：直接使用遙控輸入作為比例項(xiàng)。

errorGyroI[] 和 ITermGYRO：更新并計(jì)算積分項(xiàng)，同樣防止積分風(fēng)-up。對于高角速度或大遙控輸入情況，重置積分項(xiàng)以避免不穩(wěn)定的積累。

混合比例項(xiàng)和積分項(xiàng)

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;? ? }}

根據(jù)當(dāng)前飛行模式（角度模式或地平線模式），按比例混合基于角度和角速度的比例項(xiàng)和積分項(xiàng)。

計(jì)算最終的 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：進(jìn)一步調(diào)整比例項(xiàng)，考慮動態(tài)比例增益。

delta 和 deltaSum：計(jì)算最近三次角速度變化的總和，用于生成更穩(wěn)定的微分項(xiàng)。

DTerm：基于累積的角速度變化計(jì)算微分項(xiàng)，并通過縮放操作確保數(shù)值在合理范圍內(nèi)。

axisPID[]：將比例、積分和微分項(xiàng)組合成最終的姿態(tài)調(diào)整命令。

小結(jié)

pidMultiWii() 函數(shù)實(shí)現(xiàn)了多旋翼飛行器的姿態(tài)控制邏輯，通過結(jié)合來自不同傳感器的數(shù)據(jù)和用戶輸入，精確計(jì)算每個(gè)軸的姿態(tài)調(diào)整值。它根據(jù)不同的飛行模式（角度模式、地平線模式）和飛行條件，靈活調(diào)整控制策略，最終為下一級處理函數(shù)提供了axisPID內(nèi)容。

這個(gè)函數(shù)不僅體現(xiàn)了 PID 控制算法的核心思想，還展示了如何在實(shí)際應(yīng)用中針對特定需求進(jìn)行優(yōu)化和調(diào)整。例如，通過混合比例項(xiàng)和積分項(xiàng)，可以在不同飛行模式下提供更加適合的控制效果；而通過累積角速度變化（delta1[axis] + delta2[axis] + delta）來計(jì)算微分項(xiàng)，則有助于提高系統(tǒng)的穩(wěn)定性和抗噪能力。

02、mixTable()的解析

mixTable() 函數(shù)在 baseflight飛行控制器中負(fù)責(zé)將姿態(tài)調(diào)整命令（axisPID）轉(zhuǎn)換為具體的電機(jī)功率輸出（motor[]）。它還處理舵機(jī)控制信號的生成，確保飛行器按照期望的姿態(tài)和運(yùn)動進(jìn)行操作。下面是對該函數(shù)的詳細(xì)解析。

函數(shù)結(jié)構(gòu)與變量

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

maxMotor：用于記錄所有電機(jī)中的最大功率值，以便進(jìn)行后續(xù)的歸一化處理。

i：循環(huán)變量，用于遍歷電機(jī)和舵機(jī)。

防止偏航跳躍

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

如果電機(jī)數(shù)量大于3個(gè)，則限制 axisPID[YAW]的范圍，以防止偏航修正過程中出現(xiàn)突然的大角度變化（即“偏航跳躍”），這有助于保持飛行器的穩(wěn)定性。

計(jì)算非伺服混合的電機(jī)功率

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]：計(jì)算每個(gè)電機(jī)的功率輸出，基于油門輸入、姿態(tài)調(diào)整命令（俯仰、滾轉(zhuǎn)、偏航）以及當(dāng)前混合配置 (`currentMixer`)。

固定翼模式：根據(jù)配置是否啟用矢量推力（fw_vector_thrust），進(jìn)一步調(diào)整電機(jī)輸出。如果啟用了直通模式（PASSTHRU_MODE），則對每個(gè)電機(jī)應(yīng)用不同的偏航修正；否則，直接使用油門輸入作為電機(jī)功率。

處理飛機(jī)或舵機(jī)混合

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;}

根據(jù)混合配置類型調(diào)用相應(yīng)的舵機(jī)混合函數(shù)（如 servoMixer()）或直接設(shè)置舵機(jī)位置（如云臺控制）。

相機(jī)穩(wěn)定（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;? ? ? ? }? ? }}

如果啟用了相機(jī)穩(wěn)定功能（FEATURE_SERVO_TILT），則根據(jù)姿態(tài)調(diào)整命令（俯仰、滾轉(zhuǎn)）更新舵機(jī)位置，以實(shí)現(xiàn)相機(jī)的穩(wěn)定。

約束舵機(jī)輸出

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

確保每個(gè)舵機(jī)的輸出值在其允許范圍內(nèi)，避免超出極限導(dǎo)致?lián)p壞或其他問題。

AUX通道轉(zhuǎn)發(fā)到舵機(jī)輸出

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）的數(shù)據(jù)直接轉(zhuǎn)發(fā)到指定的舵機(jī)輸出，適用于需要額外控制信號的應(yīng)用場景。

歸一化電機(jī)輸出并約束功率

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;? ? }}

歸一化：找到所有電機(jī)的最大功率值，并將其歸一化至最大允許值（mcfg.maxthrottle），以確保不會超過安全范圍。

約束功率：根據(jù)不同條件（如3D飛行模式、未武裝狀態(tài)等）進(jìn)一步約束電機(jī)功率，確保其在合理范圍內(nèi)工作。

特殊處理：對于某些特定的混合配置（如三軸飛行器），還需要額外處理舵機(jī)輸出，以適應(yīng)特定的機(jī)械設(shè)計(jì)需求。

小結(jié)

mixTable()函數(shù)是連接姿態(tài)控制和電機(jī)驅(qū)動的關(guān)鍵環(huán)節(jié)，它負(fù)責(zé)將 PID 控制器計(jì)算出的姿態(tài)調(diào)整命令轉(zhuǎn)換為具體的電機(jī)功率輸出，并處理相關(guān)的舵機(jī)控制信號。這個(gè)過程不僅考慮了基本的飛行控制需求，還涵蓋了多種特殊情況和功能擴(kuò)展，如防止偏航跳躍、相機(jī)穩(wěn)定、輔助通道轉(zhuǎn)發(fā)等，確保飛行器能夠在各種條件下穩(wěn)定運(yùn)行。

通過這種方式，mixTable() 實(shí)現(xiàn)了從姿態(tài)調(diào)整到實(shí)際物理動作的橋梁作用，保證了飛行器能夠精確響應(yīng)用戶指令和環(huán)境變化，提供了靈活且可靠的飛行體驗(yàn)。

03、pwmWriteMotor()解析

我們直接看主體的控制動力電機(jī)部分pwmWriteMotor()

pwmWriteMotor()在 drv_pwm.c 文件中， 函數(shù)的具體實(shí)現(xiàn)如下。

輸入?yún)?shù)

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

uint8_t index：表示電機(jī)的索引號。它指定了要控制哪個(gè)電機(jī)。

uint16_t value`：表示要設(shè)置的PWM值，通常是一個(gè)介于0到最大占空比之間的整數(shù)值。

該條件確保提供的電機(jī)索引在合法范圍內(nèi)（即小于 numMotors）。如果索引超出范圍，則不會執(zhí)行任何操作，避免訪問無效的內(nèi)存地址或引起其他錯(cuò)誤。

如果索引有效，則通過 pwmWritePtr函數(shù)指針調(diào)用實(shí)際的PWM寫入函數(shù)。

pwmWritePtr是一個(gè)函數(shù)指針，在初始化時(shí)根據(jù)配置選擇不同的PWM寫入方法。這允許代碼根據(jù)不同的硬件配置或需求靈活選擇最合適的PWM生成方式。

pwmWritePtr的選擇

在 pwmInit()函數(shù)中，pwmWritePtr被賦值為以下幾種可能之一：

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

pwmWriteStandard：標(biāo)準(zhǔn)的PWM寫入方法，適用于大多數(shù)情況。

pwmWriteBrushed：用于高頻率PWM（如無刷電機(jī)），當(dāng) motorPwmRate大于500Hz時(shí)使用。

pwmWriteSyncPwm：同步PWM模式，適用于需要嚴(yán)格同步的場景。

實(shí)際寫入函數(shù)解析

以下是三種可能的PWM寫入函數(shù)的具體實(shí)現(xiàn)：

1. pwmWriteStandard

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

直接將PWM值寫入對應(yīng)的定時(shí)器捕獲/比較寄存器 (CCR)，以設(shè)置占空比。

2. pwmWriteBrushed

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

對于高頻率PWM，先對輸入值進(jìn)行縮放和偏移處理，然后寫入定時(shí)器捕獲/比較寄存器。

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? ?}

在同步PWM模式下，先禁用定時(shí)器，重置計(jì)數(shù)器，設(shè)置新的PWM值，最后重新啟用定時(shí)器。這種模式確保所有PWM信號同步更新。

小結(jié)

pwmWriteMotor()函數(shù)通過簡單的索引檢查和函數(shù)指針調(diào)用機(jī)制，實(shí)現(xiàn)了高效且靈活的PWM信號生成。它能夠根據(jù)不同的硬件配置和需求選擇最合適的方法來設(shè)置電機(jī)的PWM值，確保飛行控制器能夠在各種條件下穩(wěn)定運(yùn)行并精確控制電機(jī)輸出。這種方式不僅簡化了代碼結(jié)構(gòu)，還提高了系統(tǒng)的可維護(hù)性和擴(kuò)展性。

04、最后

pidMultiWii()函數(shù)，該函數(shù)通過結(jié)合傳感器數(shù)據(jù)（如陀螺儀、加速度計(jì)）和遙控器輸入來計(jì)算每個(gè)軸的姿態(tài)調(diào)整值，確保飛行器在各種飛行模式下保持穩(wěn)定與響應(yīng)性。它根據(jù)不同的飛行條件靈活調(diào)整控制策略，并通過混合比例項(xiàng)和積分項(xiàng)為不同模式提供更佳的控制效果。

mixTable()函數(shù)，這是連接姿態(tài)控制與實(shí)際物理動作的關(guān)鍵環(huán)節(jié)，將PID控制器計(jì)算出的姿態(tài)調(diào)整命令轉(zhuǎn)換成具體的電機(jī)功率輸出，并處理相關(guān)的舵機(jī)控制信號，以滿足多種特殊配置需求，如防止偏航跳躍、相機(jī)穩(wěn)定等。

pwmWriteMotor()的具體實(shí)現(xiàn)，包括如何選擇適當(dāng)?shù)腜WM寫入方法以及如何安全地設(shè)置占空比以控制電機(jī)轉(zhuǎn)速。

PS：微信【聚翼無人機(jī)】后臺點(diǎn)【聯(lián)系我們】加微信發(fā)送開源項(xiàng)目。