ArduPilot开源代码之AP_DAL研读系列

1. 源由

AP_DAL作为NavEKF3_core的成员变量，在深入研读NavEKF3_core之前，有必要先明确或者搞清楚该类的作用。

首先，What does DAL in AP_DAL mean?

从字面意思分析，感觉像是API接口封装。主要涉及以下传感器对外的统一接口：

• AP_DAL_InertialSensor
• AP_DAL_Baro
• AP_DAL_GPS
• AP_DAL_RangeFinder
• AP_DAL_Compass
• AP_DAL_Airspeed
• AP_DAL_Beacon
• AP_DAL_VisualOdom

AP_DAL整合上述设备对象，做一个整合后，统一对外提供API服务，或者说为NavEKF3_core提供服务。

2. 框架设计

AP_DAL 类通过枚举类型、成员变量和方法提供了对飞行控制数据抽象的封装和管理。它允许记录不同类型的事件和传感器数据，管理不同类型的传感器对象，并提供了单例访问方式以及消息处理的接口。这种设计有助于保持代码的结构化和可维护性，同时提供了对底层硬件和传感器的抽象接口。

2.1 枚举类型

1. FrameType

   • 枚举了不同的帧类型，用于标识不同的操作或日志记录。具体的枚举值包括 InitialiseFilterEKF2、UpdateFilterEKF2、InitialiseFilterEKF3、UpdateFilterEKF3、LogWriteEKF2 和 LogWriteEKF3。

2. Event

   • 定义了各种事件，每个事件用一个唯一的枚举值来标识。例如 resetGyroBias、resetHeightDatum、setTerrainHgtStable 等等。

3. VehicleClass

   • 标识了不同的车辆类别，包括 UNKNOWN、GROUND、COPTER、FIXED_WING 和 SUBMARINE。

4. StateMask

   • 定义了一个状态掩码，目前只包含 ARMED。

5. EKFType

   • 用于标识不同的扩展卡尔曼滤波器（EKF）类型，包括 EKF2 和 EKF3。

2.2 成员变量

• 静态指针 _singleton：用于保存类的单例实例。
• 日志和传感器结构体：包括 _RFRH、_RFRF、_RFRN 等，用于存储不同类型的日志数据或传感器信息。
• 各种传感器对象：包括 _ins、_baro、_gps、_compass 等，用于访问与传感器相关的功能。

2.3 方法

• 构造函数和单例获取方法：构造函数初始化对象，单例方法 get_singleton() 用于获取类的唯一实例。
• 日志记录方法：包括 log_event2、log_SetOriginLLH2、log_event3 等，用于记录不同类型的事件或传感器数据。
• 状态获取方法：包括 get_armed、get_ekf_type、get_vehicle_class 等，用于获取状态或配置信息。
• 消息处理方法：一系列的 handle_message 方法，用于处理不同类型的传感器数据或日志消息。

2.4 其他特性

• 条件编译：在一些方法中使用了条件编译，根据编译器定义的配置选择不同的实现或参数。

3. 设备实例

3.1 IMU

3.1.1 ins

    AP_DAL_InertialSensor &ins() { return _ins; }

3.1.2 handle_message

    void handle_message(const log_RISH &msg) {
        _ins.handle_message(msg);
    }
    void handle_message(const log_RISI &msg) {
        _ins.handle_message(msg);
    }

3.2 气压计

3.2.1 baro

    AP_DAL_Baro &baro() { return _baro; }

3.2.2 handle_message

    void handle_message(const log_RBRH &msg) {
        _baro.handle_message(msg);
    }
    void handle_message(const log_RBRI &msg) {
        _baro.handle_message(msg);
    }

3.3 磁力计

3.3.1 compass

    AP_DAL_Compass &compass() { return _compass; }

3.3.2 handle_message

    void handle_message(const log_RMGH &msg) {
        _compass.handle_message(msg);
    }
    void handle_message(const log_RMGI &msg) {
        _compass.handle_message(msg);
    }

3.4 GPS

3.4.1 gps

    AP_DAL_GPS &gps() { return _gps; }

3.4.2 handle_message

    void handle_message(const log_RGPH &msg) {
        _gps.handle_message(msg);
    }
    void handle_message(const log_RGPI &msg) {
        _gps.handle_message(msg);
    }
    void handle_message(const log_RGPJ &msg) {
        _gps.handle_message(msg);
    }

3.5 测距仪

3.5.1 rangefinder

#if AP_RANGEFINDER_ENABLED
    AP_DAL_RangeFinder *rangefinder() {
        return _rangefinder;
    }
#endif

3.5.2 handle_message

    void handle_message(const log_RRNH &msg) {
#if AP_RANGEFINDER_ENABLED
        if (_rangefinder == nullptr) {
            _rangefinder = NEW_NOTHROW AP_DAL_RangeFinder;
        }
        _rangefinder->handle_message(msg);
#endif
    }
    void handle_message(const log_RRNI &msg) {
#if AP_RANGEFINDER_ENABLED
        if (_rangefinder == nullptr) {
            _rangefinder = NEW_NOTHROW AP_DAL_RangeFinder;
        }
        _rangefinder->handle_message(msg);
#endif
    }

3.6 空速计

3.6.1 airspeed

    AP_DAL_Airspeed *airspeed() {
        return _airspeed;
    }

3.6.2 handle_message

    void handle_message(const log_RASH &msg) {
        if (_airspeed == nullptr) {
            _airspeed = NEW_NOTHROW AP_DAL_Airspeed;
        }
        _airspeed->handle_message(msg);
    }
    void handle_message(const log_RASI &msg) {
        if (_airspeed == nullptr) {
            _airspeed = NEW_NOTHROW AP_DAL_Airspeed;
        }
        _airspeed->handle_message(msg);
    }

3.6.3 airspeed_sensor_enabled

获取空速计传感使能状态

    // random methods that AP_NavEKF3 wants to call on AHRS:
    bool airspeed_sensor_enabled(void) const {
        return _RFRN.ahrs_airspeed_sensor_enabled;
    }

3.7 BeaCon

3.7.1 beacon

#if AP_BEACON_ENABLED
    AP_DAL_Beacon *beacon() {
        return _beacon;
    }
#endif

3.7.2 handle_message

    void handle_message(const log_RBCH &msg) {
#if AP_BEACON_ENABLED
        if (_beacon == nullptr) {
            _beacon = NEW_NOTHROW AP_DAL_Beacon;
        }
        _beacon->handle_message(msg);
#endif
    }
    void handle_message(const log_RBCI &msg) {
#if AP_BEACON_ENABLED
        if (_beacon == nullptr) {
            _beacon = NEW_NOTHROW AP_DAL_Beacon;
        }
        _beacon->handle_message(msg);
#endif
    }

3.8 实际里程计

3.8.1 visualodom

#if HAL_VISUALODOM_ENABLED
    AP_DAL_VisualOdom *visualodom() {
        return _visualodom;
    }
#endif

3.8.2 handle_message

    void handle_message(const log_RVOH &msg) {
#if HAL_VISUALODOM_ENABLED
        if (_visualodom == nullptr) {
            _visualodom = NEW_NOTHROW AP_DAL_VisualOdom;
        }
        _visualodom->handle_message(msg);
#endif
    }

4. 其他函数

4.1 辅助函数

4.1.1 构造函数

构造函数（空函数）

    AP_DAL() {}

4.1.2 获取对象实例

获取对象实例

    static AP_DAL *get_singleton() {
        if (!_singleton) {
            _singleton = NEW_NOTHROW AP_DAL();
        }
        return _singleton;
    }

4.1.3 时间函数

micros: 微秒数（百万分之一秒）
millis: 毫秒数（千分之一秒）
us: 64位微妙

    uint64_t micros64() const { return _RFRH.time_us; }
    uint32_t micros() const { return _micros; }
    uint32_t millis() const { return _millis; }

4.1.4 ekf_low_time_remaining

检查这个核心的 CPU 是否不足。

bool AP_DAL::ekf_low_time_remaining(EKFType etype, uint8_t core)
{
    static_assert(MAX_EKF_CORES <= 4, "支持最多 4 个 EKF 核心");
    const uint8_t mask = (1U << (core + (uint8_t(etype) * 4)));
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
    /*
      如果我们在一个循环中使用的时间超过了 1/3，那么我们
      返回 true，表示 CPU 不足。这会改变不同通道间的融合调度。
     */
    const auto &imu = AP::ins();
    if ((AP_HAL::micros() - imu.get_last_update_usec()) * 1.0e-6 > imu.get_loop_delta_t() * 0.33) {
        _RFRF.core_slow |= mask;
    } else {
        _RFRF.core_slow &= ~mask;
    }
#endif
    return (_RFRF.core_slow & mask) != 0;
}

4.1.5 get_armed

获取当前帧的激活状态

bool get_armed() const { return _RFRN.armed; }

4.1.6 available_memory

当前帧开始时的可用内存。

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    uint32_t available_memory() const { return _RFRN.available_memory + 10240; }
#else
    uint32_t available_memory() const { return _RFRN.available_memory; }
#endif

注：虽然在帧处理过程中可能会发生变化，但如果内存不足，将无法启动 EKF（扩展卡尔曼滤波器），因此为了简单起见为整个帧使用一个值是值得的。

4.1.7 get_ekf_type

获取当前EKF算法类型。

    int8_t get_ekf_type(void) const {
        return _RFRN.ekf_type;
    }

4.1.8 snprintf

C 语言标准库中的一个函数，用于将格式化的数据写入一个字符数组中。它的全称是“string print formatted”函数。它的使用类似于 sprintf，但具有一个重要的安全特性：它允许你指定输出缓冲区的大小，从而防止缓冲区溢出的问题。

int AP_DAL::snprintf(char* str, size_t size, const char *format, ...) const
{
    va_list ap;
    va_start(ap, format);
    int res = hal.util->vsnprintf(str, size, format, ap);
    va_end(ap);
    return res;
}

4.1.9 malloc_type

内存分配函数

void *AP_DAL::malloc_type(size_t size, Memory_Type mem_type) const
{
    return hal.util->malloc_type(size, AP_HAL::Util::Memory_Type(mem_type));
}

4.1.10 get_EAS2TAS

// 这段代码替代了 AP::ahrs()->EAS2TAS()，这个方法可能应该被淘汰
// 可以直接使用 Baro 方法来替代。
// 获取表观空气速度到真实空气速度的比率

    float get_EAS2TAS(void) const {
        return _RFRN.EAS2TAS;
    }

4.1.11 get_vehicle_class

获取设备种类

    VehicleClass get_vehicle_class(void) const {
        return (VehicleClass)_RFRN.vehicle_class;
    }

4.1.12 get_fly_forward

这个函数主要应用区别在于地面车辆设备与飞行器之间，AP_AHRS会重载该函数实现。

    bool get_fly_forward(void) const {
        return _RFRN.fly_forward;
    }

4.1.13 get_takeoff_expected

    bool get_takeoff_expected(void) const {
        return _RFRN.takeoff_expected;
    }

4.1.14 get_touchdown_expected

    bool get_touchdown_expected(void) const {
        return _RFRN.touchdown_expected;
    }

4.1.15 set_takeoff_expected

用于 EKF，使能起飞预期为 true

void AP_DAL::set_takeoff_expected()
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
    AP_AHRS &ahrs = AP::ahrs();
    ahrs.set_takeoff_expected(true);
#endif
}

4.1.16 get_trim

    const Vector3f &get_trim() const {
        return _RFRN.ahrs_trim;
    }

4.1.17 get_rotation_vehicle_body_to_autopilot_body

    const Matrix3f &get_rotation_vehicle_body_to_autopilot_body(void) const {
        return _rotation_vehicle_body_to_autopilot_body;
    }

4.1.18 get_home

获取家位置。该方法为 const，以防止在未通知 AHRS 的情况下更改家位置

const class Location &get_home(void) const {
    return _home;
}

4.1.19 get_time_flying_ms

获取飞行时间

    uint32_t get_time_flying_ms(void) const {
        return _RFRH.time_flying_ms;
    }

4.1.20 opticalflow_enabled

    bool opticalflow_enabled(void) const {
        return _RFRN.opticalflow_enabled;
    }

4.1.21 wheelencoder_enabled

    bool wheelencoder_enabled(void) const {
        return _RFRN.wheelencoder_enabled;
    }

4.2 重要函数

4.2.1 start_frame

void AP_DAL::start_frame(AP_DAL::FrameType frametype)
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)

    // 如果未初始化，进行传感器初始化
    if (!init_done) {
        init_sensors();
    }

    // 获取惯性导航系统和AHRS对象
    const AP_AHRS &ahrs = AP::ahrs();

    // 获取最近的IMU更新时间
    const uint32_t imu_us = AP::ins().get_last_update_usec();
    if (_last_imu_time_us == imu_us) {
        // 如果时间相同，更新帧类型并返回
        _RFRF.frame_types |= uint8_t(frametype);
        return;
    }
    _last_imu_time_us = imu_us;

    // 在日志开始时强制写入所有消息
#if HAL_LOGGING_ENABLED
    bool logging = AP::logger().logging_started() && AP::logger().allow_start_ekf();
    if (logging && !logging_started) {
        force_write = true;
    }
    logging_started = logging;
#endif

    // 结束当前帧
    end_frame();

    // 设置新帧的帧类型
    _RFRF.frame_types = uint8_t(frametype);

#if AP_VEHICLE_ENABLED
    // 获取飞行时间
    _RFRH.time_flying_ms = AP::vehicle()->get_time_flying_ms();
#else
    _RFRH.time_flying_ms = 0;
#endif
    // 获取当前微秒时间
    _RFRH.time_us = AP_HAL::micros64();
    WRITE_REPLAY_BLOCK(RFRH, _RFRH);

    // 更新RFRN数据
    const log_RFRN old = _RFRN;
    _RFRN.armed = hal.util->get_soft_armed();
    _home = ahrs.get_home();
    _RFRN.lat = _home.lat;
    _RFRN.lng = _home.lng;
    _RFRN.alt = _home.alt;
    _RFRN.EAS2TAS = ahrs.get_EAS2TAS();
    _RFRN.vehicle_class = (uint8_t)ahrs.get_vehicle_class();
    _RFRN.fly_forward = ahrs.get_fly_forward();
    _RFRN.takeoff_expected = ahrs.get_takeoff_expected();
    _RFRN.touchdown_expected = ahrs.get_touchdown_expected();
    _RFRN.ahrs_airspeed_sensor_enabled = ahrs.airspeed_sensor_enabled(ahrs.get_active_airspeed_index());
    _RFRN.available_memory = hal.util->available_memory();
    _RFRN.ahrs_trim = ahrs.get_trim();
#if AP_OPTICALFLOW_ENABLED
    _RFRN.opticalflow_enabled = AP::opticalflow() && AP::opticalflow()->enabled();
#endif
    _RFRN.wheelencoder_enabled = AP::wheelencoder() && (AP::wheelencoder()->num_sensors() > 0);
    _RFRN.ekf_type = ahrs.get_ekf_type();
    WRITE_REPLAY_BLOCK_IFCHANGED(RFRN, _RFRN, old);

    // 更新机体转换
    _rotation_vehicle_body_to_autopilot_body = ahrs.get_rotation_vehicle_body_to_autopilot_body();

    // 启动各传感器帧
    _ins.start_frame();
    _baro.start_frame();
    _gps.start_frame();
    _compass.start_frame();
    if (_airspeed) {
        _airspeed->start_frame();
    }
#if AP_RANGEFINDER_ENABLED
    if (_rangefinder) {
        _rangefinder->start_frame();
    }
#endif
#if AP_BEACON_ENABLED
    if (_beacon) {
        _beacon->start_frame();
    }
#endif
#if HAL_VISUALODOM_ENABLED
    if (_visualodom) {
        _visualodom->start_frame();
    }
#endif

    // 填充一些衍生值
    _micros = _RFRH.time_us;
    _millis = _RFRH.time_us / 1000UL;

    force_write = false;
#endif
}

4.2.1.1 NavEKF2::InitialiseFilter 入口

AP_AHRS::update_EKF2/AP_AHRS::reset/AP_DAL::handle_message
 └──> NavEKF2::InitialiseFilter
     └──> AP_DAL::start_frame 

4.2.1.2 NavEKF2::UpdateFilter 入口

AP_AHRS::update_EKF2/AP_DAL::handle_message
 └──> NavEKF2::UpdateFilter
     └──> AP_DAL::start_frame 

4.2.1.3 NavEKF2::Log_Write 入口

AP_AHRS::Log_Write/AP_DAL::handle_message
 └──> NavEKF2::Log_Write
     └──> AP_DAL::start_frame 

4.2.1.4 NavEKF3::InitialiseFilter 入口

AP_AHRS::update_EKF3/AP_AHRS::reset/AP_DAL::handle_message
 └──> NavEKF3::InitialiseFilter
     └──> AP_DAL::start_frame 

4.2.1.5 NavEKF3::UpdateFilter 入口

AP_AHRS::update_EKF3/AP_DAL::handle_message
 └──> NavEKF3::UpdateFilter
     └──> AP_DAL::start_frame 

4.2.2 end_frame

结束帧。必须在所有事件和数据注入（例如流量）上调用，并且在开始新帧之前调用。

void AP_DAL::end_frame(void)
{
    if (_RFRF.frame_types != 0) {
        WRITE_REPLAY_BLOCK(RFRF, _RFRF);
        _RFRF.frame_types = 0;
    }
}

4.2.3 handle_message

    // framing structures
    struct log_RFRH _RFRH;  //Replay Frame Reference Head
    struct log_RFRF _RFRF;  //Replay Frame Reference Frame
    struct log_RFRN _RFRN;  //Replay Frame Reference Navigation

    // push-based sensor structures
    struct log_ROFH _ROFH;  //Replay Odometry Frame Head
    struct log_REPH _REPH;  //eplay Error Prediction Head
    struct log_REVH _REVH;  //Replay Error Velocity Head
    struct log_RWOH _RWOH;  //Replay Wheel Odometry Head
    struct log_RBOH _RBOH;  //Replay Barometer Odometry Head
    struct log_RSLL _RSLL;  //Replay Sensor Location Log

void handle_message(const log_RFRH &msg)
void handle_message(const log_RFRN &msg)
void handle_message(const log_RFRF &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
void handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
void handle_message(const log_REPH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
void handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
void handle_message(const log_RWOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
void handle_message(const log_RBOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
void handle_message(const log_RSLL &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);

4.2.4 logging_core

// map core number for replay
uint8_t AP_DAL::logging_core(uint8_t c) const
{
#if APM_BUILD_TYPE(APM_BUILD_Replay)
    return c+100U;
#else
    return c;
#endif
}

#define DAL_CORE(c) AP::dal().logging_core(c)

4.2.5 WriteLogMessage

#if HAL_LOGGING_ENABLED
// write out a DAL log message. If old_msg is non-null, then
// only write if the content has changed
void AP_DAL::WriteLogMessage(enum LogMessages msg_type, void *msg, const void *old_msg, uint8_t msg_size)
{
    if (!logging_started) {
        // we're not logging
        return;
    }
    // we use the _end byte to hold a flag for forcing output
    uint8_t &_end = ((uint8_t *)msg)[msg_size];
    if (old_msg && !force_write && _end == 0 && memcmp(msg, old_msg, msg_size) == 0) {
        // no change, skip this block write
        return;
    }
    if (!AP::logger().WriteReplayBlock(msg_type, msg, msg_size)) {
        // mark for forced write next time
        _end = 1;
    } else {
        _end = 0;
    }
}
#endif

4.3 日志函数

4.3.1 EKF2 日志

4.3.1.1 log_event2

void AP_DAL::log_event2(AP_DAL::Event event)
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
    end_frame();
    struct log_REV2 pkt{
        event          : uint8_t(event),
    };
    WRITE_REPLAY_BLOCK(REV2, pkt);
#endif
}

4.3.1.2 log_SetOriginLLH2

void AP_DAL::log_SetOriginLLH2(const Location &loc)
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
    struct log_RSO2 pkt{
        lat            : loc.lat,
        lng            : loc.lng,
        alt            : loc.alt,
    };
    WRITE_REPLAY_BLOCK(RSO2, pkt);
#endif
}

4.3.1.3 log_writeDefaultAirSpeed2

void AP_DAL::log_writeDefaultAirSpeed2(const float aspeed, const float uncertainty)
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
    struct log_RWA2 pkt{
        airspeed:      aspeed,
        uncertainty:   uncertainty,
    };
    WRITE_REPLAY_BLOCK(RWA2, pkt);
#endif
}

4.3.2 EKF3 日志

4.3.2.1 log_event3

void AP_DAL::log_event3(AP_DAL::Event event)
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
    end_frame();
    struct log_REV3 pkt{
        event          : uint8_t(event),
    };
    WRITE_REPLAY_BLOCK(REV3, pkt);
#endif
}

4.3.2.2 log_SetOriginLLH3

void AP_DAL::log_SetOriginLLH3(const Location &loc)
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
    struct log_RSO3 pkt{
        lat            : loc.lat,
        lng            : loc.lng,
        alt            : loc.alt,
    };
    WRITE_REPLAY_BLOCK(RSO3, pkt);
#endif
}

4.3.2.3 log_writeDefaultAirSpeed3

void AP_DAL::log_writeDefaultAirSpeed3(const float aspeed, const float uncertainty)
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
    struct log_RWA3 pkt{
        airspeed:      aspeed,
        uncertainty:   uncertainty
    };
    WRITE_REPLAY_BLOCK(RWA3, pkt);
#endif
}

4.3.2.4 log_writeEulerYawAngle

void AP_DAL::log_writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type)
{
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
    struct log_REY3 pkt{
        yawangle       : yawAngle,
        yawangleerr    : yawAngleErr,
        timestamp_ms   : timeStamp_ms,
        type           : type,
    };
    WRITE_REPLAY_BLOCK(REY3, pkt);
#endif
}

4.4.3 传感器日志

4.4.3.1 光流日志

// log optical flow data
void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
{
    end_frame();

    const log_ROFH old = _ROFH;
    _ROFH.rawFlowQuality = rawFlowQuality;
    _ROFH.rawFlowRates = rawFlowRates;
    _ROFH.rawGyroRates = rawGyroRates;
    _ROFH.msecFlowMeas = msecFlowMeas;
    _ROFH.posOffset = posOffset;
    _ROFH.heightOverride = heightOverride;
    WRITE_REPLAY_BLOCK_IFCHANGED(ROFH, _ROFH, old);
}

4.4.3.2 外部导航日志

// log external navigation data
void AP_DAL::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
{
    end_frame();

    const log_REPH old = _REPH;
    _REPH.pos = pos;
    _REPH.quat = quat;
    _REPH.posErr = posErr;
    _REPH.angErr = angErr;
    _REPH.timeStamp_ms = timeStamp_ms;
    _REPH.delay_ms = delay_ms;
    _REPH.resetTime_ms = resetTime_ms;
    WRITE_REPLAY_BLOCK_IFCHANGED(REPH, _REPH, old);
}

4.4.3.3 外部速度日志

// log external velocity data
void AP_DAL::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
{
    end_frame();

    const log_REVH old = _REVH;
    _REVH.vel = vel;
    _REVH.err = err;
    _REVH.timeStamp_ms = timeStamp_ms;
    _REVH.delay_ms = delay_ms;
    WRITE_REPLAY_BLOCK_IFCHANGED(REVH, _REVH, old);

}

4.4.3.4 车轮里程计数据

// log wheel odometry data
void AP_DAL::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius)
{
    end_frame();

    const log_RWOH old = _RWOH;
    _RWOH.delAng = delAng;
    _RWOH.delTime = delTime;
    _RWOH.timeStamp_ms = timeStamp_ms;
    _RWOH.posOffset = posOffset;
    _RWOH.radius = radius;
    WRITE_REPLAY_BLOCK_IFCHANGED(RWOH, _RWOH, old);
}

4.4.3.5 机体坐标系里程计数据

void AP_DAL::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset)
{
    end_frame();

    const log_RBOH old = _RBOH;
    _RBOH.quality = quality;
    _RBOH.delPos = delPos;
    _RBOH.delAng = delAng;
    _RBOH.delTime = delTime;
    _RBOH.timeStamp_ms = timeStamp_ms;
    WRITE_REPLAY_BLOCK_IFCHANGED(RBOH, _RBOH, old);
}

5. 总结

AP_DAL通过frame的方式随着AP_AHRS进行定期更新数据，进而保持其API提供最新的信息用于EKF算法。

6. 参考资料

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