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 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);
-
解码消息
- 定义
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 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设计