ArduPilot开源代码之AP_DAL_RangeFinder

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_270ROTATION_PITCH_180_YAW_270   = 27, // same as ROTATION_ROLL_180_YAW_90ROTATION_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.3ROTATION_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);}
#endifreturn _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);}
#endifreturn _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研读系列