ArduPilot开源代码之AP_VisualOdom_Backend

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 300msreturn ((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 bestint8_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);

1. 解码消息

   • 定义mavlink_vision_position_delta_t packet;
   • 调用mavlink_msg_vision_position_delta_decode(&msg, &packet);将消息解码到packet中。

2. 角度和位置增量的旋转应用

   • 获取当前的旋转方式const enum Rotation rot = _frontend.get_orientation();
   • 创建Vector3f angle_delta并使用消息中的角度增量初始化。
     • 调用angle_delta.rotate(rot);应用旋转。
   • 创建Vector3f position_delta并使用消息中的位置增量初始化。
     • 调用position_delta.rotate(rot);应用旋转。

3. 获取当前时间

   • 调用AP_HAL::millis();获取当前时间并保存到now_ms。
   • 更新_last_update_ms为当前时间now_ms。

4. 发送到EKF

   • 如果启用了AP_AHRS或日志记录，计算时间增量const float time_delta_sec = packet.time_delta_usec * 1.0E-6;
   • 如果启用了AP_AHRS，调用AP::ahrs().writeBodyFrameOdom()方法，将姿态和位置增量数据发送到EKF。
     • 传递参数包括：置信度、位置增量、角度增量、时间增量、当前时间、前端的延迟和位置偏移。

5. 日志记录

   • 如果启用了日志记录，调用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 changedif (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 bestvirtual 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 bestvirtual 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 attitudevirtual void request_align_yaw_to_ahrs() {}// handle request to align position with AHRSvirtual void align_position_to_ahrs(bool align_xy, bool align_z) {}// arming check - by default no checks performedvirtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const { return true; }

5. 参考资料

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