BetaFlight深入传感设计之三:IMU传感模块
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: CW0FLIPgyro_1_align_roll = 0
Allowed range: -3600 - 3600gyro_1_align_pitch = 0
Allowed range: -3600 - 3600
Default value: 1800gyro_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算法,对飞控的姿态进行更新。
注:看来有必要学习下该理论,否则会在原理方面被卡住。
补充:BetaFlight深入传感设计之五:MahonyAHRS & 方向余弦矩阵理论
4. 总结
gyro & acc的传感器更多涉及航模飞行姿态问题,gps/mag/baro涉及的是位置及导航问题,这些是飞控必要的基本传感器。
- BetaFlight深入传感设计之一:Baro传感模块
- BetaFlight深入传感设计之二:Mag传感模块
- BetaFlight深入传感设计之三:IMU传感模块
- BetaFlight深入传感设计之四:GPS传感模块
关于其他更多辅助传感器,无非是为了更多智能化功能,比如:避障(动态,静态)等等。
从技术的角度,飞控姿态逻辑是控制面非常重要的一环,后续我们将重点突破这一环节,以期望在飞控控制面逻辑能够更进一步。
5. 参考资料
【1】BetaFlight深入传感设计:传感模块设计框架
【2】BetaFlight模块设计之十一:Gyro&Acc任务分析
【3】BetaFlight模块设计之二十七:姿态更新任务分析