ArduPilot开源代码之AP_DAL_InertialSensor

已于 2024-07-19 10:51:27 修改
阅读量782 收藏 29
点赞数 9
分类专栏： ArduPilot 文章标签： 开源 ardupilot c++
于 2024-07-17 17:39:46 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/lida2003/article/details/140486379
版权

ArduPilot开源代码之AP_DAL_InertialSensor

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系列研读
【7】ArduPilot开源代码之AP_DAL研读系列

lida2003
关注
专栏目录
Ardupilot源码分析（一）
low米的博客
12-05 2064
1.名词解释 缩写 解释   EKF Extended Kalman Filter 扩展卡尔曼滤波 DCM Direction cosine matrix 方向余弦矩阵 AC Attitude Control 姿态控制 AP Adding a New Parameter GCS ground control station 地面站 2. 常见的导航系统：惯性...
pixhawk源码分析（1）
11-03
飞控源码分析，ardupilot网站中文翻译，代码注释学习方法解析
Ardupilot源码框架
li5830的博客
07-04 1095
1、Ardupilot介绍 在Pixhawk的历史那节中我们谈到，Pixhawk是硬件平台，PX4是pixhawk的原生固件，专门为pixhawk开发的。APM（Ardupilot Mega）也是硬件，Ardupilot是APM的固件，所以称ArduPilot固件也叫APM。Ardupilot由一群爱好者开发维护的,从最早的APM1,APM2开始,后来软件代码不断状大,原来的APM2的硬...
学习ardupilot 源码
power的专栏
05-07 461
https://ardupilot.org/dev/docs/learning-the-ardupilot-codebase.html ardupilot约有70万行。对新手来说很吓人。本页意在给出一些快速上手的建议。假定你熟悉C++，本教程的一些示例是在linux系统上探索源码的。 你应该一步一步地阅读下面的页面，自己亲自试试。如果你认为有些重要的信息遗漏了，请访问我们的wiki留言，...
ardupilot/Ardupilot V4.3.4 源码解析：Ardupilot.cpp
qq_17031687的博客
08-02 588
if (AP_HAL::millis() - plane.launch_detector_pin_last_inactive_time_ms < ((uint32_t)plane.g2.launch_detector_delay_ms)) { // 虽然已经触发，但是没有达到延时时间。if (hal.gpio->read(plane.g2.launch_detector_pin) == plane.g2.launch_detector_polarity) { // 处于已触发状态。
Ardupilot 源码笔记（1）
kyrie_irving_mc的博客
12-03 835
Ardupilot 源码笔记（1） 一.框架介绍 二.sketch例程分析
DAL.rar_DAL_employee attendance_java Attendance
09-21
标题 &quot;DAL.rar_DAL_employee attendance_java Attendance&quot; 暗示这是一个关于使用Java技术开发的员工考勤和休假追踪系统的数据访问层（DAL）项目。在这个系统中，开发人员可能使用了DB4O数据库来存储和管理员工的出勤...
DAL.zip_E9A_SQLHelper_dal库_dbhelper_oracle
09-15
它包括了一个定制的SQLHelper类，以及一个DAL库，可以帮助开发者轻松管理和操作Oracle数据库，提升开发效率，同时保持代码的整洁和可维护性。在实际开发过程中，掌握这些工具的使用，将对提升项目质量产生积极影响。
PetShop_Model_DAL.rar_CASTLE_Castle mod_activeRecord_castle d
09-24
在.NET开发中，数据访问层（DAL）是应用程序与数据库交互的关键部分。本项目“PetShop_Model_DAL.rar”提供了一个使用Castle ActiveRecord框架构建的数据访问层示例，适用于ASP.NET应用。通过这个压缩包，我们可以...
DAL.rar_AutoCoder_DAL_数据库接口_数据访问层
09-24
在本项目&quot;DAL.rar_AutoCoder_DAL_数据库接口_数据访问层&quot;中，我们关注的是一个针对C++语言设计的数据访问层接口，它具有高度的通用性，能够适应多种主流的数据库管理系统。 首先，让我们深入理解数据访问层（DAL）...
DAL.rar_DAL
09-20
9. **分页和排序**：如果DAL.cs涉及到从数据库获取数据，可能会包含实现分页和排序功能的代码，这通常涉及使用SQL的OFFSET/FETCH或TOP/LIMIT子句，或者在EF中使用Skip和Take方法。 10. **缓存策略**：为了提高性能...
APM.ardupilot 源码笔记1
MRGCODING
10-15 1330
源码：Apm.ardupilot 各个版本 本例：以 3.1.0_rc1 为例分析 APM硬件: 版本2.0以上 By: devel   这一段时间看了看APM的源代码：ardupilot。从中，学到了一些好的方法和技巧，打算花些时间把自己的获知与感悟写下来，以便以后自己再回顾，同时，如果能帮上一些需要的人那就最好了(^-^)。 今天先开个头，具体的过几天再写，现在把要用到的资源先贴上
Ardupilot开源代码之Rover上路计划
lida2003的专栏
05-03 1372
有点感觉像买车的时候，有人买Toyota GR86，而有的人去买了沃尔沃S60。车子要出问题，可以直接踩下刹车，飞机在天空中可没有刹车，刹停就掉下来了。当然，本着DIY的精神，绝对不去买成品，折腾而且要性价比高，那么妥妥的上高大上的东西ArduRover。注：期望能够把最近这套OpenIPC给整合进去，如果能拿到4GLTE的模块，就一起整活！注：其实最简单的遥控无人车，压根就不需要飞控，只要接收机+电机+Servo车就能开的。显然，从穿越机往无人机方向转进，存在一个至关重要的问题：安全性。
ArduPilot之开源代码Library&Sketches设计
lida2003的专栏
04-29 1952
总的来说，ArduPilot从代码框架设计结构上遵循了Ardunio设计思路；在上述基础上，加入了设备应用建模，并对模型共性部分Library化抽象，方便维护。当然，这里只是从概念和代码的角度管中窥豹，很多细节问题：驱动，数据共享，通信等问题我们在后面的章节中再行深入的讨论和学习。
ArduPilot开源代码之AP_WheelEncoder
最新发布
lida2003的专栏
05-22 1140
结合遇到的问题。在Ardupilot开源代码之Rover上路计划CPR参数值定义INT16，最大不能超过32768对于高精度编码器的应用，会直接导致MCU占用率增加本文针对进行代码级别的梳理和研读，同时也提出了对于高精度1024线编码器，高减速比应用的情况做了适当的兼容处理。使得APM代码在上述场景，依然可以使用。注：同时，可以通过适当软缩减编码器精度，或者增加状态变量字节长度的方法，将上述应用类进行兼容性扩展。这样对于高精度要求的机器人应用上，APM将会更加适应其精度要求。
ArduPilot之开源代码UARTs and the Console使用
lida2003的专栏
05-11 1279
HAL框架设计角度，ArduPilot支持1 + 9个UART端口，其中1个是USB转串口。鉴于ArduPilot支持OS版本不同，或多或少有一些差异，UART可能存在不同的配置情况。
ArduPilot开源代码之AP_Scheduler
lida2003的专栏
06-06 1080
AP_Scheduler是ArduPilot任务调度的核心，并且飞控的调度器都是目的性很强的自研调度算法。 因此，要理解飞控的应用，尤其是飞行姿态时间相关性极高的任务更需要理解调度算法逻辑。
ArduPilot开源代码之AP_Logger
lida2003的专栏
06-22 748
通过AP_Logger模块的代码初步研读，整个日志的应用是如何初始化？如何日志记录？如何驱动更新数据？应该都能很好的找到入口点。通过研读，希望能够从代码框架的角度，更好的理解设计。当我们需要新增一些内容的时候，可以有的放矢。移植新的SPI-Flash芯片驱动新增应用日志记录调整日志记录频率定位日志应用的异常问题等等。。。。。。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值