ArduPilot开源代码之AP_VisualOdom_Backend

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);
  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设计

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/bicheng/42554.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

Android 获取当前电池状态

在 API 级别 23 上获取充电状态 要在 API 级别 23 上获取电池的当前状态&#xff0c;只需使用电池管理器系统服务&#xff1a; BatteryManager batteryManager (BatteryManager) getSystemService(BATTERY_SERVICE); boolean isCharging batteryManager.isCharging();使用 S…

2024暑假集训

Day1——枚举 Day2——测试 Day3——贪心 Day4、5——测试 ——————————————————————————————————————————— Day3T7&Day5T7:没思路 Day3T8:不知道怎么排序筛选 Day5T5:没有算法难度&#xff0c;但是不知道怎么处理2队奶牛的情…

Python 访问和设置私有属性

Python __init__ 初始化函数中在属性名前面加2条下划线的形式表示把当前属性设置为私有实例属性后&#xff0c;在外部&#xff08;指创建当前类的实例对象&#xff09;如何进行访问并更新该属性值&#xff1f; 首先创建一个 Person类&#xff0c;在类中通过 设置2个实例函数去…

什么牌子的头戴式蓝牙耳机好性价比高?

说起性价比高的头戴式蓝牙耳机,就不得不提倍思H1s,作为倍思最新推出的新款,在各项功能上都实现了不错的升级,二字开头的价格,配置却毫不含糊, 倍思H1s的音质表现堪称一流。它采用了40mm天然生物纤维振膜,这种振膜柔韧而有弹性,能够显著提升低音的量感。无论是深沉的低音还是清…

qt播放视频

在Qt中播放视频&#xff0c;通常可以使用QMediaPlayer和QVideoWidget这两个类。QMediaPlayer用于控制视频的播放&#xff0c;而QVideoWidget则用于显示视频。 以下是一个简单的示例&#xff0c;展示了如何使用Qt播放视频&#xff1a; cpp复制代码 #include <QApplication…

算法训练(leetcode)第二十六天 | 452. 用最少数量的箭引爆气球、435. 无重叠区间、763. 划分字母区间

刷题记录 452. 用最少数量的箭引爆气球思路一思路二 435. 无重叠区间763. 划分字母区间 452. 用最少数量的箭引爆气球 leetcode题目地址 思路一 先按起始坐标从小到大排序。排序后找交集并将交集存入一个数组中&#xff0c;遍历气球数组从交集数组中找交集&#xff0c;找到与…

空对象模式在金融业务中的应用及其框架实现

引言 空对象模式&#xff08;Null Object Pattern&#xff09;是一种行为设计模式&#xff0c;它通过使用一个无操作行为的对象来替代NULL&#xff0c;从而避免对空值进行显式的检查。空对象模式可以简化代码&#xff0c;避免因空值导致的空指针异常。在金融业务中&#xff0c…

数据跨境法案:美国篇上

近年来随着全球数字化的加速发展&#xff0c;数据已成为国家竞争力的重要基石。在这样的背景下&#xff0c;中国软件和技术出海的场景日益丰富。本系列邀请到在跨境数据方面的研究人员针对海外的数据跨境政策进行解读。 本期将针对美国对数据跨境流动的态度和政策进行阐释。过…

非比较排序 计数排序

1.核心思路 首先要找出max 和 min&#xff0c;最大值 - 最小值 1&#xff0c;就可以计算出数据在什么范围然后创建计数数组大小&#xff0c;a[i] - min 在数组的相对位置计数 通过自然序列排序然后把计数好的值&#xff0c;按照顺序依次放回原数组即可 动图解释&#xff0c;其…

Linux—网络设置

目录 一、ifconfig——查看网络配置 1、查看网络接口信息 1.1、查看所有网络接口 1.2、查看具体的网络接口 2、修改网络配置 3、添加网络接口 4、禁用/激活网卡 二、hostname——查看主机名称 1、查看主机名称 2、临时修改主机名称 3、永久修改主机名称 4、查看本…

Java引用的4种类型:强、软、弱、虚

在Java中&#xff0c;引用的概念不仅限于强引用&#xff0c;还包括软引用、弱引用和虚引用&#xff08;也称为幻影引用&#xff09;。这些引用类型主要用于不同的内存管理策略&#xff0c;尤其是在垃圾收集过程中。以下是对这四种引用类型的详细解释&#xff1a; 1. 强引用&am…

algorithm算法库学习之——不修改序列的操作

algorithm此头文件是算法库的一部分。本篇介绍不修改序列的操作函数。 不修改序列的操作 all_ofany_ofnone_of (C11)(C11)(C11) 检查谓词是否对范围中所有、任一或无元素为 true (函数模板) for_each 应用函数到范围中的元素 (函数模板) for_each_n (C17) 应用一个函数对象到序…

一.7.(2)基本运算电路,包括比例运算电路、加减运算电路、积分运算电路、微分电路等常见电路的分析、计算及应用;(未完待续)

what id the 虚短虚断虚地? 虚短&#xff1a;运放的正相输入端和反相输入端貌似连在一起了&#xff0c;所以两端的电压相等&#xff0c;即UU- 虚断&#xff1a;输入端输入阻抗无穷大 虚地&#xff1a;运放正相输入端接地&#xff0c;导致U&#xff1d;U-&#xff1d;0。 虚…

魔法自如:精通 IPython %automagic 命令的切换艺术

魔法自如&#xff1a;精通 IPython %automagic 命令的切换艺术 在 IPython 的神奇世界里&#xff0c;魔术命令是其强大交互功能的核心。这些以 % 或 %% 开头的命令&#xff0c;能够执行一系列特殊的操作&#xff0c;从而增强用户的编程体验。但是&#xff0c;你是否知道&#…

对话大模型Prompt是否需要礼貌点?

大模型相关目录 大模型&#xff0c;包括部署微调prompt/Agent应用开发、知识库增强、数据库增强、知识图谱增强、自然语言处理、多模态等大模型应用开发内容 从0起步&#xff0c;扬帆起航。 基于Dify的QA数据集构建&#xff08;附代码&#xff09;Qwen-2-7B和GLM-4-9B&#x…

秋招突击——7/5——复习{}——新作{跳跃游戏II、划分字母区间、数组中的第K个大的元素(模板题,重要)、前K个高频元素}

文章目录 引言正文贪心——45 跳跃游戏II个人实现参考实现 划分字母区间个人实现参考实现 数组中的第K个最大元素个人实现参考做法 前K个高频元素个人实现参考实现 总结 引言 今天就开始的蛮早的&#xff0c;现在是九点多&#xff0c;刚好开始做算法&#xff0c;今天有希望能够…

【leetcode周赛记录——405】

405周赛记录 #1.leetcode100339_找出加密后的字符串2.leetcode100328_生成不含相邻零的二进制字符串3.leetcode100359_统计X和Y频数相等的子矩阵数量4.leetcode100350_最小代价构造字符串 刷了一段时间算法了&#xff0c;打打周赛看看什么水平了 #1.leetcode100339_找出加密后的…

【UML用户指南】-30-对体系结构建模-模式和框架

目录 1、机制 2、框架 3、常用建模技术 3.1、对设计模式建模 3.2、对体系结构模式建模 用模式来详述形成系统体系结构的机制和框架。通过清晰地标识模式的槽、标签、按钮和刻度盘 在UML中&#xff0c; 对设计模式&#xff08;也叫做机制&#xff09;建模&#xff0c;将它…

【web前端HTML+CSS+JS】--- CSS学习笔记02

一、CSS&#xff08;层叠样式表&#xff09;介绍 1.优势 2.定义解释 如果有多个选择器共同作用的话&#xff0c;只有优先级最高那层样式决定最终的效果 二、无语义化标签 div和span&#xff1a;只起到描述的作用&#xff0c;不带任何样式 三、标签选择器 1.标签/元素选择器…

【算法笔记自学】第 8 章 提高篇(2)——搜索专题

8.1深度优先搜索&#xff08;DFS&#xff09; #include <cstdio>const int MAXN 5; int n, m, maze[MAXN][MAXN]; bool visited[MAXN][MAXN] {false}; int counter 0;const int MAXD 4; int dx[MAXD] {0, 0, 1, -1}; int dy[MAXD] {1, -1, 0, 0};bool isValid(int …