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

Posted 2022-06-08 lida2003

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

• 姿态更新任务
• 姿态欧拉角定义
• 主要函数分析
• • imuUpdateAttitude函数
  • imuCalculateEstimatedAttitude函数

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

姿态更新任务

描述：主要用于计算更新当前机体姿态欧拉角。

 ├──> 初始化
 │   ├──> [x]硬件初始化
 │   └──> [x]业务初始化
 ├──> 任务
 │   ├──> [x]实时任务
 │   ├──> [x]事件任务
 │   └──> [v]时间任务[TASK_ATTITUDE] = DEFINE_TASK("ATTITUDE", NULL, NULL, imuUpdateAttitude, TASK_PERIOD_HZ(100), TASK_PRIORITY_MEDIUM),
 ├──> 驱动
 │   ├──> [x]查询
 │   └──> [x]中断
 └──> 接口
     ├──> void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value)
     ├──> bool isUpright(void);
     └──> float getCosTiltAngle(void);

姿态欧拉角定义

\\src\\main\\flight\\imu.h
typedef union 
    int16_t raw[XYZ_AXIS_COUNT];
    struct 
        // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
        int16_t roll;
        int16_t pitch;
        int16_t yaw;
     values;
 attitudeEulerAngles_t;
#define EULER_INITIALIZE    0, 0, 0  

\\src\\main\\flight\\imu.c
// absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
attitudeEulerAngles_t attitude = EULER_INITIALIZE;

主要函数分析

imuUpdateAttitude函数

imuUpdateAttitude
 ├──> <sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce>
 ├──> IMU_LOCK
 ├──> <SIMULATOR_BUILD><SIMULATOR_IMU_SYNC>
 │   ├──> <imuUpdated == false>
 │   │   ├──> IMU_UNLOCK
 │   │   └──> return
 │   └──> imuUpdated = false;
 ├──> imuCalculateEstimatedAttitude  //计算估计的姿态
 ├──> IMU_UNLOCK
 ├──> <throttleAngleValue && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && ARMING_FLAG(ARMED)> //自稳或者半自稳下，计算油门角度修正
 │   └──> throttleAngleCorrection = calculateThrottleAngleCorrection();
 ├──> mixerSetThrottleAngleCorrection(throttleAngleCorrection);
 └──> <!(sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce)>  // ACC数据从来没有更新过的情况下，只能默认设置零
     ├──> acc.accADC[X] = 0;
     ├──> acc.accADC[Y] = 0;
     ├──> acc.accADC[Z] = 0;
     └──> schedulerIgnoreTaskStateTime

注：ACC更新详见：BetaFlight模块设计之十一：Gyro&Acc任务分析。

imuCalculateEstimatedAttitude函数

imuCalculateEstimatedAttitude
 ├──> <sensors(SENSOR_MAG) && compassIsHealthy() && !gpsRescueDisableMag()>
 │   └──> useMag = true;
 ├──> <!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED>
 │   ├──> courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
 │   ├──> useCOG = true;
 │   └──> <useCOG && shouldInitializeGPSHeading()>
 │       ├──> imuComputeQuaternionFromRPY(&qP, attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
 │       └──> useCOG = false; // Don't use the COG when we first reinitialize.  Next time around though, yes.
 ├──> gyroGetAccumulationAverage(gyroAverage);
 ├──> <accGetAccumulationAverage(accAverage)>
 │   └──> useAcc = imuIsAccelerometerHealthy(accAverage);
 ├──> imuMahonyAHRSupdate
 └──> imuUpdateEulerAngles

注1：course over ground定义
注2：imuMahonyAHRSupdate有算法来计算姿态，后续有时间再做仔细研读。