ArduPilot开源代码之AP_DAL_Beacon

原创已于 2024-07-19 10:49:33 修改 · 1k 阅读

12 ·

本内容遵循CC 4.0 BY-SA版权协议

标签

#开源 #ardupilot #c++

于 2024-07-19 05:37:05 首次发布

ArduPilot 专栏收录该内容

174 篇文章

订阅专栏

1. 源由

AP_DAL_Beacon是用于管理和访问与信标系统相关的数据，可能用于导航或定位任务。它提供方法检查信标状态、获取距离并更新位置。

2. 框架设计

2.1 公共方法

count()：返回信标数量。
get_origin(Location &loc)：设置 loc 对象的原点纬度、经度和高度，并返回一个表示成功的布尔值。
enabled()：返回信标系统是否启用。
beacon_healthy(uint8_t i)：返回指定信标（通过索引 i）是否正常。
beacon_last_update_ms(uint8_t i)：返回指定信标最后一次更新的时间（毫秒）。
beacon_distance(uint8_t i)：返回到指定信标的距离（米）。
beacon_position(uint8_t i)：返回指定信标的位置，作为 Vector3f 的引用。
get_vehicle_position_ned(Vector3f& pos, float& accuracy_estimate)：在北东下（NED）坐标系中的位置，并提供精度估计。返回表示成功的布尔值。
AP_DAL_Beacon()：构造函数用于初始化类。
beacon()：返回当前实例的指针。
start_frame()：开始处理新帧（实现未显示）。
handle_message(const log_RBCH &msg)：用新消息更新 _RBCH。
handle_message(const log_RBCI &msg)：在 msg.instance 指定的索引处更新 _RBCI 数组。

2.2 私有成员

log_RBCH _RBCH：存储整个信标系统状态。 //Replay BeaCon Head
log_RBCI _RBCI[AP_BEACON_MAX_BEACONS]：一个数组，用于存储单个信标数据，最大数量由 AP_BEACON_MAX_BEACONS 定义。 //Replay BeaCon Instance

3. 重要例程

3.1 应用函数

3.1.1 count

获取beacon数量

    uint8_t count() const {
        return _RBCH.count;
    }

3.1.2 enabled

获取使能状态

    // return beacon enabled
    bool enabled(void) const {
        return _RBCH.enabled;
    }

3.1.3 get_origin

获取初始点位置

    bool get_origin(Location &loc) const {
        loc.zero();
        loc.lat = _RBCH.origin_lat;
        loc.lng = _RBCH.origin_lng;
        loc.alt = _RBCH.origin_alt;
        return _RBCH.get_origin_returncode;
    }

3.1.4 beacon_healthy

获取指定beacon健康状态

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

3.1.5 beacon_last_update_ms

获取最近一次beancon数据更新时间

    uint32_t beacon_last_update_ms(uint8_t i) const {
        return _RBCI[i].last_update_ms;
    }

3.1.6 beacon_distance

获取到指定信标的距离

    float beacon_distance(uint8_t i) const {
        return _RBCI[i].distance;
    }

3.1.7 beacon_position

获取指定信标的位置

    const Vector3f &beacon_position(uint8_t i) const {
        return _RBCI[i].position;
    }

3.1.8 get_vehicle_position_ned

获取NED坐标系下设备坐标

    bool get_vehicle_position_ned(Vector3f& pos, float& accuracy_estimate) const {
        pos = _RBCH.vehicle_position_ned;
        accuracy_estimate = _RBCH.accuracy_estimate;
        return _RBCH.get_vehicle_position_ned_returncode;
    }

3.2 其他函数

3.2.1 AP_DAL_Beacon

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

AP_DAL_Beacon::AP_DAL_Beacon()
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
    const auto *bcon = AP::beacon();
    _RBCH.count = bcon->count();
    for (uint8_t i=0; i<ARRAY_SIZE(_RBCI); i++) {
        _RBCI[i].instance = i;
    }
#endif
}

3.2.2 start_frame

AP_DAL::start_frame
 └──> AP_DAL_Beacon::start_frame

void AP_DAL_Beacon::start_frame()
{
    const auto *bcon = AP::beacon();

    const log_RBCH old = _RBCH;
    if (bcon != nullptr) {
        _RBCH.get_vehicle_position_ned_returncode = bcon->get_vehicle_position_ned(_RBCH.vehicle_position_ned, _RBCH.accuracy_estimate);
        Location loc;
        _RBCH.get_origin_returncode = bcon->get_origin(loc);
        _RBCH.enabled = bcon->enabled();
        _RBCH.origin_lat = loc.lat;
        _RBCH.origin_lng = loc.lng;
        _RBCH.origin_alt = loc.alt;
    }
    WRITE_REPLAY_BLOCK_IFCHANGED(RBCH, _RBCH, old);
    if (bcon == nullptr) {
        return;
    }

    for (uint8_t i=0; i<ARRAY_SIZE(_RBCI); i++) {
        log_RBCI &RBCI = _RBCI[i];
        const log_RBCI old_RBCI = RBCI;
        RBCI.last_update_ms = bcon->beacon_last_update_ms(i);
        RBCI.position = bcon->beacon_position(i);
        RBCI.distance = bcon->beacon_distance(i);
        RBCI.healthy = bcon->beacon_healthy(i);

        WRITE_REPLAY_BLOCK_IFCHANGED(RBCI, RBCI, old_RBCI);
    }
}

3.2.3 handle_message

AP_DAL::handle_message
 └──> AP_DAL_Beacon::handle_message

    void handle_message(const log_RBCH &msg) {
        _RBCH = msg;
    }
    void handle_message(const log_RBCI &msg) {
        _RBCI[msg.instance] = msg;
    }