ArduPilot开源代码之AP_DAL_InertialSensor

ArduPilot开源代码之AP_DAL_InertialSensor

  • 1. 源由
  • 2. 框架设计
    • 2.1 类定义和成员变量
    • 2.2 公共方法
    • 2.3 构造函数和其他方法
    • 2.4 私有成员变量
  • 3. 重要例程
    • 3.1 InertialSensor-like 方法
      • 3.1.1 get_loop_rate_hz
      • 3.1.2 get_imu_pos_offset
    • 3.2 accel 方法
      • 3.2.1 get_accel_count
      • 3.2.2 get_first_usable_accel
      • 3.2.3 use_accel
      • 3.2.4 get_accel
      • 3.2.5 get_delta_velocity
    • 3.3 gyro 方法
      • 3.3.1 get_gyro_count
      • 3.3.2 get_first_usable_gyro
      • 3.3.3 use_gyro
      • 3.3.4 get_gyro
      • 3.3.5 get_delta_angle
    • 3.4 其他函数
      • 3.4.1 AP_DAL_InertialSensor
      • 3.4.2 get_loop_delta_t
      • 3.4.3 start_frame
      • 3.4.4 handle_message
      • 3.4.5 update_filtered
  • 4. 总结
  • 5. 参考资料

1. 源由

该类主要用于管理惯性传感器的数据，包括加速度计和陀螺仪。它提供了获取传感器数据和状态的方法，以及处理传感器消息的机制。

通过这些方法，可以访问传感器的当前状态、获取过滤后的数据以及处理新的传感器数据。

2. 框架设计

  • 该类通过多个方法对不同的传感器数据进行访问和操作，包括加速度计和陀螺仪。
  • 使用了消息处理机制，通过handle_message方法来更新传感器数据。
  • 提供了获取主要传感器参数的方法，例如循环频率、加速度计和陀螺仪的数量、位置偏移等。
  • 过滤后的数据通过私有成员变量存储，并通过公共方法提供访问接口。

2.1 类定义和成员变量

class AP_DAL_InertialSensor {
public:// 公共方法uint16_t get_loop_rate_hz(void) const { return _RISH.loop_rate_hz; }const Vector3f &get_imu_pos_offset(uint8_t instance) const { return pos[instance]; }uint8_t get_accel_count(void) const { return _RISH.accel_count; }uint8_t get_first_usable_accel(void) const { return _RISH.first_usable_accel; };bool use_accel(uint8_t instance) const { return _RISI[instance].use_accel; }const Vector3f &get_accel(uint8_t i) const { return accel_filtered[i]; }bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity, float &delta_velocity_dt) const {delta_velocity = _RISI[i].delta_velocity;delta_velocity_dt = _RISI[i].delta_velocity_dt;return _RISI[i].get_delta_velocity_ret;}uint8_t get_gyro_count(void) const { return _RISH.gyro_count; }uint8_t get_first_usable_gyro(void) const { return _RISH.first_usable_gyro; };bool use_gyro(uint8_t instance) const { return _RISI[instance].use_gyro; }const Vector3f &get_gyro(uint8_t i) const { return gyro_filtered[i]; }const Vector3f &get_gyro() const { return get_gyro(_primary_gyro); }bool get_delta_angle(uint8_t i, Vector3f &delta_angle, float &delta_angle_dt) const {delta_angle = _RISI[i].delta_angle;delta_angle_dt = _RISI[i].delta_angle_dt;return _RISI[i].get_delta_angle_ret;}float get_loop_delta_t(void) const { return _RISH.loop_delta_t; }AP_DAL_InertialSensor();void start_frame();void handle_message(const log_RISH &msg) {_RISH = msg;}void handle_message(const log_RISI &msg) {_RISI[msg.instance] = msg;pos[msg.instance] = AP::ins().get_imu_pos_offset(msg.instance);update_filtered(msg.instance);}private:struct log_RISH _RISH;struct log_RISI _RISI[INS_MAX_INSTANCES];float alpha;Vector3f pos[INS_MAX_INSTANCES];Vector3f gyro_filtered[INS_MAX_INSTANCES];Vector3f accel_filtered[INS_MAX_INSTANCES];uint8_t _primary_gyro;void update_filtered(uint8_t i);
};

2.2 公共方法

- `get_loop_rate_hz()`: 返回采样的循环频率。
- `get_imu_pos_offset(instance)`: 返回指定实例的IMU位置偏移。
- `get_accel_count()`: 返回加速度计数量。
- `get_first_usable_accel()`: 返回第一个可用加速度计的索引。
- `use_accel(instance)`: 检查是否使用指定实例的加速度计。
- `get_accel(i)`: 返回过滤后的加速度数据。
- `get_delta_velocity(i, delta_velocity, delta_velocity_dt)`: 获取指定实例的速度变化量及其时间间隔。
- `get_gyro_count()`: 返回陀螺仪数量。
- `get_first_usable_gyro()`: 返回第一个可用陀螺仪的索引。
- `use_gyro(instance)`: 检查是否使用指定实例的陀螺仪。
- `get_gyro(i)`: 返回过滤后的陀螺仪数据。
- `get_gyro()`: 返回主陀螺仪的数据。
- `get_delta_angle(i, delta_angle, delta_angle_dt)`: 获取指定实例的角度变化量及其时间间隔。
- `get_loop_delta_t()`: 返回主循环的时间间隔。

2.3 构造函数和其他方法

- `AP_DAL_InertialSensor()`: 构造函数，可能包含一些初始化逻辑（未展示具体实现）。
- `start_frame()`: 启动新的帧处理（具体实现未展示）。
- `handle_message(const log_RISH &msg)`: 处理RISH类型的消息，更新_RISH数据。
- `handle_message(const log_RISI &msg)`: 处理RISI类型的消息，更新_RISI数组中特定实例的数据，并更新位置和过滤后的数据。

2.4 私有成员变量

- `_RISH`: 存储整体的传感器头信息。  //Real-time Inertial Sensor Header
- `_RISI[]`: 存储各个传感器实例的信息。  //Real-time Inertial Sensor Instance
- `alpha`: 一个浮点型变量，可能用于滤波器。
- `pos[]`: 存储各个传感器的位置偏移。
- `gyro_filtered[]`: 存储各个传感器的过滤后的陀螺仪数据。
- `accel_filtered[]`: 存储各个传感器的过滤后的加速度数据。
- `_primary_gyro`: 存储主陀螺仪的索引。
- `update_filtered(uint8_t i)`: 更新特定传感器实例的过滤后的数据（具体实现未展示）。

3. 重要例程

3.1 InertialSensor-like 方法

3.1.1 get_loop_rate_hz

返回采样可用的选定循环频率（以赫兹为单位）

    uint16_t get_loop_rate_hz(void) const { return _RISH.loop_rate_hz; }

3.1.2 get_imu_pos_offset

获取IMU位置偏移值

    const Vector3f &get_imu_pos_offset(uint8_t instance) const {return pos[instance];}

3.2 accel 方法

3.2.1 get_accel_count

获取ACC实例数量

    uint8_t get_accel_count(void) const { return _RISH.accel_count; }

3.2.2 get_first_usable_accel

获取可用ACC实例

    uint8_t get_first_usable_accel(void) const { return _RISH.first_usable_accel; };

3.2.3 use_accel

获取ACC指定实例有效性

    bool use_accel(uint8_t instance) const { return _RISI[instance].use_accel; }

3.2.4 get_accel

获取指定ACC实例矢量值

    const Vector3f     &get_accel(uint8_t i) const { return accel_filtered[i]; }

3.2.5 get_delta_velocity

获取指定ACC实例delta值

    bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity, float &delta_velocity_dt) const {delta_velocity = _RISI[i].delta_velocity;delta_velocity_dt = _RISI[i].delta_velocity_dt;return _RISI[i].get_delta_velocity_ret;}

3.3 gyro 方法

3.3.1 get_gyro_count

获取GYRO实例数量

    uint8_t get_gyro_count(void) const { return _RISH.gyro_count; }

3.3.2 get_first_usable_gyro

获取可用GYRO实例

    uint8_t get_first_usable_gyro(void) const { return _RISH.first_usable_gyro; };

3.3.3 use_gyro

获取GYRO指定实例有效性

    bool use_gyro(uint8_t instance) const { return _RISI[instance].use_gyro; }

3.3.4 get_gyro

获取指定GYRO实例矢量值 或 获取默认GYRO实例矢量值

    const Vector3f     &get_gyro(uint8_t i) const { return gyro_filtered[i]; }const Vector3f     &get_gyro() const { return get_gyro(_primary_gyro); }

3.3.5 get_delta_angle

获取指定GYRO实例delta值

    bool get_delta_angle(uint8_t i, Vector3f &delta_angle, float &delta_angle_dt) const {delta_angle = _RISI[i].delta_angle;delta_angle_dt = _RISI[i].delta_angle_dt;return _RISI[i].get_delta_angle_ret;}

3.4 其他函数

3.4.1 AP_DAL_InertialSensor

构造函数初始化实例编号

AP_DAL_InertialSensor::AP_DAL_InertialSensor()
{for (uint8_t i=0; i<ARRAY_SIZE(_RISI); i++) {_RISI[i].instance = i;}
}

3.4.2 get_loop_delta_t

返回主循环的时间增量（以秒为单位）

    float get_loop_delta_t(void) const { return _RISH.loop_delta_t; }

3.4.3 start_frame

AP_DAL::start_frame└──> AP_DAL_InertialSensor::start_frame

void AP_DAL_InertialSensor::start_frame()
{const auto &ins = AP::ins();const log_RISH old_RISH = _RISH;// 更新主循环的频率（以 Hz 为单位）_RISH.loop_rate_hz = ins.get_loop_rate_hz();// 更新第一个可用陀螺仪的索引_RISH.first_usable_gyro = ins.get_first_usable_gyro();// 更新主循环的时间增量（以秒为单位）_RISH.loop_delta_t = ins.get_loop_delta_t();// 更新第一个可用加速度计的索引_RISH.first_usable_accel = ins.get_first_usable_accel();// 更新加速度计的数量_RISH.accel_count = ins.get_accel_count();// 更新陀螺仪的数量_RISH.gyro_count = ins.get_gyro_count();// 如果 RISH 结构体内容有变化，则写入重播数据块WRITE_REPLAY_BLOCK_IFCHANGED(RISH, _RISH, old_RISH);// 遍历每个传感器for (uint8_t i=0; i<ARRAY_SIZE(_RISI); i++) {log_RISI &RISI = _RISI[i];const log_RISI old_RISI = RISI;// 加速度计数据RISI.use_accel = ins.use_accel(i);if (RISI.use_accel) {// 获取加速度计的速度增量及其时间增量RISI.get_delta_velocity_ret = ins.get_delta_velocity(i, RISI.delta_velocity, RISI.delta_velocity_dt);}// 陀螺仪数据RISI.use_gyro = ins.use_gyro(i);if (RISI.use_gyro) {// 获取陀螺仪的角度增量及其时间增量RISI.get_delta_angle_ret = ins.get_delta_angle(i, RISI.delta_angle, RISI.delta_angle_dt);}update_filtered(i);// 如果 RISI 结构体内容有变化，则写入重播数据块WRITE_REPLAY_BLOCK_IFCHANGED(RISI, RISI, old_RISI);// 更新传感器的位置偏移pos[i] = ins.get_imu_pos_offset(i);}
}

3.4.4 handle_message

AP_DAL::handle_message└──> AP_DAL_InertialSensor::handle_message

    void handle_message(const log_RISH &msg) {_RISH = msg;}

3.4.5 update_filtered

更新滤波后的陀螺仪和加速度计数据

void AP_DAL_InertialSensor::update_filtered(uint8_t i)
{
if (!is_positive(alpha)) {
// 为EKF滤波后的加速度计和陀螺仪使用恒定的10Hz，使EKF独立于INS滤波器设置
const float cutoff_hz = 10.0;
alpha = calc_lowpass_alpha_dt(get_loop_delta_t(), cutoff_hz);
}
if (is_positive(_RISI[i].delta_angle_dt)) {
gyro_filtered[i] += ((_RISI[i].delta_angle/_RISI[i].delta_angle_dt) - gyro_filtered[i]) * alpha;
}
if (is_positive(_RISI[i].delta_velocity_dt)) {
accel_filtered[i] += ((_RISI[i].delta_velocity/_RISI[i].delta_velocity_dt) - accel_filtered[i]) * alpha;
}
}

4. 总结

AP_DAL_InertialSensor主要管理了加速度计和陀螺仪数据，并提供访问接口进行直接状态访问。

5. 参考资料

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

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

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

相关文章

无人机监测的必要性及方法

无人机监测的必要性及方法

为什么需要无人机探测&#xff1f; 无人机的快速发展和广泛使用为各个行业带来了巨大好处&#xff0c;包括送货服务、农业和监控。然而&#xff0c;这种扩散也导致滥用现象增多&#xff0c;造成非法入侵空域、侵犯隐私和安全威胁。监控和探测在特定空域盘旋的无人机的能力变得…
阅读更多...
Linux C++ 058-设计模式之解释器模式

Linux C++ 058-设计模式之解释器模式

Linux C 058-设计模式之解释器模式 本节关键字&#xff1a;Linux、C、设计模式、解释器模式 相关库函数&#xff1a; 概念 解释器模式&#xff08;Interpreter Pattern&#xff09;提供了评估语言的语法或表达式的方式&#xff0c;它属于行为型模式。 解释器模式用于构建一…
阅读更多...
python数据可视化（9）——绘制小提琴图

python数据可视化（9）——绘制小提琴图

课程学习来源&#xff1a;b站up&#xff1a;【蚂蚁学python】 【课程链接&#xff1a;【【数据可视化】Python数据图表可视化入门到实战】】 【课程资料链接&#xff1a;【链接】】 python&#xff1a;3.12.3 所有库都使用最新版。 Python绘制小提琴图 小提琴图(violin plot…
阅读更多...
理想主义者

理想主义者

我自认为不是一个理想主义者&#xff0c;可能更多的是一个实用主义者&#xff0c;做了这么多年的产品我更多的是关注产品的落地。但这并不妨碍我对理想主义者的敬重。 偏见只是偏见&#xff0c;微信之父张小龙的人生并不会被各种偏见左右。当所有人都在说张小龙迷茫时&#xf…
阅读更多...
【经验总结】将markdown文档转换为word（swagger导出word）

【经验总结】将markdown文档转换为word（swagger导出word）

工具准备&#xff1a; 任意markdown编辑器&#xff0c;以typora为例pandoc&#xff0c;官方下载地址 思路整理&#xff1a; 从swagger提取离线md文档将md文档转换为word格式 操作步骤&#xff1a; 一、安装pandoc &#xff08;markdown编辑器安装略&#xff09; 前往官网…
阅读更多...
Yak与nuclei的深度融合：打造高效漏扫生态，解锁PoC管理新姿势

Yak与nuclei的深度融合：打造高效漏扫生态，解锁PoC管理新姿势

在Yakit中使用nuclei很简单&#xff0c;只需要几行代码。在Yak Runner中&#xff0c;使用下面代码&#xff0c;指定扫描的目标与选项&#xff0c;便能调用nuclei的漏扫能力&#xff1a; results:nuclei.Scan(target,opts...)~for result in results { dump(result)} 比如以…
阅读更多...
UNiapp微信小程序Ucharts

UNiapp微信小程序Ucharts

效果图如下 以上为加载接口所得数据的玫瑰图与折线图 具体步骤如下 1&#xff0c;将插件导入Hbuiler 所需要的项目中&#xff08;插件地址&#xff1a;秋云 ucharts echarts 高性能跨全端图表组件 - DCloud 插件市场&#xff09; 2&#xff0c;导入成功是这样的 3&#xff0c…
阅读更多...
java 根据当前时间获取 yyyy-MM-dd HH:mm:ss 标准格式的时间

java 根据当前时间获取 yyyy-MM-dd HH:mm:ss 标准格式的时间

在Java中&#xff0c;可以使用java.time包中的LocalDateTime类和DateTimeFormatter类来获取并格式化当前时间为yyyy-MM-dd HH:mm:ss格式。 代码示例 以下是如何获取当前时间并格式化为yyyy-MM-dd HH:mm:ss格式的完整示例&#xff1a; import java.time.LocalDateTime; impor…
阅读更多...
app的进程启动为什么不是init的fork，而是zygote的fork

app的进程启动为什么不是init的fork，而是zygote的fork

在Android系统中&#xff0c;应用程序&#xff08;App&#xff09;的进程启动不是通过init进程的fork&#xff0c;而是由Zygote进程的fork来完成的&#xff0c;这主要是出于性能和资源利用优化的考虑。以下是详细的原因分析&#xff1a; 一、init进程的角色 初始化系统&#…
阅读更多...
2024-07-16升级问题：调用自带软件打开文件时 android.os.FileUriExposedException

2024-07-16升级问题：调用自带软件打开文件时 android.os.FileUriExposedException

2024-07-16升级问题&#xff1a;调用手机自带软件打开文件时&#xff0c;出现以下问题&#xff1a; E/AndroidRuntime: FATAL EXCEPTION: mainProcess: rs.tabletcropland, PID: 10997android.os.FileUriExposedException: file:///storage/emulated/0/arcgis/%E7%9F%B3%E7%8B…
阅读更多...
ES6基本语法（二）——函数与数组

ES6基本语法（二）——函数与数组

函数 函数是JavaScript中组织代码的一种方式&#xff0c;它可以提高代码的复用性&#xff0c;并使其更加模块化。 在<script>标签中定义函数 你可以在HTML文件的<script>标签内定义函数&#xff0c;或者在JavaScript文件中定义后再引入到HTML中。 <script&g…
阅读更多...
相对定位语法：css+xpath基础语法使用-定位页面元素

相对定位语法：css+xpath基础语法使用-定位页面元素

文章目录 CSS相对定位获取元素关系定位顺序关系 XPath相对定位基础语法顺序关系-通过索引获取元素选取元素 总结 ✨✨✨学习的道路很枯燥&#xff0c;希望我们能并肩走下来&#xff01; 编程真是一件很奇妙的东西。你只是浅尝辄止&#xff0c;那么只会觉得枯燥乏味&#xff0c…
阅读更多...
HTML5应用的安全防护策略与实践

HTML5应用的安全防护策略与实践

随着HTML5及其相关技术&#xff08;如CSS3和JavaScript&#xff09;的普及&#xff0c;Web应用变得越来越强大和复杂&#xff0c;同时也成为黑客攻击的目标。本文将探讨HTML5应用面临的常见安全威胁&#xff0c;以及如何通过最佳实践和代码示例来增强应用的安全性。 HTML5安全…
阅读更多...
uniapp小程序上传pdf文件

uniapp小程序上传pdf文件

<template><view class="mainInnBox"><view class="formBox"><!-- 注意，如果需要兼容微信小程序，最好通过setRules方法设置rules规则 --><u-form :model="form" ref="uForm" :rules="rules"&g…
阅读更多...
用于可穿戴传感器的人类活动识别、健康监测和行为建模的大型语言模型

用于可穿戴传感器的人类活动识别、健康监测和行为建模的大型语言模型

这篇论文题为《用于可穿戴传感器的人类活动识别、健康监测和行为建模的大型语言模型&#xff1a;早期趋势、数据集和挑战的综述》&#xff0c;由埃米利奥费拉拉&#xff08;Emilio Ferrara&#xff09;撰写。论文主要内容如下&#xff1a; 摘要 可穿戴技术的普及使得传感器数…
阅读更多...
韦东山嵌入式linux系列-具体单板的 LED 驱动程序

韦东山嵌入式linux系列-具体单板的 LED 驱动程序

笔者使用的是STM32MP157的板子 1 怎么写 LED 驱动程序&#xff1f; 详细步骤如下&#xff1a; ① 看原理图确定引脚&#xff0c;确定引脚输出什么电平才能点亮/熄灭 LED ② 看主芯片手册&#xff0c;确定寄存器操作方法&#xff1a;哪些寄存器&#xff1f;哪些位&#xff1f;…
阅读更多...
STM32 BootLoader 刷新项目 (三) 程序框架搭建及刷新演示

STM32 BootLoader 刷新项目 (三) 程序框架搭建及刷新演示

STM32 Customer BootLoader 刷新项目 (三) 程序框架搭建 文章目录 STM32 Customer BootLoader 刷新项目 (三) 程序框架搭建典型工作流程 1. 硬件原理图介绍1.1 USART硬件介绍1.2 LED和按键介绍 2. STM32 CubeMX工程搭建2.1 创建工程2.2 系统配置2.3 USART串口配置2.4 配置按键G…
阅读更多...
GD32 MCU上电跌落导致启动异常如何解决

GD32 MCU上电跌落导致启动异常如何解决

大家是否碰到过MCU上电过程中存在电源波动或者电压跌落导致MCU启动异常的问题&#xff1f;本视频将会为大家讲解可能的原因以及解决方法&#xff1a; GD32 MCU上下电复位波形如下图所示&#xff0c;上电过程中如果存在吃电的模块&#xff0c;比如wifi模块/4G模块/开启某块电路…
阅读更多...
【pytorch】thread: [16,0,0] Assertion `t ＞= 0 t ＜ n_classes` failed

【pytorch】thread: [16,0,0] Assertion `t ＞= 0 t ＜ n_classes` failed

报错信息&#xff1a;C:\cb\pytorch_1000000000000\work\aten\src\ATen\native\cuda\Loss.cu:250: block: [0,0,0], thread: [16,0,0] Assertion t > 0 && t < n_classes failed 可能原因&#xff1a;标签label或者预测的结果pred&#xff0c;超出了数据的范围&…
阅读更多...
10校大满贯！中国内地高校2024年1-6月CNS发文统计出炉

10校大满贯！中国内地高校2024年1-6月CNS发文统计出炉

随着全球科研竞争的日趋激烈&#xff0c;CNS&#xff08;Cell、Nature、Science&#xff09;作为科学领域的三大顶级期刊&#xff0c;不仅是科研成果的展示平台&#xff0c;更是各国科研实力比拼的重要战场。近年来&#xff0c;中国高校在国际科研舞台上的表现愈发抢眼&#xf…
阅读更多...
最新文章

Copyright @ 2022~2023