庖丁解牛 | Baseflight中慣導(dǎo)模塊(IMU.c)的代碼解讀

Baseflight是十分新手友好的，其中一點(diǎn)就體現(xiàn)在每個(gè)模塊的文件平均不愁過(guò)500行。（請(qǐng)注意是平均，確實(shí)存在幾個(gè)八九百行或者上千行的文件）

軟件工程中，幾乎一切困難都是規(guī)模造成的。隨著代碼規(guī)模的增長(zhǎng)，代碼的復(fù)雜度是幾何級(jí)數(shù)增加，而不是線性增長(zhǎng)。也就是代碼長(zhǎng)度翻一倍，理解和開(kāi)發(fā)維護(hù)的復(fù)雜度翻四倍而不是兩倍。大型代碼庫(kù)往往包含多個(gè)模塊、類和函數(shù)，這些組件之間的交互變得越來(lái)越復(fù)雜，導(dǎo)致開(kāi)發(fā)者難以理解和維護(hù)代碼。特別是在缺乏良好的文檔和注釋的情況下，新加入的開(kāi)發(fā)人員可能需要花費(fèi)大量時(shí)間來(lái)理解現(xiàn)有代碼的功能和結(jié)構(gòu)。

所以老手一般會(huì)選擇一個(gè)足夠小但功能全的工程入手，快速理清全局脈絡(luò)。后面添加自己的功能或者讀懂其他人添加的功能就很容易了。

01、imu模塊的全覽

imu模塊并不是自成一體的模塊，而更像一個(gè)工具庫(kù)，供其它模塊調(diào)用。

這里所說(shuō)的其他模塊就是main.c拉起來(lái)的無(wú)限循環(huán)while(1)里面的loop（）函數(shù)。loop是整個(gè)項(xiàng)目最大的函數(shù)，其實(shí)現(xiàn)在mx.c中（mx代表main work，意思是主要干活的地方），值得以后單獨(dú)解析。

無(wú)人機(jī)飛行就是不停執(zhí)行l(wèi)oop，每一次都感知環(huán)境+接收指令，然后變?yōu)榻o四個(gè)電機(jī)的本loop的轉(zhuǎn)速和方向（一般是占空比信號(hào)PWM）并執(zhí)行。飛機(jī)就這樣不停地飛起來(lái)了。直到飛機(jī)關(guān)機(jī)形成硬中斷來(lái)退出main.c拉起來(lái)的無(wú)限循環(huán)while(1),這就是關(guān)機(jī)了。

圖1 imu中函數(shù)的全列表

查看調(diào)用關(guān)系，實(shí)際上只有兩個(gè)函數(shù)是整個(gè)imu模塊的出口

getEstimatedAttitude 獲取飛行器姿態(tài)估計(jì)

getEstimatedAltitude 獲取飛行器高度估計(jì)

其余函數(shù)都是模塊內(nèi)部函數(shù)被這兩位調(diào)用的，這兩位再被loop調(diào)用，用于每一輪飛行中先估計(jì)姿態(tài)和高度。而且注意，獲取高度的函數(shù)getEstimatedAltitude是在預(yù)定義判斷的ifdef BARO里面，BARO是氣壓計(jì)的意思，也就是說(shuō)沒(méi)有氣壓計(jì)無(wú)法估算高度，高度函數(shù)根本就不執(zhí)行。

02、imu模塊的源碼和詳細(xì)注釋

本文采?。◣缀酰┮恍幸蛔⑨尩男问絹?lái)解讀代碼，英文注釋是代碼原有的，比較少，不足以解釋，中文注釋是筆者加的，解釋性較強(qiáng)。這種超長(zhǎng)注釋只是為了解釋，并不符合良好的代碼規(guī)范。

#include?"board.h"#include?"mw.h"// 存儲(chǔ)來(lái)自陀螺儀的原始數(shù)據(jù)int16_t?gyroADC[3], accADC[3], accSmooth[3], magADC[3];// 存儲(chǔ)加速度計(jì)數(shù)據(jù)的累積和，長(zhǎng)度3是因?yàn)閤yzint32_t?accSum[3];// 累積加速度計(jì)數(shù)據(jù)的時(shí)間總和uint32_t?accTimeSum =?0;// 加速度計(jì)數(shù)據(jù)的累積次數(shù)int?accSumCount =?0;// 小角度值int16_t?smallAngle =?0;// 氣壓計(jì)壓力int32_t?baroPressure =?0;// 氣壓計(jì)溫度int32_t?baroTemperature =?0;// 氣壓計(jì)壓力的累積和uint32_t?baroPressureSum =?0;// 氣壓計(jì)高度int32_t?BaroAlt =?0;// 超聲波轉(zhuǎn)換系數(shù)float?sonarTransition =?0;// 氣壓計(jì)高度偏移int32_t?baroAlt_offset =?0;// 超聲波測(cè)距高度int32_t?sonarAlt =?-1; ? ? ? ??// in cm , -1 indicate sonar is not in range// 估計(jì)的高度int32_t?EstAlt; ? ? ? ? ? ? ? ?// 氣壓計(jì)PID控制器輸出int32_t?BaroPID =?0;// 高度設(shè)定值int32_t?AltHold;// 設(shè)置速度int32_t?setVelocity =?0;// 速度控制標(biāo)志uint8_t?velocityControl =?0;// 速度積分誤差int32_t?errorVelocityI =?0;// 變速率（垂直速度）int32_t?vario =?0; ? ? ? ? ? ? ? ? ? ? ?// 油門(mén)角度修正int16_t?throttleAngleCorrection =?0; ? ?// 磁偏角float?magneticDeclination =?0.0f; ? ? ??// 加速度計(jì)速度比例因子float?accVelScale;// 油門(mén)角度比例因子float?throttleAngleScale;// 加速度計(jì)時(shí)間常數(shù)float?fc_acc;// 存儲(chǔ)來(lái)自陀螺儀的原始數(shù)據(jù)int16_t?gyroData[3] = {?0,?0,?0?};// 存儲(chǔ)陀螺儀零點(diǎn)int16_t?gyroZero[3] = {?0,?0,?0?};// 絕對(duì)角度傾角（單位為0.1度）int16_t?angle[2] = {?0,?0?};// 絕對(duì)角度傾角（單位為弧度）float?anglerad[2] = {?0.0f,?0.0f?};// 獲取估計(jì)姿態(tài)的函數(shù)static?void?getEstimatedAttitude(void);

// 初始化IMU相關(guān)參數(shù)void?imuInit(void){? ??// 計(jì)算小角度值? ? smallAngle =?lrintf(acc_1G *?cosf(RAD * cfg.small_angle));? ??/* lrintf多次用到，是一個(gè)數(shù)值轉(zhuǎn)換函數(shù)，不是打印函數(shù)? ? lrintf 是 C 語(yǔ)言標(biāo)準(zhǔn)庫(kù)中的一個(gè)數(shù)學(xué)函數(shù)，用于將單精度浮點(diǎn)數(shù)（float 類型）舍入為最接近的整數(shù)值，并將其轉(zhuǎn)換為 long int 類型。一般向下取整*/? ??// 計(jì)算加速度計(jì)速度比例因子? ? accVelScale =?9.80665f?/ acc_1G /?10000.0f;? ??// 計(jì)算油門(mén)角度比例因子? ? throttleAngleScale = (1800.0f?/ M_PI) * (900.0f?/ cfg.throttle_correction_angle);? ??// 計(jì)算加速度計(jì)Z軸低通濾波器的時(shí)間常數(shù)? ? fc_acc =?0.5f?/ (M_PI * cfg.accz_lpf_cutoff);#ifdef?MAG? ??// 如果啟用磁力計(jì)，初始化磁力計(jì)? ??if?(sensors(SENSOR_MAG))? ? ? ??Mag_init();#endif}

// 計(jì)算IMU數(shù)據(jù)void?computeIMU(void){? ??static?int16_t?gyroYawSmooth =?0;? ??// 獲取陀螺儀數(shù)據(jù)? ??Gyro_getADC();? ??// 如果啟用加速度計(jì)，獲取加速度計(jì)數(shù)據(jù)并計(jì)算姿態(tài)? ??if?(sensors(SENSOR_ACC)) {? ? ? ??ACC_getADC();? ? ? ??getEstimatedAttitude();? ? }?else?{? ? ? ??// 如果未啟用加速度計(jì)，將加速度計(jì)數(shù)據(jù)設(shè)為0? ? ? ? accADC[X] =?0;? ? ? ? accADC[Y] =?0;? ? ? ? accADC[Z] =?0;? ? }? ??// 如果混合配置為三旋翼，平滑陀螺儀偏航數(shù)據(jù)? ??if?(mcfg.mixerConfiguration == MULTITYPE_TRI) {? ? ? ? gyroData[YAW] = (gyroYawSmooth *?2?+ gyroADC[YAW]) /?3;? ? ? ? gyroYawSmooth = gyroData[YAW];? ? }?else?{? ? ? ??// 否則直接使用陀螺儀偏航數(shù)據(jù)? ? ? ? gyroData[YAW] = gyroADC[YAW];? ? }? ??// 直接使用陀螺儀滾轉(zhuǎn)和俯仰數(shù)據(jù)? ? gyroData[ROLL] = gyroADC[ROLL];? ? gyroData[PITCH] = gyroADC[PITCH];}// 使用互補(bǔ)濾波器簡(jiǎn)化IMU計(jì)算#define?INV_GYR_CMPF_FACTOR ? (1.0f / ((float)mcfg.gyro_cmpf_factor + 1.0f))#define?INV_GYR_CMPFM_FACTOR ?(1.0f / ((float)mcfg.gyro_cmpfm_factor + 1.0f))/*互補(bǔ)濾波器的工作原理其實(shí)就是人為根據(jù)信號(hào)特點(diǎn)對(duì)不同信號(hào)做不同原則的濾波互補(bǔ)濾波器通常應(yīng)用于慣性測(cè)量單元（IMU）的姿態(tài)估計(jì)中，結(jié)合加速度計(jì)和陀螺儀的數(shù)據(jù)來(lái)計(jì)算物體的姿態(tài)角。具體來(lái)說(shuō)：1.加速度計(jì)：能夠提供靜態(tài)姿態(tài)信息，但容易受到高頻噪聲的影響。2.陀螺儀：能夠提供動(dòng)態(tài)姿態(tài)變化信息，但長(zhǎng)時(shí)間積分會(huì)導(dǎo)致漂移誤差。為了克服各自的缺點(diǎn)，互補(bǔ)濾波器采用以下策略：·對(duì)于加速度計(jì)，使用低通濾波器（Low-Pass Filter, LPF）去除高頻噪聲，保留低頻部分。·對(duì)于陀螺儀，使用高通濾波器（High-Pass Filter, HPF）去除低頻漂移，保留高頻變化。*/

// 定義浮點(diǎn)向量結(jié)構(gòu)體typedef?struct?fp_vector?{? ??float?X;? ??float?Y;? ??float?Z;} t_fp_vector_def;// 定義浮點(diǎn)向量聯(lián)合體typedef?union?{? ??float?A[3];? ? t_fp_vector_def V;} t_fp_vector;// 存儲(chǔ)估計(jì)的姿態(tài)向量t_fp_vector EstG;

// 歸一化向量void?normalizeV(struct?fp_vector *src,?struct?fp_vector *dest){? ??float?length;? ??// 計(jì)算向量長(zhǎng)度? ? length = sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z);? ??if?(length !=?0) {? ? ? ??// 歸一化向量? ? ? ? dest->X = src->X / length;? ? ? ? dest->Y = src->Y / length;? ? ? ? dest->Z = src->Z / length;? ? }}

// 根據(jù)陀螺儀數(shù)據(jù)旋轉(zhuǎn)估計(jì)向量void?rotateV(struct?fp_vector *v,?float?*delta){? ??struct?fp_vector v_tmp = *v;? ??// 計(jì)算旋轉(zhuǎn)矩陣元素? ??float?cosx, sinx, cosy, siny, cosz, sinz;? ??float?coszcosx, sinzcosx, coszsinx, sinzsinx;? ? cosx = cosf(delta[ROLL]);? ? sinx = sinf(delta[ROLL]);? ? cosy = cosf(delta[PITCH]);? ? siny = sinf(delta[PITCH]);? ? cosz = cosf(delta[YAW]);? ? sinz = sinf(delta[YAW]);? ? coszcosx = cosz * cosx;? ? sinzcosx = sinz * cosx;? ? coszsinx = sinx * cosz;? ? sinzsinx = sinx * sinz;? ??float?mat[3][3] = {? ? ? ? { cosz * cosy, -cosy * sinz, siny },? ? ? ? { sinzcosx + (coszsinx * siny), coszcosx - (sinzsinx * siny), -sinx * cosy },? ? ? ? { (sinzsinx) - (coszcosx * siny), (coszsinx) + (sinzcosx * siny), cosy * cosx }? ? };? ??// 應(yīng)用旋轉(zhuǎn)矩陣? ? v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0];? ? v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1];? ? v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];}

// 應(yīng)用死區(qū)int32_t?applyDeadband(int32_t?value,?int32_t?deadband){? ??if?(abs(value) < deadband) {? ? ? ? value =?0;? ? }?else?if?(value >?0) {? ? ? ? value -= deadband;? ? }?else?if?(value <?0) {? ? ? ? value += deadband;? ? }? ??return?value;}

// 計(jì)算加速度在地球坐標(biāo)系中的分量void?acc_calc(uint32_t?deltaT){? ??static?int32_t?accZoffset =?0;? ??static?float?accz_smooth =?0;? ??float?dT =?0;? ??float?rpy[3];? ? t_fp_vector accel_ned;? ??// 計(jì)算時(shí)間間隔? ? dT = (float)deltaT *?1e-6f;? ??// 計(jì)算旋轉(zhuǎn)角度? ? rpy[0] = -(float)anglerad[ROLL];? ? rpy[1] = -(float)anglerad[PITCH];? ? rpy[2] = -(float)heading * RAD;? ??// 獲取加速度計(jì)數(shù)據(jù)? ? accel_ned.V.X = accSmooth[0];? ? accel_ned.V.Y = accSmooth[1];? ? accel_ned.V.Z = accSmooth[2];? ??// 旋轉(zhuǎn)加速度計(jì)數(shù)據(jù)? ??rotateV(&accel_ned.V, rpy);? ??// 如果啟用無(wú)武裝校準(zhǔn)? ??if?(cfg.acc_unarmedcal ==?1) {? ? ? ??if?(!f.ARMED) {? ? ? ? ? ? accZoffset -= accZoffset /?64;? ? ? ? ? ? accZoffset += accel_ned.V.Z;? ? ? ? }? ? ? ? accel_ned.V.Z -= accZoffset /?64; ?// 補(bǔ)償重力? ? }?else? ? ? ? accel_ned.V.Z -= acc_1G;? ??// 低通濾波加速度計(jì)數(shù)據(jù)? ? accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth);? ??// 應(yīng)用死區(qū)以減少積分漂移和振動(dòng)影響? ? accSum[X] +=?applyDeadband(lrintf(accel_ned.V.X), cfg.accxy_deadband);? ? accSum[Y] +=?applyDeadband(lrintf(accel_ned.V.Y), cfg.accxy_deadband);? ? accSum[Z] +=?applyDeadband(lrintf(accz_smooth), cfg.accz_deadband);? ??// 更新累積時(shí)間和計(jì)數(shù)? ? accTimeSum += deltaT;? ? accSumCount++;}

// 重置加速度計(jì)累加器void?accSum_reset(void){? ? accSum[0] =?0;? ? accSum[1] =?0;? ? accSum[2] =?0;? ? accSumCount =?0;? ? accTimeSum =?0;}

// 計(jì)算航向static int16_t calculateHeading(t_fp_vector *vec){? ? int16_t?head;? ??float?cosineRoll = cosf(anglerad[ROLL]);? ??float?sineRoll = sinf(anglerad[ROLL]);? ??float?cosinePitch = cosf(anglerad[PITCH]);? ??float?sinePitch = sinf(anglerad[PITCH]);? ??float?Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll;? ??float?Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll;? ??float?hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;? ??head?= lrintf(hd);? ??if?(head?< 0)? ? ? ??head?+= 360;? ??return?head;}

// 獲取估計(jì)的姿態(tài)static?void?getEstimatedAttitude(void){? ??int32_t?axis;? ??int32_t?accMag =?0;? ??static?t_fp_vector EstM;? ??static?t_fp_vector EstN = { .A = {?1.0f,?0.0f,?0.0f?} };? ??static?float?accLPF[3];? ??static?uint32_t?previousT;? ??uint32_t?currentT =?micros();? ??uint32_t?deltaT;? ??float?scale, deltaGyroAngle[3];? ? deltaT = currentT - previousT;? ? scale = deltaT * gyro.scale;? ? previousT = currentT;? ??// 初始化? ??for?(axis =?0; axis <?3; axis++) {? ? ? ? deltaGyroAngle[axis] = gyroADC[axis] * scale;? ? ? ??if?(cfg.acc_lpf_factor >?0) {? ? ? ? ? ? accLPF[axis] = accLPF[axis] * (1.0f?- (1.0f?/ cfg.acc_lpf_factor)) + accADC[axis] * (1.0f?/ cfg.acc_lpf_factor);? ? ? ? ? ? accSmooth[axis] = accLPF[axis];? ? ? ? }?else?{? ? ? ? ? ? accSmooth[axis] = accADC[axis];? ? ? ? }? ? ? ? accMag += (int32_t)accSmooth[axis] * accSmooth[axis];? ? }? ? accMag = accMag *?100?/ ((int32_t)acc_1G * acc_1G);? ??// 旋轉(zhuǎn)估計(jì)向量? ??rotateV(&EstG.V, deltaGyroAngle);? ??// 應(yīng)用互補(bǔ)濾波器進(jìn)行陀螺儀漂移校正? ??if?(72?< (uint16_t)accMag && (uint16_t)accMag <?133) {? ? ? ??for?(axis =?0; axis <?3; axis++)? ? ? ? ? ? EstG.A[axis] = (EstG.A[axis] * (float)mcfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;? ? }? ? f.SMALL_ANGLE = (EstG.A[Z] > smallAngle);? ??// 計(jì)算姿態(tài)角度? ? anglerad[ROLL] =?atan2f(EstG.V.Y, EstG.V.Z);? ? anglerad[PITCH] =?atan2f(-EstG.V.X,?sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));? ? angle[ROLL] =?lrintf(anglerad[ROLL] * (1800.0f?/ M_PI));? ? angle[PITCH] =?lrintf(anglerad[PITCH] * (1800.0f?/ M_PI));? ??if?(sensors(SENSOR_MAG)) {? ? ? ??rotateV(&EstM.V, deltaGyroAngle);? ? ? ??for?(axis =?0; axis <?3; axis++)? ? ? ? ? ? EstM.A[axis] = (EstM.A[axis] * (float)mcfg.gyro_cmpfm_factor + magADC[axis]) * INV_GYR_CMPFM_FACTOR;? ? ? ? heading =?calculateHeading(&EstM);? ? }?else?{? ? ? ??rotateV(&EstN.V, deltaGyroAngle);? ? ? ??normalizeV(&EstN.V, &EstN.V);? ? ? ? heading =?calculateHeading(&EstN);? ? }? ??// 旋轉(zhuǎn)加速度計(jì)數(shù)據(jù)? ??acc_calc(deltaT);? ??if?(cfg.throttle_correction_value) {? ? ? ??float?cosZ = EstG.V.Z /?sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);? ? ? ??if?(cosZ <=?0.015f) {?// 我們倒立，垂直或小角度<0.86度，0.015是弧度? ? ? ? ? ? throttleAngleCorrection =?0;? ? ? ? }?else?{? ? ? ? ? ??int?deg =?lrintf(acosf(cosZ) * throttleAngleScale);? ? ? ? ? ??if?(deg >?900)? ? ? ? ? ? ? ? deg =?900;? ? ? ? ? ? throttleAngleCorrection =?lrintf(cfg.throttle_correction_value *?sinf(deg / (900.0f?* M_PI /?2.0f)));? ? ? ? }? ? }}

#ifdef?BARO#define?UPDATE_INTERVAL 25000 ??// 40hz update rate (20hz LPF on acc)

// 獲取估計(jì)的高度int?getEstimatedAltitude(void){? ?//估算高度是綜合兩種數(shù)據(jù)，必須的氣壓計(jì)數(shù)據(jù)，可選的超聲波雷達(dá)數(shù)據(jù)，二者用PID控制算法融合? ??static?uint32_t?previousT;? ??uint32_t?currentT =?micros();? ??uint32_t?dTime;? ??int32_t?error;? ??int32_t?baroVel;? ??int32_t?vel_tmp;? ??int32_t?BaroAlt_tmp;? ??int32_t?setVel;? ??float?dt;? ??float?vel_acc;? ??float?accZ_tmp;? ??static?float?accZ_old =?0.0f;? ??static?float?vel =?0.0f;? ??static?float?accAlt =?0.0f;? ??static?int32_t?lastBaroAlt;? ??static?int32_t?baroGroundAltitude =?0;? ??static?int32_t?baroGroundPressure =?0;? ??int16_t?tiltAngle =?max(abs(angle[ROLL]),?abs(angle[PITCH]));? ??// 計(jì)算時(shí)間間隔? ? dTime = currentT - previousT;? ??if?(dTime < UPDATE_INTERVAL)? ? ? ??return?0;? ? previousT = currentT;? ??if?(calibratingB >?0) {? ? ? ??// 校準(zhǔn)氣壓計(jì)? ? ? ? baroGroundPressure -= baroGroundPressure /?8;? ? ? ? baroGroundPressure += baroPressureSum / (cfg.baro_tab_size -?1);? ? ? ? baroGroundAltitude = (1.0f?-?powf((baroGroundPressure /?8) /?101325.0f,?0.190295f)) *?4433000.0f;? ? ? ? vel =?0;? ? ? ? accAlt =?0;? ? ? ? calibratingB--;? ? }? ??// 計(jì)算地面高度? ? BaroAlt_tmp =?lrintf((1.0f?-?powf((float)(baroPressureSum / (cfg.baro_tab_size -?1)) /?101325.0f,?0.190295f)) *?4433000.0f);?// in cm? ? BaroAlt_tmp -= baroGroundAltitude;? ? BaroAlt =?lrintf((float)BaroAlt * cfg.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f?- cfg.baro_noise_lpf));?// additional LPF to reduce baro noise? ??// 計(jì)算超聲波高度? ??if?(tiltAngle >?250)? ? ? ? sonarAlt =?-1;? ??else? ? ? ? sonarAlt = sonarAlt * (900.0f?- tiltAngle) /?900.0f;? ??// 超聲波高度和氣壓計(jì)高度融合? ??if?(sonarAlt >?0?&& sonarAlt <?200) {? ? ? ? baroAlt_offset = BaroAlt - sonarAlt;? ? ? ? BaroAlt = sonarAlt;? ? }?else?{?? ? ? ? BaroAlt -= baroAlt_offset;? ? ? ??if?(sonarAlt >?0) {? ? ? ? ? ? sonarTransition = (300?- sonarAlt) /?100.0f;? ? ? ? ? ? BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f?- sonarTransition);? ? ? ? }? ? }? ? dt = accTimeSum *?1e-6f;?// delta acc reading time in seconds? ??// 積分 - 速度，cm/s? ? accZ_tmp = (float)accSum[2] / (float)accSumCount;? ? vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;? ??// 積分 - 高度，cm? ? accAlt += (vel_acc *?0.5f) * dt + vel * dt; ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??// integrate velocity to get distance (x= a/2 * t^2)? ? accAlt = accAlt * cfg.baro_cf_alt + (float)BaroAlt * (1.0f?- cfg.baro_cf_alt); ? ? ?// complementary filter for altitude estimation (baro & acc)//這里的PID只進(jìn)行了一輪，沒(méi)有反復(fù)迭代，所以是開(kāi)環(huán)控制而非閉環(huán)控制? ??// 當(dāng)超聲波在最佳范圍內(nèi)時(shí)? ??if?(sonarAlt >?0?&& sonarAlt <?200)?//200cm,大多數(shù)超聲波傳感器，如 HC-SR04，其測(cè)距范圍通常以厘米為單位，并且典型的測(cè)距范圍是從 2 厘米到 400 厘米?? ? ? ? EstAlt = BaroAlt;? ??else? ? ? ? EstAlt = accAlt;? ? vel += vel_acc;#if?0? ? debug[0] = accSum[2] / accSumCount;?// acceleration? ? debug[1] = vel; ? ? ? ? ? ? ? ? ? ??// velocity? ? debug[2] = accAlt; ? ? ? ? ? ? ? ? ?// height#endif? ??// 重置加速度計(jì)累加器? ??accSum_reset();? ??// 計(jì)算氣壓計(jì)速度? ? baroVel = (BaroAlt - lastBaroAlt) *?1000000.0f?/ dTime;? ? lastBaroAlt = BaroAlt;? ??// 限制氣壓計(jì)速度? ? baroVel =?constrain(baroVel,?-1500,?1500);? ??// 應(yīng)用死區(qū)以減少噪聲? ? baroVel =?applyDeadband(baroVel,?10);? ??// 應(yīng)用互補(bǔ)濾波器保持基于氣壓計(jì)速度的計(jì)算速度（即近似真實(shí)速度）? ? vel = vel * cfg.baro_cf_vel + baroVel * (1?- cfg.baro_cf_vel);? ? vel_tmp =?lrintf(vel);? ??// 設(shè)置變速率? ? vario =?applyDeadband(vel_tmp,?5);? ??// 如果推力朝下（<80度）? ??if?(tiltAngle <?800) {?//這里也是一次性PID，用于倒飛時(shí)，倒飛時(shí)墜地速度遠(yuǎn)比正飛快，所以倒飛的高度單獨(dú)估算? ? ? ??// 計(jì)算高度P控制器? ? ? ??if?(!velocityControl) {? ? ? ? ? ? error =?constrain(AltHold - EstAlt,?-500,?500);? ? ? ? ? ? error =?applyDeadband(error,?10); ? ? ??// 去除小P參數(shù)以減少接近零位置的噪聲? ? ? ? ? ? setVel =?constrain((cfg.P8[PIDALT] * error /?128),?-300, +300);?// 限制速度為+/-3m/s? ? ? ? }?else?{? ? ? ? ? ? setVel = setVelocity;? ? ? ? }? ? ? ??// 計(jì)算速度PID控制器? ? ? ??// P? ? ? ? error = setVel - vel_tmp;? ? ? ? BaroPID =?constrain((cfg.P8[PIDVEL] * error /?32),?-300, +300);? ? ? ??// I? ? ? ? errorVelocityI += (cfg.I8[PIDVEL] * error);? ? ? ? errorVelocityI =?constrain(errorVelocityI, -(8196?*?200), (8196?*?200));? ? ? ? BaroPID += errorVelocityI /?8196; ? ??// I在+/-200范圍內(nèi)? ? ? ??// D? ? ? ? BaroPID -=?constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) /?512,?-150,?150);? ? }?else?{? ? ? ? BaroPID =?0;? ? }? ? accZ_old = accZ_tmp;? ??return?1;}#endif?/* BARO */

03、廠商、工程師和創(chuàng)業(yè)者在做什么？

除非自己研發(fā)硬件，否則主要就是買(mǎi)來(lái)各種硬件，組裝在一起，然后燒入自己修改的固件（不限于baseflight），里面有自己開(kāi)發(fā)的新算法，或者有你改進(jìn)的參數(shù)。

一般情況下，除了底層算法外，CLI（命令行）等等交互模塊都會(huì)被修改。如何根據(jù)各種硬件的性價(jià)比，搭配出最優(yōu)性價(jià)比的硬件系統(tǒng)，然后就是如何讓自己的軟件系統(tǒng)能夠發(fā)揮出這套硬件最佳性能，使得產(chǎn)品體現(xiàn)出最高的性價(jià)比。

比如一千五百元的機(jī)型性能幾乎和別家五千元產(chǎn)品就絕大多數(shù)場(chǎng)景能性能持平，這就是成功。再加上測(cè)試和用戶反饋，這就是研發(fā)工作的全部。