庖丁解牛 | Baseflight中惯导模块(IMU.c)的代码解读

Baseflight是十分新手友好的，其中一点就体现在每个模块的文件平均不愁过500行。（请注意是平均，确实存在几个八九百行或者上千行的文件）

软件工程中，几乎一切困难都是规模造成的。随着代码规模的增长，代码的复杂度是几何级数增加，而不是线性增长。也就是代码长度翻一倍，理解和开发维护的复杂度翻四倍而不是两倍。大型代码库往往包含多个模块、类和函数，这些组件之间的交互变得越来越复杂，导致开发者难以理解和维护代码。特别是在缺乏良好的文档和注释的情况下，新加入的开发人员可能需要花费大量时间来理解现有代码的功能和结构。

所以老手一般会选择一个足够小但功能全的工程入手，快速理清全局脉络。后面添加自己的功能或者读懂其他人添加的功能就很容易了。

01、imu模块的全览

imu模块并不是自成一体的模块，而更像一个工具库，供其它模块调用。

这里所说的其他模块就是main.c拉起来的无限循环while(1)里面的loop（）函数。loop是整个项目最大的函数，其实现在mx.c中（mx代表main work，意思是主要干活的地方），值得以后单独解析。

无人机飞行就是不停执行loop，每一次都感知环境+接收指令，然后变为给四个电机的本loop的转速和方向（一般是占空比信号PWM）并执行。飞机就这样不停地飞起来了。直到飞机关机形成硬中断来退出main.c拉起来的无限循环while(1),这就是关机了。

图1 imu中函数的全列表

查看调用关系，实际上只有两个函数是整个imu模块的出口

getEstimatedAttitude 获取飞行器姿态估计

getEstimatedAltitude 获取飞行器高度估计

其余函数都是模块内部函数被这两位调用的，这两位再被loop调用，用于每一轮飞行中先估计姿态和高度。而且注意，获取高度的函数getEstimatedAltitude是在预定义判断的ifdef BARO里面，BARO是气压计的意思，也就是说没有气压计无法估算高度，高度函数根本就不执行。

02、imu模块的源码和详细注释

本文采取（几乎）一行一注释的形式来解读代码，英文注释是代码原有的，比较少，不足以解释，中文注释是笔者加的，解释性较强。这种超长注释只是为了解释，并不符合良好的代码规范。

#include "board.h"#include "mw.h"// 存储来自陀螺仪的原始数据int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];// 存储加速度计数据的累积和，长度3是因为xyzint32_t accSum[3];// 累积加速度计数据的时间总和uint32_t accTimeSum = 0;// 加速度计数据的累积次数int accSumCount = 0;// 小角度值int16_t smallAngle = 0;// 气压计压力int32_t baroPressure = 0;// 气压计温度int32_t baroTemperature = 0;// 气压计压力的累积和uint32_t baroPressureSum = 0;// 气压计高度int32_t BaroAlt = 0;// 超声波转换系数float sonarTransition = 0;// 气压计高度偏移int32_t baroAlt_offset = 0;// 超声波测距高度int32_t sonarAlt = -1;         // in cm , -1 indicate sonar is not in range// 估计的高度int32_t EstAlt;                // 气压计PID控制器输出int32_t BaroPID = 0;// 高度设定值int32_t AltHold;// 设置速度int32_t setVelocity = 0;// 速度控制标志uint8_t velocityControl = 0;// 速度积分误差int32_t errorVelocityI = 0;// 变速率（垂直速度）int32_t vario = 0;                      // 油门角度修正int16_t throttleAngleCorrection = 0;    // 磁偏角float magneticDeclination = 0.0f;       // 加速度计速度比例因子float accVelScale;// 油门角度比例因子float throttleAngleScale;// 加速度计时间常数float fc_acc;// 存储来自陀螺仪的原始数据int16_t gyroData[3] = { 0, 0, 0 };// 存储陀螺仪零点int16_t gyroZero[3] = { 0, 0, 0 };// 绝对角度倾角（单位为0.1度）int16_t angle[2] = { 0, 0 };// 绝对角度倾角（单位为弧度）float anglerad[2] = { 0.0f, 0.0f };// 获取估计姿态的函数static void getEstimatedAttitude(void);

// 初始化IMU相关参数void imuInit(void){    // 计算小角度值    smallAngle = lrintf(acc_1G * cosf(RAD * cfg.small_angle));    /* lrintf多次用到，是一个数值转换函数，不是打印函数    lrintf 是 C 语言标准库中的一个数学函数，用于将单精度浮点数（float 类型）舍入为最接近的整数值，并将其转换为 long int 类型。一般向下取整*/    // 计算加速度计速度比例因子    accVelScale = 9.80665f / acc_1G / 10000.0f;    // 计算油门角度比例因子    throttleAngleScale = (1800.0f / M_PI) * (900.0f / cfg.throttle_correction_angle);    // 计算加速度计Z轴低通滤波器的时间常数    fc_acc = 0.5f / (M_PI * cfg.accz_lpf_cutoff);#ifdef MAG    // 如果启用磁力计，初始化磁力计    if (sensors(SENSOR_MAG))        Mag_init();#endif}

// 计算IMU数据void computeIMU(void){    static int16_t gyroYawSmooth = 0;    // 获取陀螺仪数据    Gyro_getADC();    // 如果启用加速度计，获取加速度计数据并计算姿态    if (sensors(SENSOR_ACC)) {        ACC_getADC();        getEstimatedAttitude();    } else {        // 如果未启用加速度计，将加速度计数据设为0        accADC[X] = 0;        accADC[Y] = 0;        accADC[Z] = 0;    }    // 如果混合配置为三旋翼，平滑陀螺仪偏航数据    if (mcfg.mixerConfiguration == MULTITYPE_TRI) {        gyroData[YAW] = (gyroYawSmooth * 2 + gyroADC[YAW]) / 3;        gyroYawSmooth = gyroData[YAW];    } else {        // 否则直接使用陀螺仪偏航数据        gyroData[YAW] = gyroADC[YAW];    }    // 直接使用陀螺仪滚转和俯仰数据    gyroData[ROLL] = gyroADC[ROLL];    gyroData[PITCH] = gyroADC[PITCH];}// 使用互补滤波器简化IMU计算#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))/*互补滤波器的工作原理其实就是人为根据信号特点对不同信号做不同原则的滤波互补滤波器通常应用于惯性测量单元（IMU）的姿态估计中，结合加速度计和陀螺仪的数据来计算物体的姿态角。具体来说：1.加速度计：能够提供静态姿态信息，但容易受到高频噪声的影响。2.陀螺仪：能够提供动态姿态变化信息，但长时间积分会导致漂移误差。为了克服各自的缺点，互补滤波器采用以下策略：·对于加速度计，使用低通滤波器（Low-Pass Filter, LPF）去除高频噪声，保留低频部分。·对于陀螺仪，使用高通滤波器（High-Pass Filter, HPF）去除低频漂移，保留高频变化。*/

// 定义浮点向量结构体typedef struct fp_vector {    float X;    float Y;    float Z;} t_fp_vector_def;// 定义浮点向量联合体typedef union {    float A[3];    t_fp_vector_def V;} t_fp_vector;// 存储估计的姿态向量t_fp_vector EstG;

// 归一化向量void normalizeV(struct fp_vector *src, struct fp_vector *dest){    float length;    // 计算向量长度    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;    }}

// 根据陀螺仪数据旋转估计向量void rotateV(struct fp_vector *v, float *delta){    struct fp_vector v_tmp = *v;    // 计算旋转矩阵元素    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 }    };    // 应用旋转矩阵    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];}

// 应用死区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;}

// 计算加速度在地球坐标系中的分量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;    // 计算时间间隔    dT = (float)deltaT * 1e-6f;    // 计算旋转角度    rpy[0] = -(float)anglerad[ROLL];    rpy[1] = -(float)anglerad[PITCH];    rpy[2] = -(float)heading * RAD;    // 获取加速度计数据    accel_ned.V.X = accSmooth[0];    accel_ned.V.Y = accSmooth[1];    accel_ned.V.Z = accSmooth[2];    // 旋转加速度计数据    rotateV(&accel_ned.V, rpy);    // 如果启用无武装校准    if (cfg.acc_unarmedcal == 1) {        if (!f.ARMED) {            accZoffset -= accZoffset / 64;            accZoffset += accel_ned.V.Z;        }        accel_ned.V.Z -= accZoffset / 64;  // 补偿重力    } else        accel_ned.V.Z -= acc_1G;    // 低通滤波加速度计数据    accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth);    // 应用死区以减少积分漂移和振动影响    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);    // 更新累积时间和计数    accTimeSum += deltaT;    accSumCount++;}

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

// 计算航向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;}

// 获取估计的姿态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);    // 旋转估计向量    rotateV(&EstG.V, deltaGyroAngle);    // 应用互补滤波器进行陀螺仪漂移校正    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);    // 计算姿态角度    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);    }    // 旋转加速度计数据    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)

// 获取估计的高度int getEstimatedAltitude(void){   //估算高度是综合两种数据，必须的气压计数据，可选的超声波雷达数据，二者用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]));    // 计算时间间隔    dTime = currentT - previousT;    if (dTime < UPDATE_INTERVAL)        return 0;    previousT = currentT;    if (calibratingB > 0) {        // 校准气压计        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--;    }    // 计算地面高度    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    // 计算超声波高度    if (tiltAngle > 250)        sonarAlt = -1;    else        sonarAlt = sonarAlt * (900.0f - tiltAngle) / 900.0f;    // 超声波高度和气压计高度融合    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只进行了一轮，没有反复迭代，所以是开环控制而非闭环控制    // 当超声波在最佳范围内时    if (sonarAlt > 0 && sonarAlt < 200) //200cm,大多数超声波传感器，如 HC-SR04，其测距范围通常以厘米为单位，并且典型的测距范围是从 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    // 重置加速度计累加器    accSum_reset();    // 计算气压计速度    baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;    lastBaroAlt = BaroAlt;    // 限制气压计速度    baroVel = constrain(baroVel, -1500, 1500);    // 应用死区以减少噪声    baroVel = applyDeadband(baroVel, 10);    // 应用互补滤波器保持基于气压计速度的计算速度（即近似真实速度）    vel = vel * cfg.baro_cf_vel + baroVel * (1 - cfg.baro_cf_vel);    vel_tmp = lrintf(vel);    // 设置变速率    vario = applyDeadband(vel_tmp, 5);    // 如果推力朝下（<80度）    if (tiltAngle < 800) { //这里也是一次性PID，用于倒飞时，倒飞时坠地速度远比正飞快，所以倒飞的高度单独估算        // 计算高度P控制器        if (!velocityControl) {            error = constrain(AltHold - EstAlt, -500, 500);            error = applyDeadband(error, 10);       // 去除小P参数以减少接近零位置的噪声            setVel = constrain((cfg.P8[PIDALT] * error / 128), -300, +300); // 限制速度为+/-3m/s        } else {            setVel = setVelocity;        }        // 计算速度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范围内        // D        BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 512, -150, 150);    } else {        BaroPID = 0;    }    accZ_old = accZ_tmp;    return 1;}#endif /* BARO */