BetaFlight模块设计之二十二：地面测距任务分析

Posted 2022-06-03 lida2003

BetaFlight模块设计之二十二：地面测距任务分析

• 地面测距任务
• 硬件配置
• • 超声波测距模型
  • 雷达测距
• 软件配置
• 主要函数分析
• • taskUpdateRangefinder
  • rangefinderUpdate
  • rangefinderProcess

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

地面测距任务

描述：主要使用超声波或者雷达感知FC与地面之间的距离。

 ├──> 初始化
 │   ├──> [v]硬件初始化hcsr04Detect/lidarTFminiDetect/lidarTF02Detect
 │   └──> [v]业务初始化rangefinderInit
 ├──> 任务
 │   ├──> [x]实时任务
 │   ├──> [x]事件任务
 │   └──> [v]时间任务[TASK_RANGEFINDER] = DEFINE_TASK("RANGEFINDER", NULL, NULL, taskUpdateRangefinder, TASK_PERIOD_HZ(10), TASK_PRIORITY_LOWEST),
 ├──> 驱动
 │   ├──> [v]查询hcsr04_update/lidarTFUpdate
 │   └──> [v]中断hcsr04_extiHandler
 └──> 接口
     ├──> 支持HC-SR04声呐和Benewake TF02/TF03 lidar
     ├──> int32_t rangefinderGetLatestAltitude(void)
     ├──> bool rangefinderIsHealthy(void)
     └──> bool isSurfaceAltitudeValid()

硬件配置

kakute F7 AIO模型未配置上述距离感知模块，所以在目标板配置文件里面没有以下参数定义。

USE_RANGEFINDER_HCSR04  //超声波传感
USE_RANGEFINDER_TF      //雷达测距

超声波测距模型

./src/main/target/OMNIBUS/target.h:57://#define USE_RANGEFINDER_HCSR04
./src/main/target/NUCLEOH743/target.h:246:#define USE_RANGEFINDER_HCSR04
./src/main/target/OMNIBUSF7/target.h:216:#define USE_RANGEFINDER_HCSR04
./src/main/target/ISHAPEDF3/target.h:55:#define USE_RANGEFINDER_HCSR04
./src/main/target/OMNINXT/target.h:181:#define USE_RANGEFINDER_HCSR04
./src/main/target/AG3X/target.h:157:#define USE_RANGEFINDER_HCSR04
./src/main/target/MULTIFLITEPICO/target.h:65:#define USE_RANGEFINDER_HCSR04
./src/main/target/OMNIBUSF4/target.h:280:#define USE_RANGEFINDER_HCSR04
./src/main/target/NUCLEOH723ZG/target.h:232:#define USE_RANGEFINDER_HCSR04
./src/main/target/CC3D/target.h:98://#define USE_RANGEFINDER_HCSR04
./src/main/target/CC3D/target.h:108:#undef USE_RANGEFINDER_HCSR04
./src/main/target/NAZE/target.h:117://#define USE_RANGEFINDER_HCSR04
./src/main/target/NUCLEOF103RG/target.h:115://#define USE_RANGEFINDER_HCSR04
./src/main/target/NUCLEOH7A3ZI/target.h:232:#define USE_RANGEFINDER_HCSR04
./src/main/target/SPRACINGF3EVO/target.h:79://#define USE_RANGEFINDER_HCSR04
./src/main/target/STM32F3DISCOVERY/target.h:180:#define USE_RANGEFINDER_HCSR04
./src/main/target/RCEXPLORERF3/target.h:60:#define USE_RANGEFINDER_HCSR04
./src/main/target/STM32_UNIFIED/target.h:342:#define USE_RANGEFINDER_HCSR04
./src/main/target/SPRACINGF3/target.h:103:#define USE_RANGEFINDER_HCSR04
./src/main/target/SPRACINGF3/target.h:114:#define USE_RANGEFINDER_HCSR04
./src/main/target/NUCLEOH725ZG/target.h:241:#define USE_RANGEFINDER_HCSR04
./src/main/target/SPRACINGF3MINI/target.h:79://#define USE_RANGEFINDER_HCSR04
./src/main/target/NUCLEOF303RE/target.h:62://#define USE_RANGEFINDER_HCSR04
./src/main/target/SPARKY/target.h:95://#define USE_RANGEFINDER_HCSR04

雷达测距

./src/main/target/NUCLEOH743/target.h:247:#define USE_RANGEFINDER_TF
./src/main/target/OMNINXT/target.h:182:#define USE_RANGEFINDER_TF
./src/main/target/AG3X/target.h:158://#define USE_RANGEFINDER_TF
./src/main/target/OMNIBUSF4/target.h:283:#define USE_RANGEFINDER_TF
./src/main/target/NUCLEOH723ZG/target.h:233:#define USE_RANGEFINDER_TF
./src/main/target/NUCLEOH7A3ZI/target.h:233:#define USE_RANGEFINDER_TF
./src/main/target/STM32_UNIFIED/target.h:343:#define USE_RANGEFINDER_TF
./src/main/target/NUCLEOH725ZG/target.h:242:#define USE_RANGEFINDER_TF

软件配置

代码还是存在一定的耦合度，没有从驱动API上解耦，而是直接在代码层面采用了枚举。

\\src\\main\\sensors\\rangefinder.h
typedef enum 
    RANGEFINDER_NONE        = 0,
    RANGEFINDER_HCSR04      = 1,
    RANGEFINDER_TFMINI      = 2,
    RANGEFINDER_TF02        = 3,
 rangefinderType_e;

主要函数分析

taskUpdateRangefinder

任务函数判断没有测距模块情况直接反馈，其他情况执行后续数据更新和距离计算。

taskUpdateRangefinder
 ├──> <!sensors(SENSOR_RANGEFINDER)>
 │   └──> return
 ├──> rangefinderUpdate
 └──> rangefinderProcess(getCosTiltAngle());

rangefinderUpdate

调用驱动API触发测量动作。

rangefinderUpdate
 └──> <rangefinder.dev.update>
     └──> rangefinder.dev.update(&rangefinder.dev);

rangefinderProcess

获取测量数据，并数据分析和计算当前距离地面高度。

rangefinderProcess
 ├──> <!rangefinder.dev.read> // Bad configuration
 │   └──> rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
 ├──> <rangefinder.dev.read>
 │   ├──> const int32_t distance = rangefinder.dev.read(&rangefinder.dev);
 │   ├──> <distance == RANGEFINDER_NO_NEW_DATA>
 │   │   └──> return false
 │   ├──> <distance >= 0>
 │   │   ├──> rangefinder.lastValidResponseTimeMs = millis();
 │   │   └──> rangefinder.rawAltitude = applyMedianFilter(distance);  //中位数据过滤
 │   ├──> <distance == RANGEFINDER_OUT_OF_RANGE>
 │   │   ├──> rangefinder.lastValidResponseTimeMs = millis();
 │   │   └──> rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
 │   ├──> <else>
 │   │   └──> rangefinder.rawAltitude = RANGEFINDER_HARDWARE_FAILURE;
 │   ├──> rangefinder.snr = computePseudoSnr(distance);
 │   └──> <rangefinder.snrThresholdReached == false && rangefinder.rawAltitude > 0>
 │       ├──> <rangefinder.snr < RANGEFINDER_DYNAMIC_THRESHOLD && rangefinder.dynamicDistanceThreshold < rangefinder.rawAltitude>
 │       │   └──> rangefinder.dynamicDistanceThreshold = rangefinder.rawAltitude * RANGEFINDER_DYNAMIC_FACTOR / 100;
 │       └──> <rangefinder.snr >= RANGEFINDER_DYNAMIC_THRESHOLD>
 │           └──> rangefinder.snrThresholdReached = true;
 ├──> <cosTiltAngle < rangefinder.maxTiltCos || rangefinder.rawAltitude < 0>
 │   └──> rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
 ├──> <!(cosTiltAngle < rangefinder.maxTiltCos || rangefinder.rawAltitude < 0)>
 │   └──> rangefinder.calculatedAltitude = rangefinder.rawAltitude * cosTiltAngle; //垂直距离计算
 └──> return true