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

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

  • gyro检测范围:°/sec (dps)
  1. ±250
  2. ±500
  3. ±1000
  4. ±2000
  • acc检测范围:g
  1. ±2
  2. ±4
  3. ±8
  4. ±16

注:上述规格主要来自MPU60X0系列传感器,笔者这里主要介绍的是MPU6000(经典款)。

Chip & Physical characteristics

根据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

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

  1. gyroInitSensor //初始化(类似个打包函数)
  2. gyroDetect //传感芯片侦测
  3. buildRotationMatrixFromAlignment //传感芯片方向对齐
  4. 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

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

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

1.2.3 gyroDetect

目前,支持以下硬件规格gyro传感器:

  1. GYRO_MPU6050
  2. GYRO_L3G4200D
  3. GYRO_MPU3050
  4. GYRO_L3GD20
  5. GYRO_MPU6000
  6. GYRO_MPU6500
  7. GYRO_MPU9250
  8. GYRO_ICM20601
  9. GYRO_ICM20602
  10. GYRO_ICM20608G
  11. GYRO_ICM20649
  12. GYRO_ICM20689
  13. GYRO_ICM42605
  14. GYRO_ICM42688P
  15. GYRO_BMI160
  16. GYRO_BMI270
  17. 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传感器:

  1. ACC_ADXL345
  2. ACC_MPU6050
  3. ACC_MMA8452
  4. ACC_BMA280
  5. ACC_LSM303DLHC
  6. ACC_MPU6000
  7. ACC_MPU6500
  8. ACC_MPU9250
  9. ACC_ICM20601
  10. ACC_ICM20602
  11. ACC_ICM20608G
  12. ACC_ICM20649
  13. ACC_ICM20689
  14. ACC_ICM42605
  15. ACC_ICM42688P
  16. ACC_BMI160
  17. ACC_BMI270
  18. 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通常是同一颗芯片。

Gyro alignment

或者 从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 芯片驱动

MPU Introduction
MPU-60X0 capability
MPU & FIFO

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

43

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

3B

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算法,对飞控的姿态进行更新。

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

补充:BetaFlight深入传感设计之五:MahonyAHRS & 方向余弦矩阵理论

4. 总结

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

  1. BetaFlight深入传感设计之一:Baro传感模块
  2. BetaFlight深入传感设计之二:Mag传感模块
  3. BetaFlight深入传感设计之三:IMU传感模块
  4. BetaFlight深入传感设计之四:GPS传感模块

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

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

5. 参考资料

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

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值