1. 源由

AP_VisualOdom主要是配合视觉定位应用，通常视觉定位应用于室内或者无GPS等辅助定位系统的环境。

2. 类定义

AP_VisualOdom 类用于视觉里程计系统在无人机上的实现。这个类具有多种方法和属性，用于初始化和操作视觉里程计传感器。

这些私有成员变量用于存储类的内部状态，包括传感器类型、位置偏移、方向、缩放比例、延迟时间、噪声等信息，以及一个后端驱动指针 _driver。

总体来说，这个类设计用于处理和管理无人机上的视觉里程计传感器，提供了丰富的接口和功能来初始化、操作和查询传感器状态。

2.1 类与构造函数

class AP_VisualOdom
{
public:AP_VisualOdom();static AP_VisualOdom *get_singleton() {return _singleton;}

• AP_VisualOdom 类是一个包含各种方法和属性的类。
• AP_VisualOdom 构造函数用于初始化对象。
• get_singleton 静态方法返回一个单例模式的实例。

2.2 枚举类型

enum class VisualOdom_Type {None = 0,
#if AP_VISUALODOM_MAV_ENABLEDMAV = 1,
#endif
#if AP_VISUALODOM_INTELT265_ENABLEDIntelT265 = 2,VOXL = 3,
#endif
};

• VisualOdom_Type 枚举定义了几种视觉里程计类型，如 None, MAV, IntelT265 和 VOXL。

2.3 公共方法

void init();
bool enabled() const;
bool healthy() const;
enum Rotation get_orientation() const;
float get_pos_scale() const;
const Vector3f &get_pos_offset(void) const;
uint16_t get_delay_ms() const;
float get_vel_noise() const;
float get_pos_noise() const;
float get_yaw_noise() const;
int8_t get_quality_min() const;
int8_t quality() const;

这些方法提供了对视觉里程计传感器的各种操作和查询功能：

• init：检测并初始化传感器。
• enabled：返回传感器是否启用。
• healthy：返回传感器是否处于健康状态（是否接收到数据）。
• get_orientation：获取用户定义的传感器方向。
• get_pos_scale：获取应用于位置估计的缩放比例。
• get_pos_offset：获取相机相对于机体坐标系原点的位置偏移。
• get_delay_ms：获取传感器的延迟时间（毫秒）。
• get_vel_noise：获取速度测量噪声。
• get_pos_noise：获取位置测量噪声。
• get_yaw_noise：获取航向测量噪声。
• get_quality_min：获取质量阈值。
• quality：返回质量测量值。

2.4 消息处理

#if HAL_GCS_ENABLED
void handle_vision_position_delta_msg(const mavlink_message_t &msg);
#endif
void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter, int8_t 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);
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality);

这些方法用于处理视觉里程计传感器的消息：

• handle_vision_position_delta_msg：处理视觉位置增量消息（仅在 HAL_GCS_ENABLED 时可用）。
• handle_pose_estimate：处理姿态估计数据。
• handle_vision_speed_estimate：处理速度估计数据。

2.5 其他功能

void request_align_yaw_to_ahrs();
void align_position_to_ahrs(bool align_xy, bool align_z);
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
VisualOdom_Type get_type(void) const {return _type;
}

• request_align_yaw_to_ahrs：请求将传感器的航向与车辆的姿态参考系统（AHRS/EKF）对齐。
• align_position_to_ahrs：更新位置偏移以对齐AHRS位置。
• pre_arm_check：在启动检查时返回 false 表示检查失败，并提供失败消息。
• get_type：获取传感器类型。

2.6 私有成员

private:static AP_VisualOdom *_singleton;AP_Enum<VisualOdom_Type> _type;AP_Vector3f _pos_offset;AP_Int8 _orientation;AP_Float _pos_scale;AP_Int16 _delay_ms;AP_Float _vel_noise;AP_Float _pos_noise;AP_Float _yaw_noise;AP_Int8 _quality_min;AP_VisualOdom_Backend *_driver;
};

3. 框架设计

3.1 启动代码 AP_VisualOdom::init

传统Ardupilot启动方式。

AP_Vehicle::setup└──> visual_odom.init

AP_VisualOdom::init()
└── switch (VisualOdom_Type(_type.get()))├── case VisualOdom_Type::None:│   └── // do nothing├── #if AP_VISUALODOM_MAV_ENABLED│   ├── case VisualOdom_Type::MAV:│   │   └── _driver = new AP_VisualOdom_MAV(*this);│   └── #endif├── #if AP_VISUALODOM_INTELT265_ENABLED│   ├── case VisualOdom_Type::IntelT265:│   ├── case VisualOdom_Type::VOXL:│   │   └── _driver = new AP_VisualOdom_IntelT265(*this);│   └── #endif└── default:└── // handle default case if necessary

根据不同的传感器类型选择性地实例化不同的后端驱动对象，从而初始化并准备使用视觉里程计系统。

• VisualOdom_Type::None：如果 _type 是 None，则什么也不做。
• VisualOdom_Type::MAV：如果 _type 是 MAV，则创建一个 AP_VisualOdom_MAV 的实例并将其赋给 _driver。
• VisualOdom_Type::IntelT265 或 VisualOdom_Type::VOXL：如果 _type 是 IntelT265 或 VOXL，则创建一个 AP_VisualOdom_IntelT265 的实例并将其赋给 _driver。
• 其他情况：可能需要处理默认情况，具体实现取决于代码中是否有定义。

3.2 消息处理

3.2.1 AP_VisualOdom::handle_vision_position_delta_msg

SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180, 102),└──> GCS::update_receive└──> GCS_MAVLINK::update_receive└──> GCS_MAVLINK::packetReceived└──> GCS_MAVLINK::handle_message  //MAVLINK_MSG_ID_VISION_POSITION_DELTA└──> GCS_MAVLINK::handle_vision_position_delta└──> AP_VisualOdom::handle_vision_position_delta_msg

handle_vision_position_delta_msg(msg)
├── if HAL_GCS_ENABLED
│   ├── if !enabled()
│   │   └── return
│   └── if _driver != nullptr
│       └── _driver->handle_vision_position_delta_msg(msg)
└── (end of method)

• 如果 HAL_GCS_ENABLED 没有定义，整个方法将被忽略，不会编译和执行。
• 如果 enabled() 方法返回 false，则立即退出方法，不执行后续的逻辑。
• 如果 _driver 不为 nullptr，则调用 _driver 对象的 handle_vision_position_delta_msg 方法，将 msg 作为参数传递给它。

这样的设计确保了在满足条件且传感器已启用时，有效地将接收到的视觉位置增量消息传递给相应的后端处理。

3.2.2 AP_VisualOdom::handle_pose_estimate

SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180, 102),└──> GCS::update_receive└──> GCS_MAVLINK::update_receive└──> GCS_MAVLINK::packetReceived└──> GCS_MAVLINK::handle_message //MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE/MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE/MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE└──> GCS_MAVLINK::handle_vision_position_estimate/handle_global_vision_position_estimate/handle_vicon_position_estimate└──> GCS_MAVLINK::handle_common_vision_position_estimate_data└──> AP_VisualOdom::handle_pose_estimate

handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter, int8_t quality)
│
├── if (!enabled())
│   └── return;
│
└── if (_driver != nullptr)│├── Quaternion attitude│   └── attitude.from_euler(roll, pitch, yaw)│└── _driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter, quality)

这段代码是用于处理姿态估计数据并将其发送给扩展卡尔曼滤波器（EKF）的方法。
目的是将来自视觉里程计传感器的姿态估计数据传送给后端处理，以便进一步的状态估计和导航。

1. 参数：

   • remote_time_us：远程时间戳（微秒）。
   • time_ms：本地时间戳（毫秒）。
   • x, y, z：位置坐标（米）。
   • roll, pitch, yaw：姿态角（弧度）。
   • posErr：位置误差。
   • angErr：角度误差。
   • reset_counter：重置计数器。
   • quality：质量评估（-1 表示失败，0 表示未知，1 最差，100 最佳）。

2. 功能：

   • enabled() 方法检查传感器是否启用，如果未启用则立即返回。
   • 如果 _driver 驱动器不为空，创建一个 Quaternion 对象 attitude，通过 from_euler(roll, pitch, yaw) 方法将欧拉角转换为四元数。
   • 调用 _driver->handle_pose_estimate() 方法，将处理过的姿态估计数据和质量评估传递给后端处理。

3. API：

   • uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter, int8_t qualit
   • 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

// general purpose method to consume position estimate data and send to EKF
// distances in meters, roll, pitch and yaw are in radians
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter, int8_t quality)
{// exit immediately if not enabledif (!enabled()) {return;}// call backendif (_driver != nullptr) {// convert attitude to quaternion and call backendQuaternion attitude;attitude.from_euler(roll, pitch, yaw);_driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter, quality);}
}// general purpose method to consume position estimate data and send to EKF
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
void AP_VisualOdom::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)
{// exit immediately if not enabledif (!enabled()) {return;}// call backendif (_driver != nullptr) {_driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter, quality);}
}

3.2.3 AP_VisualOdom::handle_vision_speed_estimate

SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180, 102),└──> GCS::update_receive└──> GCS_MAVLINK::update_receive└──> GCS_MAVLINK::packetReceived└──> GCS_MAVLINK::handle_message  //MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE/MAVLINK_MSG_ID_ODOMETRY└──> GCS_MAVLINK::handle_vision_speed_estimate/handle_odometry└──> AP_VisualOdom::handle_vision_speed_estimate

handle_vision_speed_estimate(remote_time_us, time_ms, vel, reset_counter, quality)
└── if not enabled()└── return
└── if _driver != nullptr└── _driver->handle_vision_speed_estimate(remote_time_us, time_ms, vel, reset_counter, quality)

用于处理视觉里程计传感器的速度估计数据，并将其发送到扩展卡尔曼滤波器（EKF）的通用方法。

1. 函数签名和说明：

   • 函数名：handle_vision_speed_estimate
   • 参数：
     • remote_time_us：远程时间戳（微秒）
     • time_ms：本地时间戳（毫秒）
     • vel：速度估计的三维向量（NED坐标系，单位为米每秒）
     • reset_counter：重置计数器
     • quality：质量评估，-1 表示失败，0 表示未知，1 表示最差，100 表示最佳。

2. 功能说明：

   • 首先，检查视觉里程计是否启用（调用了类的 enabled() 方法）。如果未启用，则立即返回，不执行后续操作。
   • 然后，检查 _driver 指针是否为 nullptr。如果 _driver 不为空，则调用 _driver 对象的 handle_vision_speed_estimate 方法，将速度估计数据传递给后端处理。

3. 执行流程：

   • 如果视觉里程计未启用，函数直接返回，不执行后续任何操作。
   • 如果视觉里程计已启用且 _driver 指针有效，则通过 _driver 对象处理速度估计数据。

3.3 辅助函数

3.3.1 AP_VisualOdom::enabled

通过判断类型设置，确认视觉传感是否被使能。

// return true if sensor is enabled
bool AP_VisualOdom::enabled() const
{return ((_type != VisualOdom_Type::None));
}

3.3.2 AP_VisualOdom::healthy

首先检查传感器是否已启用，然后再检查是否有有效的驱动程序实例来处理传感器数据。
只有在这两个条件都满足时，才会进一步检查传感器的健康状态并返回结果。

healthy()
├── if (!enabled())
│   └── return false
├── if (_driver == nullptr)
│   └── return false
└── return _driver->healthy()

3.3.3 AP_VisualOdom::quality

返回质量测量值（百分比）。

• 0：Unknown
• 1：Worst
• 100：Best

// return quality as a measure from 0 ~ 100
// -1 means failed, 0 means unknown, 1 is worst, 100 is best
int8_t AP_VisualOdom::quality() const
{if (_driver == nullptr) {return 0;}return _driver->quality();
}

3.4 参数接口

3.4.1 AP_VisualOdom::get_orientation

获取用户定义的传感器方向。

    // get user defined orientationenum Rotation get_orientation() const { return (enum Rotation)_orientation.get(); }

3.4.2 AP_VisualOdom::get_pos_scale

获取应用于位置估计的缩放比例。

    // get user defined scaling applied to position estimatesfloat get_pos_scale() const { return _pos_scale; }

3.4.3 AP_VisualOdom::get_pos_offset

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

    // return a 3D vector defining the position offset of the camera in meters relative to the body frame originconst Vector3f &get_pos_offset(void) const { return _pos_offset; }

3.4.4 AP_VisualOdom::get_vel_noise

获取速度测量噪声。

    // return velocity measurement noise in m/sfloat get_vel_noise() const { return _vel_noise; }

3.4.5 AP_VisualOdom::get_pos_noise

获取位置测量噪声。

    // return position measurement noise in mfloat get_pos_noise() const { return _pos_noise; }

3.4.6 AP_VisualOdom::get_yaw_noise

获取航向测量噪声。

    // return yaw measurement noise in radfloat get_yaw_noise() const { return _yaw_noise; }

3.4.7 AP_VisualOdom::get_delay_ms

获取传感器的延迟时间（毫秒）。

    // return the sensor delay in milliseconds (see _DELAY_MS parameter)uint16_t get_delay_ms() const { return MAX(0, _delay_ms); }

3.4.8 AP_VisualOdom::get_quality_min

后端驱动获取配置最小质量阈值。

    // return quality thresholdint8_t get_quality_min() const { return _quality_min; }

4. 总结

AP_VisualOdom视觉定位传感器应用遵循《ArduPilot之开源代码Sensor Drivers设计》。

• ront-end / back-end分层
• 消息机制驱动
• 评估视觉位置、姿态、速度

后续我们继续研讨三个后端程序：

• AP_VisualOdom_Backend
• AP_VisualOdom_IntelT265
• AP_VisualOdom_MAV

5. 参考资料

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