阅读量:2
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_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研读系列
