BetaFlight模块设计之十四：高度计算任务分析

Posted 2022-05-30 lida2003

BetaFlight模块设计之十四：高度计算任务分析

• 高度计算任务
• 配置情况
• • GPS配置参数
  • 气压计配置参数
  • 支持GPS和或气压计高度计算
• taskCalculateAltitude函数分析
• calculateEstimatedAltitude函数分析

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

高度计算任务

描述：计算飞控当前垂直高度(不是海拔，是基于起飞地面的垂直高度)。

 ├──> 初始化
 │   ├──> [x]硬件初始化
 │   └──> [x]业务初始化
 ├──> 任务
 │   ├──> [x]实时任务
 │   ├──> [x]事件任务
 │   └──> [v]时间任务[TASK_ALTITUDE] = DEFINE_TASK("ALTITUDE", NULL, NULL, taskCalculateAltitude, TASK_PERIOD_HZ(40), TASK_PRIORITY_LOW),
 ├──> 驱动
 │   ├──> [x]查询
 │   └──> [x]中断
 └──> 接口
     ├──> int16_t getEstimatedVario(void)
     ├──> bool isAltitudeOffset(void)
     └──> int32_t getEstimatedAltitudeCm(void)

配置情况

GPS配置参数

\\src\\main\\target\\common_pre.h
#define USE_GPS

气压计配置参数

\\src\\main\\target\\KAKUTEF7\\target.h
#define USE_BARO

支持GPS和或气压计高度计算

\\src\\main\\fc\\tasks.c
#if defined(USE_BARO) || defined(USE_GPS)
static void taskCalculateAltitude(timeUs_t currentTimeUs)

    calculateEstimatedAltitude(currentTimeUs);

#endif // USE_BARO || USE_GPS

taskCalculateAltitude函数分析

这里还是和之前的有类似的问题，两个函数的定义也都一样，为什么不直接使用calculateEstimatedAltitude？？？

难道是想将代码进一步解耦：

1. calculateEstimatedAltitudeByGps
2. calculateEstimatedAltitudeByBaro
3. calculateEstimatedAltitudeByBoth

taskCalculateAltitude
 └──> calculateEstimatedAltitude

calculateEstimatedAltitude函数分析

这里看下代码总共是210 - 94 = 116行，感觉是不是有点长？其实还好，只不过确实有较多的全局变量：（
值得考虑进一步封装和代码优化，这也体现了代码长时间的功能增加带来的问题，希望有朝一日，能够梳理和重构！！！

calculateEstimatedAltitude
 ├──> const uint32_t dTime = currentTimeUs - previousTimeUs;
 ├──> <dTime < BARO_UPDATE_FREQUENCY_40HZ>  // 25ms更新一次垂直地面高度
 │   ├──> schedulerIgnoreTaskExecTime
 │   └──> return
 ├──> previousTimeUs = currentTimeUs;
 ├──> float gpsTrust = 0.3; //conservative default
 ├──> <USE_BARO><sensors(SENSOR_BARO)>
 │   ├──> <!baroIsCalibrationComplete()>
 │   │   └──> performBaroCalibrationCycle()
 │   └──> <baroIsCalibrationComplete()>
 │       ├──> baroAlt = baroCalculateAltitude();
 │       └──> haveBaroAlt = true;
 ├──> <USE_GPS><sensors(SENSOR_GPS)><STATE(GPS_FIX)>
 │   ├──> gpsAlt = gpsSol.llh.altCm;
 │   ├──> gpsNumSat = gpsSol.numSat;
 │   ├──> <USE_VARIO>
 │   │   └──> gpsVertSpeed = GPS_verticalSpeedInCmS;
 │   ├──> haveGpsAlt = true;
 │   ├──> <gpsSol.hdop != 0>
 │   │   └──> gpsTrust = 100.0 / gpsSol.hdop;
 │   └──> gpsTrust = MIN(gpsTrust, 0.9f); // always use at least 10% of other sources besides gps if available
 ├──> <ARMING_FLAG(ARMED) && !altitudeOffsetSetBaro>
 │   ├──> baroAltOffset = baroAlt;
 │   └──> altitudeOffsetSetBaro = true;
 ├──> <!ARMING_FLAG(ARMED) && altitudeOffsetSetBaro>
 │   └──> altitudeOffsetSetBaro = false;
 ├──> baroAlt -= baroAltOffset;
 ├──> int goodGpsSats = 0;
 ├──> int badGpsSats = -1;
 ├──> <haveBaroAlt>
 │   ├──> goodGpsSats = positionConfig()->altNumSatsGpsUse;
 │   └──> badGpsSats = positionConfig()->altNumSatsBaroFallback;
 ├──> <ARMING_FLAG(ARMED)>
 │   ├──> <!altitudeOffsetSetGPS && gpsNumSat >= goodGpsSats>
 │   │   ├──> gpsAltOffset = gpsAlt - baroAlt;
 │   │   └──> altitudeOffsetSetGPS = true;
 │   └──> <gpsNumSat <= badGpsSats>
 │       └──> altitudeOffsetSetGPS = false;
 ├──> <!ARMING_FLAG(ARMED) && altitudeOffsetSetGPS>
 │   └──> altitudeOffsetSetGPS = false;
 ├──> gpsAlt -= gpsAltOffset;
 ├──> <!altitudeOffsetSetGPS>
 │   ├──> haveGpsAlt = false;
 │   └──> gpsTrust = 0.0f;
 ├──> <haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT>
 │   ├──> <ARMING_FLAG(ARMED)>
 │   │   └──> estimatedAltitudeCm = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust);
 │   ├──> <!ARMING_FLAG(ARMED)>
 │   │   └──> estimatedAltitudeCm = gpsAlt; //absolute altitude is shown before arming, ignore baro
 │   ├──> <USE_VARIO> // baro is a better source for vario, so ignore gpsVertSpeed
 │   │   └──> estimatedVario = calculateEstimatedVario(baroAlt, dTime);
 ├──> <haveGpsAlt && (positionConfig()->altSource == GPS_ONLY || positionConfig()->altSource == DEFAULT>
 │   ├──> estimatedAltitudeCm = gpsAlt;
 │   └──> <USE_VARIO><USE_GPS>
 │       └──> estimatedVario = gpsVertSpeed;
 └──> <haveBaroAlt && (positionConfig()->altSource == BARO_ONLY || positionConfig()->altSource == DEFAULT>
     ├──> estimatedAltitudeCm = baroAlt;
     └──> <USE_VARIO>
         └──> estimatedVario = calculateEstimatedVario(baroAlt, dTime);

注：关于HDOP范围已经描述，中文详见链接，DOP Wiki全面解释，通常HDOP=100，就是100cm。