ArduPilot开源代码之AP_DAL_RangeFinder

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

cpp 复制代码

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

获取指定方向安全距离

cpp 复制代码

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

获取指定方向最大距离

cpp 复制代码

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

指定方向测距仪是否有效

cpp 复制代码

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

获取后台驱动实例

cpp 复制代码

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

cpp 复制代码

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

cpp 复制代码

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

cpp 复制代码

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

cpp 复制代码

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

cpp 复制代码

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]);
        }
    }
}