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

Posted 2022-05-30 lida2003

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

• IMU硬件配置
• Gyro任务
• • taskGyroSample函数分析
  • gyroUpdate函数分析
• Acc任务
• • taskUpdateAccelerometer函数分析
  • accUpdate函数分析

基于BetaFlight开源代码框架简介的框架设计，逐步分析内部模块功能设计。

IMU硬件配置

KAKUTEF7 AIO硬件配置，详见\\src\\main\\target\\KAKUTEF7\\target.h; 手头的板子是ICM20689硬件。

#define USE_ACC
#define USE_GYRO
#define USE_EXTI

// ICM-20689
#define USE_ACC_SPI_ICM20689
#define USE_GYRO_SPI_ICM20689
#define GYRO_1_CS_PIN           SPI4_NSS_PIN
#define GYRO_1_SPI_INSTANCE     SPI4
#define GYRO_1_ALIGN            CW270_DEG

#define USE_GYRO_EXTI
#define GYRO_1_EXTI_PIN         PE1
#define USE_MPU_DATA_READY_SIGNAL

Gyro任务

描述：主要获取x，y，z方向gyroADC值。

 ├──> 初始化
 │   ├──> [v]硬件初始化gyroDetect(icm20689SpiGyroDetect)/mpuDetect(icm20689SpiDetect)/gyroDetectSensor()/gyroPreInit
 │   └──> [v]业务初始化pgResetFn_gyroConfig/gyroInit
 ├──> 任务
 │   ├──> [v]实时任务[TASK_GYRO] = DEFINE_TASK("GYRO", NULL, NULL, taskGyroSample, TASK_GYROPID_DESIRED_PERIOD, TASK_PRIORITY_REALTIME),
 │   ├──> [x]事件任务
 │   └──> [x]时间任务
 ├──> 驱动
 │   ├──> [v]查询mpuGyroRead
 │   └──> [x]中断
 └──> 接口
     ├──> void dynLpfGyroUpdate(float throttle);
	 ├──> float dynThrottle(float throttle);
	 ├──> void initYawSpinRecovery(int maxYawRate);
	 ├──> uint16_t gyroAbsRateDps(int axis);
	 ├──> bool gyroYawSpinDetected(void);
	 ├──> bool gyroOverflowDetected(void);
	 ├──> void gyroReadTemperature(void);
	 ├──> int16_t gyroGetTemperature(void);
	 ├──> bool isFirstArmingGyroCalibrationRunning(void);
	 ├──> void gyroStartCalibration(bool isFirstArmingCalibration);
	 ├──> FAST_CODE bool gyroFilterReady(void)
     └──> FAST_CODE bool pidLoopReady(void)

taskGyroSample函数分析

这里activePidLoopDenom和pidUpdateCounter主要把gyroFilter和pidLoop两个时间片分开来缓解MCU压力。STM32F1只有STM32F7的1/8。

taskGyroSample
 ├──> gyroUpdate
 ├──> <pidUpdateCounter % activePidLoopDenom == 0> //
 │   └──> pidUpdateCounter = 0;
 └──> pidUpdateCounter++;

注：配置PID_PROCESS_DENOM_DEFAULT，详见：\\src\\main\\flight\\pid.c

#if defined(STM32F1)
#define PID_PROCESS_DENOM_DEFAULT       8
#elif defined(STM32F3)
#define PID_PROCESS_DENOM_DEFAULT       4
#elif defined(STM32F411xE) || defined(STM32G4) //G4 sometimes cpu overflow when PID rate set to higher than 4k
#define PID_PROCESS_DENOM_DEFAULT       2
#else
#define PID_PROCESS_DENOM_DEFAULT       1
#endif

gyroUpdate函数分析

gyroUpdate
 ├──> <GYRO_CONFIG_USE_GYRO_1>
 │   ├──> gyroUpdateSensor(&gyro.gyroSensor1);
 │   └──> <isGyroSensorCalibrationComplete(&gyro.gyroSensor1)>
 │       └──> 更新gyro.gyroADC[X]，gyro.gyroADC[Y]，gyro.gyroADC[Z]
 ├──> <USE_MULTI_GYRO><GYRO_CONFIG_USE_GYRO_2>
 │   ├──> gyroUpdateSensor(&gyro.gyroSensor2);
 │   └──> <isGyroSensorCalibrationComplete(&gyro.gyroSensor2)>
 │       └──> 更新gyro.gyroADC[X]，gyro.gyroADC[Y]，gyro.gyroADC[Z]
 ├──> <USE_MULTI_GYRO><GYRO_CONFIG_USE_GYRO_BOTH>
 │   ├──> gyroUpdateSensor(&gyro.gyroSensor1);
 │   ├──> gyroUpdateSensor(&gyro.gyroSensor2);
 │   └──> <isGyroSensorCalibrationComplete gyroSensor1 && gyroSensor1>
 │       └──> 根据权重更新gyro.gyroADC[X]，gyro.gyroADC[Y]，gyro.gyroADC[Z]
 ├──> <using gyro lowpass 2 filter for downsampling>
 │   └──> lowpass2FilterApplyFn, gyro.sampleSum[X], gyro.sampleSum[Y], gyro.sampleSum[Z]
 └──> <using simple averaging for downsampling>
     └──> gyro.sampleCount++; gyro.sampleSum[X], gyro.sampleSum[Y], gyro.sampleSum[Z]

Acc任务

描述：主要获取x，y，z方向accADC值。

 ├──> 初始化
 │   ├──> [v]硬件初始化accDetect(icm20689SpiAccDetect)/mpuDetect(icm20689SpiDetect)
 │   └──> [v]业务初始化pgResetFn_accelerometerConfig/accInit
 ├──> 任务
 │   ├──> [x]实时任务
 │   ├──> [x]事件任务
 │   └──> [v]时间任务[TASK_ACCEL] = DEFINE_TASK("ACC", NULL, NULL, taskUpdateAccelerometer, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM),
 ├──> 驱动
 │   ├──> [v]查询mpuAccReadSPI
 │   └──> [x]中断
 └──> 接口
     └──> bool accGetAccumulationAverage(float *accumulationAverage)

taskUpdateAccelerometer函数分析

这个函数太简洁了，貌似都感觉应该直接用accUpdate；不过这里的问题是任务通用函数格式与accUpdate不一样，所以有了一个转换。

注：从设计的角度，如果能够整理抽取一个private data指针指向任务各自特殊的数据区（通常OS会这么做），就更加简洁明了。

taskUpdateAccelerometer
 └──> accUpdate(currentTimeUs, &accelerometerConfigMutable()->accelerometerTrims);

accUpdate函数分析

这里的数据主要是采集获取，并未涉及到使用，所以整体结构比较清楚和容易理解。

accUpdate
 ├──> <!acc.dev.readFn(&acc.dev)>
 │   └──> return
 ├──> acc.isAccelUpdatedAtLeastOnce = true;
 ├──> 赋值acc.accADC[axis] = acc.dev.ADCRaw[axis];
 ├──> <accelerationRuntime.accLpfCutHz>
 │   └──> 滤波acc.accADC[axis] = biquadFilterApply(&accelerationRuntime.accFilter[axis], acc.accADC[axis]);
 ├──> <ALIGN_CUSTOM>
 │   └──> alignSensorViaMatrix(acc.accADC, &acc.dev.rotationMatrix);
 ├──> <其他acc.dev.accAlign>
 │   └──> alignSensorViaRotation(acc.accADC, acc.dev.accAlign);
 ├──> <!accIsCalibrationComplete()>
 │   └──> performAcclerationCalibration(rollAndPitchTrims);
 ├──> <featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL)>
 │   └──> performInflightAccelerationCalibration(rollAndPitchTrims);
 ├──> applyAccelerationTrims(accelerationRuntime.accelerationTrims);
 └──> 累计测量值更新
     ├──> ++accelerationRuntime.accumulatedMeasurementCount;
     └──> accelerationRuntime.accumulatedMeasurements[axis] += acc.accADC[axis];