ArduPilot开源代码之AP_VisualOdom

ArduPilot开源代码之AP_VisualOdom_Backend

1. 源由
2. 类定义
- 2.1 类与构造函数
- 2.2 公共部分
- 2.3 保护部分
- 2.4 成员变量
3. 重要例程
- 3.1 AP_VisualOdom_Backend::healthy
- 3.2 AP_VisualOdom_Backend::quality
- 3.3 AP_VisualOdom_Backend::handle_vision_position_delta_msg
- 3.4 AP_VisualOdom_Backend::get_reset_timestamp_ms
- 3.5 AP_VisualOdom_Backend::get_type
4. 日志记录
- 4.1 AP_VisualOdom_Backend::Write_VisualOdom
- 4.2 AP_VisualOdom_Backend::Write_VisualPosition
- 4.3 AP_VisualOdom_Backend::Write_VisualVelocity
4. 总结
5. 参考资料

1. 源由

继续研读《ArduPilot开源飞控之AP_VisualOdom》关于AP_VisualOdom_Backend模版设备类。

其抽象了哪些共用方法和数据。

2. 类定义

AP_VisualOdom_Backend类作为视觉里程计处理的后端，与传感器交互，处理姿态和速度估计，并确保系统的健康。派生类需要实现关键功能，如处理姿态和速度估计，而基类提供实用函数并管理内部状态，如最后的重置时间戳和质量指标。

注：这个类含有部分条件编译（#if）。

2.1 类与构造函数

class AP_VisualOdom_Backend
{
... ...

/*
  base class constructor. 
  This incorporates initialisation as well.
*/
AP_VisualOdom_Backend::AP_VisualOdom_Backend(AP_VisualOdom &frontend) :
    _frontend(frontend)
{
}

2.2 公共部分

构造函数：
```
AP_VisualOdom_Backend(AP_VisualOdom &frontend);
```
构造函数初始化后端，接收一个AP_VisualOdom对象的引用，称为前端。
健康检查：
```
bool healthy() const;
```
如果传感器正在接收数据并且状态良好，则返回true。
质量检查：
```
int8_t quality() const { return _quality; }
```
返回视觉里程计的质量，范围从-1到100。
条件编译用于MAVLink：
```
#if HAL_GCS_ENABLED
void handle_vision_position_delta_msg(const mavlink_message_t &msg);
#endif
```
如果定义了HAL_GCS_ENABLED，处理MAVLink消息以获取视觉位置增量。

姿态估计处理：

virtual void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality) = 0;

纯虚函数，用于处理姿态估计。派生类必须实现此函数。

视觉速度估计处理：

virtual void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality) = 0;

纯虚函数，用于处理视觉速度估计。派生类必须实现此函数。

请求与AHRS对齐偏航：
```
virtual void request_align_yaw_to_ahrs() {}
```
虚函数，请求偏航与姿态和航向参考系统（AHRS）对齐。默认实现为空。
请求与AHRS对齐位置：
```
virtual void align_position_to_ahrs(bool align_xy, bool align_z) {}
```
虚函数，请求位置与AHRS对齐。默认实现为空。

起动检查：

virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const { return true; }

虚函数，执行起动前的检查。默认实现总是返回true。

2.3 保护部分

重置时间戳处理：

uint32_t get_reset_timestamp_ms(uint8_t reset_counter);

视觉里程计类型：

AP_VisualOdom::VisualOdom_Type get_type(void) const { return _frontend.get_type(); }

条件编译用于日志记录：

#if HAL_LOGGING_ENABLED
void Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence);
void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored, int8_t quality);
void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored, int8_t quality);
#endif

2.4 成员变量

前端引用：
```
AP_VisualOdom &_frontend;
```
最后更新时间：
```
uint32_t _last_update_ms;
```

重置计数器处理：

uint8_t _last_reset_counter;
uint32_t _reset_timestamp_ms;

质量：
```
int8_t _quality;
```

3. 重要例程

3.1 AP_VisualOdom_Backend::healthy

300ms内有数据更新，认为传感器健康。

#define AP_VISUALODOM_TIMEOUT_MS 300

// return true if sensor is basically healthy (we are receiving data)
bool AP_VisualOdom_Backend::healthy() const
{
    // healthy if we have received sensor messages within the past 300ms
    return ((AP_HAL::millis() - _last_update_ms) < AP_VISUALODOM_TIMEOUT_MS);
}

3.2 AP_VisualOdom_Backend::quality

    // return quality as a measure from -1 ~ 100
    // -1 means failed, 0 means unknown, 1 is worst, 100 is best
    int8_t quality() const { return _quality; }

3.3 AP_VisualOdom_Backend::handle_vision_position_delta_msg

AP_VisualOdom_Backend::handle_vision_position_delta_msg(const mavlink_message_t &msg)
|
|-- 解码消息
|   |-- mavlink_vision_position_delta_t packet;
|   |-- mavlink_msg_vision_position_delta_decode(&msg, &packet);
|
|-- 角度和位置增量的旋转应用
|   |-- const enum Rotation rot = _frontend.get_orientation();
|   |-- Vector3f angle_delta = Vector3f(packet.angle_delta[0], packet.angle_delta[1], packet.angle_delta[2]);
|   |   |-- angle_delta.rotate(rot);
|   |-- Vector3f position_delta = Vector3f(packet.position_delta[0], packet.position_delta[1], packet.position_delta[2]);
|   |   |-- position_delta.rotate(rot);
|
|-- 获取当前时间
|   |-- const uint32_t now_ms = AP_HAL::millis();
|   |-- _last_update_ms = now_ms;
|
|-- 发送到EKF (扩展卡尔曼滤波)
|   |
|   |-- 如果启用了AP_AHRS或日志记录
|   |   |-- const float time_delta_sec = packet.time_delta_usec * 1.0E-6;
|
|   |-- 如果启用了AP_AHRS
|   |   |-- AP::ahrs().writeBodyFrameOdom(packet.confidence,
|   |                                     position_delta,
|   |                                     angle_delta,
|   |                                     time_delta_sec,
|   |                                     now_ms,
|   |                                     _frontend.get_delay_ms(),
|   |                                     _frontend.get_pos_offset());
|
|-- 如果启用了日志记录
|   |-- Write_VisualOdom(time_delta_sec,
|                        angle_delta,
|                        position_delta,
|                        packet.confidence);

解码消息
- 定义mavlink_vision_position_delta_t packet;
- 调用mavlink_msg_vision_position_delta_decode(&msg, &packet);将消息解码到packet中。
角度和位置增量的旋转应用
- 获取当前的旋转方式const enum Rotation rot = _frontend.get_orientation();
- 创建Vector3f angle_delta并使用消息中的角度增量初始化。
  - 调用angle_delta.rotate(rot);应用旋转。
- 创建Vector3f position_delta并使用消息中的位置增量初始化。
  - 调用position_delta.rotate(rot);应用旋转。
获取当前时间
- 调用AP_HAL::millis();获取当前时间并保存到now_ms。
- 更新_last_update_ms为当前时间now_ms。
发送到EKF
- 如果启用了AP_AHRS或日志记录，计算时间增量const float time_delta_sec = packet.time_delta_usec * 1.0E-6;
- 如果启用了AP_AHRS，调用AP::ahrs().writeBodyFrameOdom()方法，将姿态和位置增量数据发送到EKF。
  - 传递参数包括：置信度、位置增量、角度增量、时间增量、当前时间、前端的延迟和位置偏移。
日志记录
- 如果启用了日志记录，调用Write_VisualOdom()方法记录传感器数据。
  - 传递参数包括：时间增量、角度增量、位置增量和置信度。

3.4 AP_VisualOdom_Backend::get_reset_timestamp_ms

// returns the system time of the last reset if reset_counter has not changed
// updates the reset timestamp to the current system time if the reset_counter has changed
uint32_t AP_VisualOdom_Backend::get_reset_timestamp_ms(uint8_t reset_counter)
{
    // update reset counter and timestamp if reset_counter has changed
    if (reset_counter != _last_reset_counter) {
        _last_reset_counter = reset_counter;
        _reset_timestamp_ms = AP_HAL::millis();
    }
    return _reset_timestamp_ms;
}

3.5 AP_VisualOdom_Backend::get_type

从前台类型中获取设备类型。

    AP_VisualOdom::VisualOdom_Type get_type(void) const {
        return _frontend.get_type();
    }

4. 日志记录

4.1 AP_VisualOdom_Backend::Write_VisualOdom

// Write visual odometry sensor data
void AP_VisualOdom_Backend::Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence)
{
    const struct log_VisualOdom pkt_visualodom {
        LOG_PACKET_HEADER_INIT(LOG_VISUALODOM_MSG),
        time_us             : AP_HAL::micros64(),
        time_delta          : time_delta,
        angle_delta_x       : angle_delta.x,
        angle_delta_y       : angle_delta.y,
        angle_delta_z       : angle_delta.z,
        position_delta_x    : position_delta.x,
        position_delta_y    : position_delta.y,
        position_delta_z    : position_delta.z,
        confidence          : confidence
    };
    AP::logger().WriteBlock(&pkt_visualodom, sizeof(log_VisualOdom));
}

4.2 AP_VisualOdom_Backend::Write_VisualPosition

// Write visual position sensor data.  x,y,z are in meters, angles are in degrees
void AP_VisualOdom_Backend::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored, int8_t quality)
{
    const struct log_VisualPosition pkt_visualpos {
        LOG_PACKET_HEADER_INIT(LOG_VISUALPOS_MSG),
        time_us         : AP_HAL::micros64(),
        remote_time_us  : remote_time_us,
        time_ms         : time_ms,
        pos_x           : x,
        pos_y           : y,
        pos_z           : z,
        roll            : roll,
        pitch           : pitch,
        yaw             : yaw,
        pos_err         : pos_err,
        ang_err         : ang_err,
        reset_counter   : reset_counter,
        ignored         : (uint8_t)ignored,
        quality         : quality
    };
    AP::logger().WriteBlock(&pkt_visualpos, sizeof(log_VisualPosition));
}

4.3 AP_VisualOdom_Backend::Write_VisualVelocity

// Write visual velocity sensor data, velocity in NED meters per second
void AP_VisualOdom_Backend::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored, int8_t quality)
{
    const struct log_VisualVelocity pkt_visualvel {
        LOG_PACKET_HEADER_INIT(LOG_VISUALVEL_MSG),
        time_us         : AP_HAL::micros64(),
        remote_time_us  : remote_time_us,
        time_ms         : time_ms,
        vel_x           : vel.x,
        vel_y           : vel.y,
        vel_z           : vel.z,
        vel_err         : _frontend.get_vel_noise(),
        reset_counter   : reset_counter,
        ignored         : (uint8_t)ignored,
        quality         : quality
    };
    AP::logger().WriteBlock(&pkt_visualvel, sizeof(log_VisualVelocity));
}

4. 总结

AP_VisualOdom_Backend类作为视觉里程计处理的后端模版，实现了通用的处理流程。因硬件设备驱动而已的API函数封装如下：

handle_pose_estimate
handle_vision_speed_estimate
request_align_yaw_to_ahrs
align_position_to_ahrs
pre_arm_check

    // consume vision pose estimate data and send to EKF. distances in meters
    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
    virtual void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality) = 0;

    // consume vision velocity estimate data and send to EKF, velocity in NED meters per second
    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
    virtual void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality) = 0;

    // request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
    virtual void request_align_yaw_to_ahrs() {}

    // handle request to align position with AHRS
    virtual void align_position_to_ahrs(bool align_xy, bool align_z) {}

    // arming check - by default no checks performed
    virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const { return true; }