ArduPilot开源代码之AP_VisualOdom_MAV

1. 源由

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

除了《ArduPilot开源代码之AP_VisualOdom_Backend》抽象的共用方法和数据外，来进一步研读下设备差异导致的函数实现。

2. 类定义

这段C++代码定义了一个类 AP_VisualOdom_MAV，它继承自 AP_VisualOdom_Backend。

class AP_VisualOdom_MAV : public AP_VisualOdom_Backend
{
public:
    // 构造函数，使用从AP_VisualOdom_Backend继承的构造函数
    using AP_VisualOdom_Backend::AP_VisualOdom_Backend;

    // 处理姿态估计数据，将其发送给EKF。距离单位为米
    // Parameters:
    // - remote_time_us: Remote timestamp in microseconds
    // - time_ms: Local timestamp in milliseconds
    // - x, y, z: Position coordinates in meters
    // - attitude: Quaternion representing attitude (orientation)
    // - posErr: Position error
    // - angErr: Angular error
    // - reset_counter: Counter for reset events
    // - quality: Quality of pose estimate (-1 for failed, 0 for unknown, 1 to 100 for varying levels of quality)
    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) override;

    // 处理速度估计数据，将其发送给EKF。速度单位为NED坐标系下的米每秒
    // Parameters:
    // - remote_time_us: Remote timestamp in microseconds
    // - time_ms: Local timestamp in milliseconds
    // - vel: Velocity vector (NED coordinates) in meters per second
    // - reset_counter: Counter for reset events
    // - quality: Quality of velocity estimate (-1 for failed, 0 for unknown, 1 to 100 for varying levels of quality)
    void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality) override;
};

• 类继承： AP_VisualOdom_MAV 继承自 AP_VisualOdom_Backend，意味着它扩展或专门化了 AP_VisualOdom_Backend 提供的功能。

• 构造函数： AP_VisualOdom_MAV 的构造函数使用了 AP_VisualOdom_Backend 的构造函数，通过 using AP_VisualOdom_Backend::AP_VisualOdom_Backend; 实现。这使得 AP_VisualOdom_MAV 可以使用与 AP_VisualOdom_Backend 相同的参数进行初始化。

• 方法：

  • handle_pose_estimate：处理姿态估计数据，包括位置 (x, y, z)、姿态 (Quaternion)、误差 (posErr, angErr)、时间戳、重置计数器以及估计质量。

  • handle_vision_speed_estimate：处理速度估计数据，包括速度向量 (NED坐标系下)、时间戳、重置计数器以及估计质量。

上述方法可能旨在处理用于MAVLink接口的视觉里程计数据，将其转发给某导航算法，以便进一步处理和集成到飞行器的状态估计中。

但是相较于《ArduPilot开源代码之AP_VisualOdom_IntelT265》不同，似乎没有具体对应的硬件设备。

3. 重要例程

该类例程比较简单，仅有两个API：

• handle_pose_estimate
• handle_vision_speed_estimate

3.1 AP_VisualOdom_MAV::handle_pose_estimate

AP_VisualOdom_MAV::handle_pose_estimate
|
|-- const float scale_factor = _frontend.get_pos_scale();
|-- Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
|
|-- posErr = constrain_float(posErr, _frontend.get_pos_noise(), 100.0f);
|-- angErr = constrain_float(angErr, _frontend.get_yaw_noise(), 1.5f);
|
|-- // record quality
|-- _quality = quality;
|
|-- // send attitude and position to EKF if quality OK
|-- bool consume = (_quality >= _frontend.get_quality_min());
|-- if (consume) {
|--     AP::ahrs().writeExtNavData(pos, attitude, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
|-- }
|
|-- // calculate euler orientation for logging
|-- float roll;
|-- float pitch;
|-- float yaw;
|-- attitude.to_euler(roll, pitch, yaw);
|
|-- #if HAL_LOGGING_ENABLED
|-- // log sensor data
|-- Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter, !consume, _quality);
|-- #endif
|
|-- // record time for health monitoring
|-- _last_update_ms = AP_HAL::millis();

1. 位置缩放

   • const float scale_factor = _frontend.get_pos_scale();
   • 使用前端对象的方法获取位置缩放因子。

2. 位置向量

   • Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
   • 创建位置向量，并根据缩放因子调整传入的位置坐标。

3. 误差约束

   • posErr = constrain_float(posErr, _frontend.get_pos_noise(), 100.0f);
   • angErr = constrain_float(angErr, _frontend.get_yaw_noise(), 1.5f);
   • 对位置误差和角度误差进行约束，确保它们在指定的范围内。

4. 记录质量

   • _quality = quality;
   • 记录传入的质量值。

5. 发送到EKF

   • bool consume = (_quality >= _frontend.get_quality_min());
   • 检查质量是否足够高，如果是，则将姿态和位置数据发送到扩展导航数据（EKF）。

6. 计算欧拉角

   • attitude.to_euler(roll, pitch, yaw);
   • 将四元数姿态转换为欧拉角，用于日志记录。

7. 日志记录

   • #if HAL_LOGGING_ENABLED
   • 调用日志记录函数Write_VisualPosition()，记录传感器数据，包括位置、姿态、误差等信息。

8. 记录更新时间

   • _last_update_ms = AP_HAL::millis();
   • 记录最后一次更新的时间，用于健康状态监控。

3.2 AP_VisualOdom_MAV::handle_vision_speed_estimate

AP_VisualOdom_MAV::handle_vision_speed_estimate
|
|-- 记录质量
|   |-- _quality = quality;
|
|-- 如果质量合格则发送速度到EKF
|   |-- bool consume = (_quality >= _frontend.get_quality_min());
|   |-- if (consume) {
|   |   |-- AP::ahrs().writeExtNavVelData(vel, _frontend.get_vel_noise(), time_ms, _frontend.get_delay_ms());
|   |-- }
|
|-- 记录时间以进行健康监测
|   |-- _last_update_ms = AP_HAL::millis();
|
|-- 如果启用了日志记录
|   |-- Write_VisualVelocity(remote_time_us, time_ms, vel, reset_counter, !consume, _quality);

1. 记录质量

   • _quality = quality;
     将传入的质量值记录到成员变量 _quality 中。

2. 发送速度到EKF

   • bool consume = (_quality >= _frontend.get_quality_min());
     检查当前质量是否达到前端对象 _frontend 设置的最小质量要求。
   • if (consume) { ... }
     如果当前质量符合要求，则调用 AP::ahrs().writeExtNavVelData() 方法，将速度数据 vel 发送到扩展导航速度数据（EKF）处理。
     • 参数包括：速度数据 vel、速度噪声 _frontend.get_vel_noise()、时间戳 time_ms、延迟时间 _frontend.get_delay_ms()。

3. 记录时间

   • _last_update_ms = AP_HAL::millis();
     记录最后一次更新的时间戳，用于健康监测。

4. 日志记录

   • #if HAL_LOGGING_ENABLED
     如果日志记录功能启用：
     • Write_VisualVelocity(remote_time_us, time_ms, vel, reset_counter, !consume, _quality);
       调用日志记录函数 Write_VisualVelocity()，记录传感器数据的相关信息。
       • 参数包括：远程时间戳 remote_time_us、本地时间戳 time_ms、速度数据 vel、重置计数器 reset_counter、是否被忽略 !consume、质量 _quality。

4. 总结

该AP_VisualOdom_MAV类实现非常简单，似乎仅仅是完成了一个MAVLink API接口，但是不太清楚实际对应的硬件摄像头是哪个牌子。

• Any VIO hardware camera using VisualOdom_Type = 1?

5. 参考资料

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