BetaFlight深入传感设计之三：IMU传感模块

Posted 2022-10-12 lida2003

tags:

篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了BetaFlight深入传感设计之三：IMU传感模块相关的知识，希望对你有一定的参考价值。

BetaFlight深入传感设计之三：IMU传感模块

1. HwPreInit/HwInit阶段
2. HwIo阶段
3. HwDataAnalysis阶段
- 3.1 Calibration
- 3.2 Attitude Caclulation
4. 总结
5. 参考资料

IMU传感器内部集成3个16bit ADC gyroscope数据和3个16bit ADC accelerometer数据，主要用于运动姿态和加速度检测。

gyro检测范围：°/sec (dps)

±250
±500
±1000
±2000

acc检测范围：g

±2
±4
±8
±16

注：上述规格主要来自MPU60X0系列传感器，笔者这里主要介绍的是MPU6000（经典款）。

根据BetaFlight深入传感设计：传感模块设计框架，我们针对如下几个阶段进行分析。

1. HwPreInit/HwInit阶段

1.1 【业务HwPreInit】gyroPreInit

该阶段对SPI片选信号脚进行了硬件配置(仅当代码宏定义支持SPI的情况下)。

gyroPreInit
 ├──> gyroPreInitSensor(gyroDeviceConfig(0))
 └──> <USE_MULTI_GYRO>
     └──> gyroPreInitSensor(gyroDeviceConfig(1))

gyroPreInitSensor
 └──> <USE_SPI_GYRO>
     └──> spiPreinitRegister(config->csnTag, IOCFG_IPU, 1)

1.2 【业务HwInit】gyroInit & accInit

1.2.1 gyroInit

业务初始化阶段主要涉及：

gyroInitSensor //初始化(类似个打包函数)
gyroDetect //传感芯片侦测
buildRotationMatrixFromAlignment //传感芯片方向对齐
gyroInitSensorFilters //传感滤波器初始化

gyroInit
 ├──> [外围参数初始化]
 ├──> gyroDetectSensor                  //调用gyroDetect自动侦测gyro芯片
 ├──> [gyro应用配置：1)gyro1; 2)gyro2; 3)gyro1 + gyro2, 仅当两颗gyro硬件类型一致的情况(类型不一致的时候采用gyro1)]
 ├──> gyroInitSensor                    //根据gyro应用配置进行初始化
 └──> [gyro & acc 采样频率设置]

gyroInitSensor
 ├──> buildRotationMatrixFromAlignment  //构建一个传感芯片的方向对齐矩阵
 ├──> gyroSensor->gyroDev.initFn        //传感芯片初始化
 └──> gyroInitSensorFilters             //传感滤波器初始化

1.2.2 accInit

业务初始化阶段主要涉及：

buildRotationMatrixFromAlignment //传感芯片方向对齐
accDetect //传感芯片侦测
accInitFilters //传感滤波器初始化

accInit
 ├──> buildRotationMatrixFromAlignment  //构建一个传感芯片的方向对齐矩阵
 ├──> accDetect                         //自动侦测acc芯片
 ├──> acc.dev.initFn                    //传感芯片初始化
 └──> accInitFilters                    //传感滤波器初始化

1.2.3 gyroDetect

目前，支持以下硬件规格gyro传感器：

GYRO_MPU6050
GYRO_L3G4200D
GYRO_MPU3050
GYRO_L3GD20
GYRO_MPU6000
GYRO_MPU6500
GYRO_MPU9250
GYRO_ICM20601
GYRO_ICM20602
GYRO_ICM20608G
GYRO_ICM20649
GYRO_ICM20689
GYRO_ICM42605
GYRO_ICM42688P
GYRO_BMI160
GYRO_BMI270
GYRO_LSM6DSO

根据BetaFlight深入传感设计：传感模块设计框架，下面以MPU6000为例：

mpu6000SpiGyroInit
mpuGyroReadSPI

bool mpu6000SpiGyroDetect(gyroDev_t *gyro)

    if (gyro->mpuDetectionResult.sensor != MPU_60x0_SPI) 
        return false;
    

    gyro->initFn = mpu6000SpiGyroInit;
    gyro->readFn = mpuGyroReadSPI;
    gyro->scale = GYRO_SCALE_2000DPS;
#ifdef USE_GYRO_EXTI
    gyro->gyroShortPeriod = clockMicrosToCycles(MPU6000_SHORT_THRESHOLD);
#endif
    return true;

1.2.4 accDetect

目前，支持以下硬件规格acc传感器：

ACC_ADXL345
ACC_MPU6050
ACC_MMA8452
ACC_BMA280
ACC_LSM303DLHC
ACC_MPU6000
ACC_MPU6500
ACC_MPU9250
ACC_ICM20601
ACC_ICM20602
ACC_ICM20608G
ACC_ICM20649
ACC_ICM20689
ACC_ICM42605
ACC_ICM42688P
ACC_BMI160
ACC_BMI270
ACC_LSM6DSO

根据BetaFlight深入传感设计：传感模块设计框架，下面以MPU6000为例：

mpu6000SpiAccInit
mpuAccReadSPI

bool mpu6000SpiAccDetect(accDev_t *acc)

    if (acc->mpuDetectionResult.sensor != MPU_60x0_SPI) 
        return false;
    

    acc->initFn = mpu6000SpiAccInit;
    acc->readFn = mpuAccReadSPI;

    return true;

1.2.5 buildRotationMatrixFromAlignment

根据配置页面或者 cli配置参数，建立旋转矩阵，将芯片检测到的姿态和加速度方向旋转到与飞机一致的方向。

注：gyro和acc采用同样的cli配置参数，gyro和acc通常是同一颗芯片。

或者从cli获取相关gyro_1_align参数。

gyro_1_sensor_align = CW270
Allowed values: DEFAULT, CW0, CW90, CW180, CW270, CW0FLIP, CW90FLIP, CW180FLIP, CW270FLIP, CUSTOM
Default value: CW0FLIP

gyro_1_align_roll = 0
Allowed range: -3600 - 3600

gyro_1_align_pitch = 0
Allowed range: -3600 - 3600
Default value: 1800

gyro_1_align_yaw = 2700
Allowed range: -3600 - 3600
Default value: 0

void buildRotationMatrixFromAlignment(const sensorAlignment_t* sensorAlignment, fp_rotationMatrix_t* rm)
 ├──> rotationAngles.angles.roll  = DECIDEGREES_TO_RADIANS(sensorAlignment->roll)
 ├──> rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(sensorAlignment->pitch)
 ├──> rotationAngles.angles.yaw   = DECIDEGREES_TO_RADIANS(sensorAlignment->yaw)
 └──> buildRotationMatrix(&rotationAngles, rm)
 
void buildRotationMatrix(fp_angles_t *delta, fp_rotationMatrix_t *rotation)

    float cosx, sinx, cosy, siny, cosz, sinz
    float coszcosx, sinzcosx, coszsinx, sinzsinx

    cosx = cos_approx(delta->angles.roll)
    sinx = sin_approx(delta->angles.roll)
    cosy = cos_approx(delta->angles.pitch)
    siny = sin_approx(delta->angles.pitch)
    cosz = cos_approx(delta->angles.yaw)
    sinz = sin_approx(delta->angles.yaw)

    coszcosx = cosz * cosx
    sinzcosx = sinz * cosx
    coszsinx = sinx * cosz
    sinzsinx = sinx * sinz

    rotation->m[0][X] = cosz * cosy
    rotation->m[0][Y] = -cosy * sinz
    rotation->m[0][Z] = siny
    rotation->m[1][X] = sinzcosx + (coszsinx * siny)
    rotation->m[1][Y] = coszcosx - (sinzsinx * siny)
    rotation->m[1][Z] = -sinx * cosy
    rotation->m[2][X] = (sinzsinx) - (coszcosx * siny)
    rotation->m[2][Y] = (coszsinx) + (sinzcosx * siny)
    rotation->m[2][Z] = cosy * cosx


typedef union sensorAlignment_u 
    // value order is the same as axis_e

    // values are in DECIDEGREES, and should be limited to +/- 3600

    int16_t raw[XYZ_AXIS_COUNT]
    struct 
        int16_t roll
        int16_t pitch
        int16_t yaw
    
 sensorAlignment_t

#define SENSOR_ALIGNMENT(ROLL, PITCH, YAW) ((sensorAlignment_t)\\
    .roll = DEGREES_TO_DECIDEGREES(ROLL), \\
    .pitch = DEGREES_TO_DECIDEGREES(PITCH), \\
    .yaw = DEGREES_TO_DECIDEGREES(YAW), \\
)

1.3 MPU6000 芯片驱动

1.3.1 mpu6000SpiGyroInit & mpu6000SpiAccInit

其实MPU6000物理上是同一个芯片，虽然逻辑概念上是两个初始化，但实际上芯片频率等设置在mpu6000SpiGyroInit 都已经完成。

mpu6000SpiGyroInit
 ├──> mpuGyroInit(gyro)   //外部中断初始化中断函数mpuIntExtiHandler
 ├──> mpu6000AccAndGyroInit(gyro) //时钟、通信口、频率、采样范围设置
 ├──> spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6000_MAX_SPI_INIT_CLK_HZ))
 ├──> spiWriteReg(&gyro->dev, MPU6000_CONFIG, mpuGyroDLPF(gyro)) // Accel and Gyro DLPF Setting, 目前都是0 for 8K
 ├──> delayMicroseconds(1)
 ├──> spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6000_MAX_SPI_CLK_HZ))
 ├──> mpuGyroRead(gyro)
 └──> <((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1>
     └──> failureMode(FAILURE_GYRO_INIT_FAILED)

mpu6000SpiAccInit
 └──> acc->acc_1G = 512 * 4

芯片手册图片需要补充

1.3.2 mpuGyroReadSPI

bool mpuGyroReadSPI(gyroDev_t *gyro)

    uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf;
    switch (gyro->gyroModeSPI) 
    case GYRO_EXTI_INIT:
    
        // Initialise the tx buffer to all 0xff
        memset(gyro->dev.txBuf, 0xff, 16);
#ifdef USE_GYRO_EXTI
        // Check that minimum number of interrupts have been detected

        // We need some offset from the gyro interrupts to ensure sampling after the interrupt
        gyro->gyroDmaMaxDuration = 5;
        if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) 
            if (spiUseDMA(&gyro->dev)) 
                gyro->dev.callbackArg = (uint32_t)gyro;
                gyro->dev.txBuf[0] = gyro->accDataReg | 0x80;
                gyro->segments[0].len = gyro->gyroDataReg - gyro->accDataReg + 7;
                gyro->segments[0].callback = mpuIntcallback;
                gyro->segments[0].u.buffers.txData = gyro->dev.txBuf;
                gyro->segments[0].u.buffers.rxData = &gyro->dev.rxBuf[1];
                gyro->segments[0].negateCS = true;
                gyro->gyroModeSPI = GYRO_EXTI_INT_DMA;
             else 
                // Interrupts are present, but no DMA
                gyro->gyroModeSPI = GYRO_EXTI_INT;
            
         else
#endif
        
            gyro->gyroModeSPI = GYRO_EXTI_NO_INT;
        
        break;
    

    case GYRO_EXTI_INT:
    case GYRO_EXTI_NO_INT:
    
        gyro->dev.txBuf[0] = gyro->gyroDataReg | 0x80;

        busSegment_t segments[] = 
                .u.buffers = NULL, NULL, 7, true, NULL,
                .u.link = NULL, NULL, 0, true, NULL,
        ;
        segments[0].u.buffers.txData = gyro->dev.txBuf;
        segments[0].u.buffers.rxData = &gyro->dev.rxBuf[1];

        spiSequence(&gyro->dev, &segments[0]);

        // Wait for completion
        spiWait(&gyro->dev);

        gyro->gyroADCRaw[X] = __builtin_bswap16(gyroData[1]);
        gyro->gyroADCRaw[Y] = __builtin_bswap16(gyroData[2]);
        gyro->gyroADCRaw[Z] = __builtin_bswap16(gyroData[3]);
        break;
    

    case GYRO_EXTI_INT_DMA:
    
        // Acc and gyro data may not be continuous (MPU6xxx has temperature in between)
        const uint8_t gyroDataIndex = ((gyro->gyroDataReg - gyro->accDataReg) >> 1) + 1;

        // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
        // up an old value.
        gyro->gyroADCRaw[X] = __builtin_bswap16(gyroData[gyroDataIndex]);
        gyro->gyroADCRaw[Y] = __builtin_bswap16(gyroData[gyroDataIndex + 1]);
        gyro->gyroADCRaw[Z] = __builtin_bswap16(gyroData[gyroDataIndex + 2]);
        break;
    

    default:
        break;
    

    return true;

1.3.3 mpuAccReadSPI

bool mpuAccReadSPI(accDev_t *acc)

    switch (acc->gyro->gyroModeSPI) 
    case GYRO_EXTI_INT:
    case GYRO_EXTI_NO_INT:
    
        acc->gyro->dev.txBuf[0] = acc->gyro->accDataReg | 0x80;

        busSegment_t segments[] = 
                .u.buffers = NULL, NULL, 7, true, NULL,
                .u.link = NULL, NULL, 0, true, NULL,
        ;
        segments[0].u.buffers.txData = acc->gyro->dev.txBuf;
        segments[0].u.buffers.rxData = &acc->gyro->dev.rxBuf[1];

        spiSequence(&acc->gyro->dev, &segments[0]);

        // Wait for completion
        spiWait(&acc->gyro->dev);

        // Fall through
        FALLTHROUGH;
    

    case GYRO_EXTI_INT_DMA:
    
        // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
        // up an old value.

        // This data was read from the gyro, which is the same SPI device as the acc
        uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf;
        acc->ADCRaw[X] = __builtin_bswap16(accData[1]);
        acc->ADCRaw[Y] = __builtin_bswap16(accData[2]);
        acc->ADCRaw[Z] = __builtin_bswap16(accData[3]);
        break;
    

    case GYRO_EXTI_INIT:
    default:
        break;
    

    return true;

2. HwIo阶段

详见：BetaFlight模块设计之十一：Gyro&Acc任务分析

taskGyroSample
 └──> gyroUpdate
     └──> gyroUpdateSensor

taskUpdateAccelerometer
 └──> accUpdate

3. HwDataAnalysis阶段

3.1 Calibration

【决策】isGyroSensorCalibrationComplete

processRcStickPositions
 └──> 【业务】gyroStartCalibration
     └──> gyroSetCalibrationCycles

init
 └──> 【业务】gyroStartCalibration
     └──> gyroSetCalibrationCycles

taskGyroSample
 └──> gyroUpdate
     └──> gyroUpdateSensor
         └──> 【业务】performGyroCalibration

3.2 Attitude Caclulation

详见：BetaFlight模块设计之二十七：姿态更新任务分析

imuUpdateAttitude
 └──> imuCalculateEstimatedAttitude
     ├──> [Gyro & Acc update]
     ├──> [Mag update]
     ├──> imuMahonyAHRSupdate
     └──> imuUpdateEulerAngles

最终的问题还是和BetaFlight深入传感设计之二：Mag传感模块类似，归结到Mahony算法，对飞控的姿态进行更新。

注：看来有必要学习下该理论，否则会在原理方面被卡住。

4. 总结

gyro & acc的传感器更多涉及航模飞行姿态问题，gps/mag/baro涉及的是位置及导航问题，这些是飞控必要的基本传感器。

关于其他更多辅助传感器，无非是为了更多智能化功能，比如：避障(动态，静态)等等。

从技术的角度，飞控姿态逻辑是控制面非常重要的一环，后续我们将重点突破这一环节，以期望在飞控控制面逻辑能够更进一步。

5. 参考资料

【1】BetaFlight深入传感设计：传感模块设计框架
【2】BetaFlight模块设计之十一：Gyro&Acc任务分析
【3】BetaFlight模块设计之二十七：姿态更新任务分析

以上是关于BetaFlight深入传感设计之三：IMU传感模块的主要内容，如果未能解决你的问题，请参考以下文章

BetaFlight深入传感设计：传感模块设计框架

BetaFlight深入传感设计之二：Mag传感模块

BetaFlight深入传感设计之四：GPS传感模块

BetaFlight深入传感设计之一：Baro传感模块