ArduPilot开源代码之AP_DAL_RangeFinder

• 1. 源由
• 2. 框架设计
• • 2.1 枚举 `Status`
  • 2.2 公有方法
  • 2.3 私有成员变量
• 3. 重要例程
• • 3.1 应用函数
  • • 3.1.1 ground_clearance_cm_orient
    • 3.1.2 max_distance_cm_orient
    • 3.1.3 has_orientation
    • 3.1.4 get_backend
  • 3.2 其他函数
  • • 3.2.1 AP_DAL_RangeFinder
    • 3.2.2 start_frame
    • 3.2.3 handle_message
• 4. 总结
• 5. 参考资料

1. 源由

AP_DAL_RangeFinder用于管理和操作测距仪的数据和状态。

它提供了一些方法来获取测距仪的高度和距离信息，检查测距仪的方向，启动数据收集帧，并处理日志消息。私有成员变量则用于存储日志信息和管理后端实例。

2. 框架设计

2.1 枚举 Status

这个枚举定义了测距仪的各种状态，包括：

• NotConnected: 测距仪未连接。
• NoData: 测距仪没有数据。
• OutOfRangeLow: 测距仪数据超出下限。
• OutOfRangeHigh: 测距仪数据超出上限。
• Good: 测距仪状态良好。

2.2 公有方法

• int16_t ground_clearance_cm_orient(enum Rotation orientation) const;

  • 根据给定的方向返回地面净空高度，单位是厘米。

• int16_t max_distance_cm_orient(enum Rotation orientation) const;

  • 根据给定的方向返回最大距离，单位是厘米。

• bool has_orientation(enum Rotation orientation) const;

  • 检查是否存在具有指定方向的测距仪，返回布尔值。

• AP_DAL_RangeFinder();

  • 构造函数，用于初始化类的实例。

• void start_frame();

  • 开始一个新的帧，可能用于初始化或重置测距仪的数据收集过程。

• AP_DAL_RangeFinder_Backend *get_backend(uint8_t id) const;

  • 根据给定的ID获取对应的后端实例，返回指向后端实例的指针。

• void handle_message(const log_RRNH &msg);

  • 处理 log_RRNH 类型的日志消息。

• void handle_message(const log_RRNI &msg);

  • 处理 log_RRNI 类型的日志消息。

2.3 私有成员变量

• struct log_RRNH _RRNH;

  • 一个 log_RRNH 结构体实例，用于存储相关的日志信息。

• struct log_RRNI *_RRNI;

  • 一个指向 log_RRNI 结构体的指针，用于存储相关的日志信息。

• AP_DAL_RangeFinder_Backend **_backend;

  • 一个指向 AP_DAL_RangeFinder_Backend 实例数组的指针，可能用于管理多个后端实例。

3. 重要例程

enum Rotation : uint8_t {     ROTATION_NONE                = 0,     ROTATION_YAW_45              = 1,     ROTATION_YAW_90              = 2,     ROTATION_YAW_135             = 3,     ROTATION_YAW_180             = 4,     ROTATION_YAW_225             = 5,     ROTATION_YAW_270             = 6,     ROTATION_YAW_315             = 7,     ROTATION_ROLL_180            = 8,     ROTATION_ROLL_180_YAW_45     = 9,     ROTATION_ROLL_180_YAW_90     = 10,     ROTATION_ROLL_180_YAW_135    = 11,     ROTATION_PITCH_180           = 12,     ROTATION_ROLL_180_YAW_225    = 13,     ROTATION_ROLL_180_YAW_270    = 14,     ROTATION_ROLL_180_YAW_315    = 15,     ROTATION_ROLL_90             = 16,     ROTATION_ROLL_90_YAW_45      = 17,     ROTATION_ROLL_90_YAW_90      = 18,     ROTATION_ROLL_90_YAW_135     = 19,     ROTATION_ROLL_270            = 20,     ROTATION_ROLL_270_YAW_45     = 21,     ROTATION_ROLL_270_YAW_90     = 22,     ROTATION_ROLL_270_YAW_135    = 23,     ROTATION_PITCH_90            = 24,     ROTATION_PITCH_270           = 25,     ROTATION_PITCH_180_YAW_90    = 26, // same as ROTATION_ROLL_180_YAW_270     ROTATION_PITCH_180_YAW_270   = 27, // same as ROTATION_ROLL_180_YAW_90     ROTATION_ROLL_90_PITCH_90    = 28,     ROTATION_ROLL_180_PITCH_90   = 29,     ROTATION_ROLL_270_PITCH_90   = 30,     ROTATION_ROLL_90_PITCH_180   = 31,     ROTATION_ROLL_270_PITCH_180  = 32,     ROTATION_ROLL_90_PITCH_270   = 33,     ROTATION_ROLL_180_PITCH_270  = 34,     ROTATION_ROLL_270_PITCH_270  = 35,     ROTATION_ROLL_90_PITCH_180_YAW_90 = 36,     ROTATION_ROLL_90_YAW_270     = 37,     ROTATION_ROLL_90_PITCH_68_YAW_293 = 38, // this is actually, roll 90, pitch 68.8, yaw 293.3     ROTATION_PITCH_315           = 39,     ROTATION_ROLL_90_PITCH_315   = 40,     ROTATION_PITCH_7             = 41,     ROTATION_ROLL_45             = 42,     ROTATION_ROLL_315            = 43,     ///     // Do not add more rotations without checking that there is not a conflict     // with the MAVLink spec. MAV_SENSOR_ORIENTATION is expected to match our     // list of rotations here. If a new rotation is added it needs to be added     // to the MAVLink messages as well.     ///     ROTATION_MAX,     ROTATION_CUSTOM_OLD          = 100,     ROTATION_CUSTOM_1            = 101,     ROTATION_CUSTOM_2            = 102,     ROTATION_CUSTOM_END, }; 

3.1 应用函数

3.1.1 ground_clearance_cm_orient

获取指定方向安全距离

int16_t AP_DAL_RangeFinder::ground_clearance_cm_orient(enum Rotation orientation) const { #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone)     const auto *rangefinder = AP::rangefinder();      if (orientation != ROTATION_PITCH_270) {         // the EKF only asks for this from a specific orientation.  Thankfully.         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);         return rangefinder->ground_clearance_cm_orient(orientation);     } #endif      return _RRNH.ground_clearance_cm; } 

3.1.2 max_distance_cm_orient

获取指定方向最大距离

int16_t AP_DAL_RangeFinder::max_distance_cm_orient(enum Rotation orientation) const { #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone)     if (orientation != ROTATION_PITCH_270) {         const auto *rangefinder = AP::rangefinder();         // the EKF only asks for this from a specific orientation.  Thankfully.         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);         return rangefinder->max_distance_cm_orient(orientation);     } #endif      return _RRNH.max_distance_cm; } 

3.1.3 has_orientation

指定方向测距仪是否有效

bool AP_DAL_RangeFinder::has_orientation(enum Rotation orientation) const {     for (uint8_t i=0; i<_RRNH.num_sensors; i++) {         if (_RRNI[i].orientation == orientation) {             return true;         }     }     return false; }  

3.1.4 get_backend

获取后台驱动实例

AP_DAL_RangeFinder_Backend *AP_DAL_RangeFinder::get_backend(uint8_t id) const {    if (id >= RANGEFINDER_MAX_INSTANCES) {        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);        return nullptr;    }    if (id >= _RRNH.num_sensors) {         return nullptr;     }     return _backend[id]; } 

3.2 其他函数

3.2.1 AP_DAL_RangeFinder

构造函数，初始化实例序号

• _RRNH //Replay Data Rangefinder Header
• _RRNI //Replay Data Rangefinder Instance
• _backend

AP_DAL_RangeFinder::AP_DAL_RangeFinder() { #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)     _RRNH.num_sensors = AP::rangefinder()->num_sensors();     _RRNI = NEW_NOTHROW log_RRNI[_RRNH.num_sensors];     _backend = NEW_NOTHROW AP_DAL_RangeFinder_Backend *[_RRNH.num_sensors];     if (!_RRNI || !_backend) {         goto failed;     }     for (uint8_t i=0; i<_RRNH.num_sensors; i++) {         _RRNI[i].instance = i;     }     for (uint8_t i=0; i<_RRNH.num_sensors; i++) {         // this avoids having to discard a const....         _backend[i] = NEW_NOTHROW AP_DAL_RangeFinder_Backend(_RRNI[i]);         if (!_backend[i]) {             goto failed;         }     }     return; failed:     AP_BoardConfig::allocation_error("DAL backends"); #endif } 

3.2.2 start_frame

AP_DAL::start_frame  └──> AP_DAL_RangeFinder::start_frame 

void AP_DAL_RangeFinder::start_frame() {     const auto *rangefinder = AP::rangefinder();  // 获取距离传感器对象的指针     if (rangefinder == nullptr) {         return;  // 如果传感器对象为空，直接返回     }      const log_RRNH old = _RRNH;  // 备份旧的 RRNH 对象状态      // EKF 只需要这个值 *向下*。     _RRNH.ground_clearance_cm = rangefinder->ground_clearance_cm_orient(ROTATION_PITCH_270);  // 设置地面间隔高度，使用 ROTATION_PITCH_270 方向     _RRNH.max_distance_cm = rangefinder->max_distance_cm_orient(ROTATION_PITCH_270);  // 设置最大测距距离，使用 ROTATION_PITCH_270 方向      WRITE_REPLAY_BLOCK_IFCHANGED(RRNH, _RRNH, old);  // 如果 RRNH 对象改变，则写入重放块      // 遍历所有传感器     for (uint8_t i = 0; i < _RRNH.num_sensors; i++) {         auto *backend = rangefinder->get_backend(i);  // 获取第 i 个传感器的后端对象指针         if (backend == nullptr) {             continue;  // 如果后端对象为空，跳过当前传感器         }         _backend[i]->start_frame(backend);  // 调用对应传感器的后端对象的 start_frame 函数     } } 

3.2.3 handle_message

AP_DAL::handle_message  └──> AP_DAL_RangeFinder::handle_message 

void AP_DAL_RangeFinder::handle_message(const log_RRNH &msg) {     _RRNH = msg;     if (_RRNH.num_sensors > 0 && _RRNI == nullptr) {         _RRNI = NEW_NOTHROW log_RRNI[_RRNH.num_sensors];         _backend = NEW_NOTHROW AP_DAL_RangeFinder_Backend *[_RRNH.num_sensors];     } }  void AP_DAL_RangeFinder::handle_message(const log_RRNI &msg) {     if (_RRNI != nullptr && msg.instance < _RRNH.num_sensors) {         _RRNI[msg.instance] = msg;         if (_backend != nullptr && _backend[msg.instance] == nullptr) {             _backend[msg.instance] = NEW_NOTHROW AP_DAL_RangeFinder_Backend(_RRNI[msg.instance]);         }     } } 

4. 总结

AP_DAL_RangeFinder主要功能是用于管理和操作测距仪的数据和状态，并提供访问接口进行直接状态访问。

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计
【6】ArduPilot开源代码之EKF系列研读
【7】ArduPilot开源代码之AP_DAL_RangeFinder_Backend
【7】ArduPilot开源代码之AP_DAL研读系列