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

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

axis：循環(huán)變量，用于遍歷三個軸（滾轉(zhuǎn)、俯仰、偏航）。

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

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

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

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

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

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

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

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

對每個軸（滾轉(zhuǎ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：計算期望角度與實際角度之間的誤差，考慮了GPS角度校正和角度修剪。

PTermACC：基于誤差計算比例項，并限制其范圍。

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

依賴于陀螺儀

error：計算基于角速度的誤差。

PTermGYRO：直接使用遙控輸入作為比例項。

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

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)前飛行模式（角度模式或地平線模式），按比例混合基于角度和角速度的比例項和積分項。

PTerm：進一步調(diào)整比例項，考慮動態(tài)比例增益。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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ā)到指定的舵機輸出，適用于需要額外控制信號的應(yīng)用場景。

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

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

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

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

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

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

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

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

uint8_t index：表示電機的索引號。它指定了要控制哪個電機。

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

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

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

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

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

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

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

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

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

1. pwmWriteStandard

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

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

2. pwmWriteBrushed

對于高頻率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   }

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

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

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

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

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