ArduPilot开源代码之AP_DAL_VisualOdom

1. 源由

AP_DAL_VisualOdom是视觉里程计系统的状态，并提供查询和更新该状态的方法。

2. 框架设计

2.1 公共方法

• healthy(): 返回一个布尔值，表示视觉里程计系统是否正常运行。

• enabled(): 返回一个布尔值，表示视觉里程计系统是否已启用。

• get_delay_ms(): 以 uint16_t 类型返回视觉里程计系统的延迟时间（毫秒）。

• get_pos_offset(): 返回一个 Vector3f 对象的常量引用，表示相机相对于机体坐标系原点的位置偏移（以米为单位）。

• align_position_to_ahrs(bool align_xy, bool align_z): 调整位置偏移以与 AHRS（姿态航向参考系统）位置对齐。只有在该库未用作主要位置来源时才应调用此函数。

• start_frame(): 可能用于初始化或准备处理新帧。具体实现未提供。

• handle_message(const log_RVOH &msg): 使用 log_RVOH 消息中的新数据更新 RVOH 结构。

2.2 私有成员

• log_RVOH RVOH: 一个结构体，可能包含与视觉里程计相关的状态和配置数据，包括健康状态、启用状态、延迟和位置偏移。 //Replay VisualOdom Head

2.3 假设

• Vector3f 假设为表示三维向量的类或结构体。
• log_RVOH 是一个包含字段如 healthy、enabled、delay_ms 和 pos_offset 的结构体。

3. 重要例程

3.1 应用函数

3.1.1 healthy

获取传感器健康状态

    bool healthy() const {
        return RVOH.healthy;
    }

3.1.2 enabled

获取传感器使能状态

    bool enabled() const {
        return RVOH.enabled;
    }

3.1.3 get_delay_ms

获取传感器延迟时间

    uint16_t get_delay_ms() const {
        return RVOH.delay_ms;
    }

3.1.4 get_pos_offset

获取相机相对于机体坐标系原点的位置偏移

    const Vector3f &get_pos_offset() const {
        return RVOH.pos_offset;
    }

3.1.5 align_position_to_ahrs

调整位置偏移以与 AHRS（姿态航向参考系统）位置对齐

void AP_DAL_VisualOdom::align_position_to_ahrs(bool align_xy, bool align_z)
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone)
    auto *vo = AP::visualodom();
    vo->align_position_to_ahrs(align_xy, align_z);
#endif
}

3.2 其他函数

3.2.1 start_frame

AP_DAL::start_frame
 └──> AP_DAL_VisualOdom::start_frame

void AP_DAL_VisualOdom::start_frame()
{
    const auto *vo = AP::visualodom();

    const log_RVOH old = RVOH;
    if (vo != nullptr) {
        RVOH.pos_offset = vo->get_pos_offset();
        RVOH.delay_ms = vo->get_delay_ms();
        RVOH.healthy = vo->healthy();
        RVOH.enabled = vo->enabled();
    }

    WRITE_REPLAY_BLOCK_IFCHANGED(RVOH, RVOH, old);
}

3.2.2 handle_message

AP_DAL::handle_message
 └──> AP_DAL_VisualOdom::handle_message

    void handle_message(const log_RVOH &msg) {
        RVOH = msg;
    }

4. 总结

AP_DAL_VisualOdom是视觉里程计系统的状态，其主要实现依赖于《ArduPilot开源飞控之AP_VisualOdom》。

5. 参考资料

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