ArduPilot开源代码之AP_DAL_Compass

lida2003

于 2024-07-18 17:07:52 发布

阅读量298

点赞数 16

分类专栏： ArduPilot 文章标签： ardupilot 开源 c++

本文链接：https://blog.csdn.net/lida2003/article/details/140528002

版权

ArduPilot 专栏收录该内容

136 篇文章 88 订阅

订阅专栏

1. 源由

AP_DAL_Compass的主要功能是管理和访问多个磁罗盘实例的状态和数据，通过公共方法提供对磁罗盘信息的查询和操作。。

2. 框架设计

2.1 成员变量和结构体

struct log_RMGH _RMGH;：这是一个结构体成员变量，用于存储主磁罗盘组合（RMGH）的数据。 //Replay Data Magnetometer Instance
struct log_RMGI _RMGI[COMPASS_MAX_INSTANCES];：这是一个数组，每个元素是 log_RMGI 结构体，用于存储多个磁罗盘实例的数据。 //Replay Data Magnetometer Header

2.2 公共方法

bool use_for_yaw(uint8_t i) const：返回第 i 个磁罗盘是否用于偏航。
bool healthy(uint8_t i) const：返回第 i 个磁罗盘是否正常。
const Vector3f &get_offsets(uint8_t i) const：返回第 i 个磁罗盘的偏移量。
bool have_scale_factor(uint8_t i) const：返回第 i 个磁罗盘是否有比例因子。
bool auto_declination_enabled() const：返回自动偏角校准是否启用。
uint8_t get_count() const：返回磁罗盘的数量。
float get_declination() const：返回磁偏角。
bool available() const：返回磁罗盘是否可用。
uint8_t get_num_enabled(void) const：返回已启用的传感器数量。
bool learn_offsets_enabled() const：返回学习偏移量是否启用。
uint32_t last_update_usec(uint8_t i) const：返回第 i 个磁罗盘的最后更新时间（微秒）。
const Vector3f &get_field(uint8_t i) const：以毫高斯（milligauss）的形式返回当前磁场作为 Vector3f。
bool consistent() const：返回磁罗盘是否指向相同方向。
uint8_t get_first_usable() const：返回第一个可用的磁罗盘索引。

2.3 构造函数和方法

AP_DAL_Compass()：构造函数，可能用于初始化对象或者分配内存。
void start_frame()：一个未实现的方法，可能用于开始一个帧。

2.4 消息处理方法

void handle_message(const log_RMGH &msg)：处理主磁罗盘组合（RMGH）的消息。
void handle_message(const log_RMGI &msg)：处理磁罗盘实例（RMGI）的消息。

3. 重要例程

3.1 应用函数

3.1.1 healthy

获取指定磁力计实例健康状态

    bool healthy(uint8_t i) const {
        return _RMGI[i].healthy;
    }

3.1.2 use_for_yaw

获取指定实例是否可用于偏航指示

    bool use_for_yaw(uint8_t i) const {
        return _RMGI[i].use_for_yaw;
    }

3.1.3 get_offsets

获取指定罗盘实例偏移量

    const Vector3f &get_offsets(uint8_t i) const {
        return _RMGI[i].offsets;
    }

3.1.4 have_scale_factor

获取指定罗盘实例是否有比例因子

    bool have_scale_factor(uint8_t i) const {
        return _RMGI[i].have_scale_factor;
    }

3.1.5 auto_declination_enabled

获取自动偏角校准是否启用

    bool auto_declination_enabled() const {
        return _RMGH.auto_declination_enabled;
    }

3.1.6 get_count

获取磁罗盘的数量

    uint8_t get_count() const {
        return _RMGH.count;
    }

3.1.7 get_declination

获取磁偏角

    float get_declination() const {
        return _RMGH.declination;
    }

3.1.8 available

磁罗盘是否可用

    bool available() const {
        return _RMGH.available;
    }

3.1.9 get_num_enabled

获取已启用的传感器数量

    uint8_t get_num_enabled(void) const { return _RMGH.num_enabled; }

3.1.10 learn_offsets_enabled

学习偏移量是否启用

    bool learn_offsets_enabled() const { return _RMGH.learn_offsets_enabled; }

3.1.11 last_update_usec

获取指定磁罗盘的最后更新时间（微秒）

    uint32_t last_update_usec(uint8_t i) const { return _RMGI[i].last_update_usec; }

3.1.12 get_field

以毫高斯（milligauss）的形式返回当前磁场作为 Vector3f

    const Vector3f &get_field(uint8_t i) const { return _RMGI[i].field; }

3.1.13 consistent

获取磁罗盘是否指向相同方向

    bool consistent() const { return _RMGH.consistent; }

3.1.14 get_first_usable

获取第一个可用的磁罗盘索引

    uint8_t get_first_usable() const { return _RMGH.first_usable; }

3.2 其他函数

3.2.1 AP_DAL_Compass

构造函数，初始化实例序号

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

3.2.2 start_frame

AP_DAL::start_frame
 └──> AP_DAL_Compass::start_frame

void AP_DAL_Compass::start_frame()
{
    const auto &compass = AP::compass();

    const log_RMGH old = _RMGH;
    _RMGH.available = compass.available();
    _RMGH.count = compass.get_count();
    _RMGH.auto_declination_enabled = compass.auto_declination_enabled();
    _RMGH.declination = compass.get_declination();
    _RMGH.num_enabled = compass.get_num_enabled();
    _RMGH.consistent = compass.consistent();
    _RMGH.first_usable = compass.get_first_usable();
    _RMGH.learn_offsets_enabled = compass.learn_offsets_enabled();

    WRITE_REPLAY_BLOCK_IFCHANGED(RMGH, _RMGH, old);

    for (uint8_t i=0; i<_RMGH.count; i++) {
        log_RMGI &RMGI = _RMGI[i];
        const log_RMGI old_RMGI = RMGI;
        RMGI.use_for_yaw = compass.use_for_yaw(i);
        RMGI.healthy = compass.healthy(i);
        RMGI.offsets = compass.get_offsets(i);
        RMGI.have_scale_factor = compass.have_scale_factor(i);
        RMGI.last_update_usec = compass.last_update_usec(i);
        RMGI.field = compass.get_field(i);

        WRITE_REPLAY_BLOCK_IFCHANGED(RMGI, RMGI, old_RMGI);
    }
}

3.2.3 handle_message

AP_DAL::handle_message
 └──> AP_DAL_Compass::handle_message

    void handle_message(const log_RMGH &msg) {
        _RMGH = msg;
    }
    void handle_message(const log_RMGI &msg) {
        _RMGI[msg.instance] = msg;
    }