ArduPilot开源代码之AP_DAL_GPS

1. 源由

AP_DAL_GPS主要功能是处理和管理GPS数据，并处理不同类型的消息，以更新GPS状态和相关信息。

2. 框架设计

2.1 类和枚举定义

class AP_DAL_GPS {
public:

    /// GPS status codes
    enum GPS_Status : uint8_t {
        NO_GPS = GPS_FIX_TYPE_NO_GPS,                     ///< 无GPS连接/检测
        NO_FIX = GPS_FIX_TYPE_NO_FIX,                     ///< 接收到有效的GPS消息但没有锁定
        GPS_OK_FIX_2D = GPS_FIX_TYPE_2D_FIX,              ///< 接收到有效的消息并且2D锁定
        GPS_OK_FIX_3D = GPS_FIX_TYPE_3D_FIX,              ///< 接收到有效的消息并且3D锁定
        GPS_OK_FIX_3D_DGPS = GPS_FIX_TYPE_DGPS,           ///< 接收到有效的消息并且3D锁定，具有差分改进
        GPS_OK_FIX_3D_RTK_FLOAT = GPS_FIX_TYPE_RTK_FLOAT, ///< 接收到有效的消息并且3D RTK浮动
        GPS_OK_FIX_3D_RTK_FIXED = GPS_FIX_TYPE_RTK_FIXED, ///< 接收到有效的消息并且3D RTK固定
    };

• AP_DAL_GPS 类：用于GPS数据的处理和管理。
• GPS_Status 枚举：定义了不同的GPS状态，如无GPS、无锁定、2D锁定、3D锁定等。

2.2 构造函数

    AP_DAL_GPS();

• 默认构造函数。

2.3 状态方法

• status(uint8_t sensor_id)：返回指定传感器的GPS状态。
• status()：返回主传感器的GPS状态。

2.4 位置和速度方法

• location(uint8_t instance)：返回指定实例的位置信息。
• location()：返回主传感器的位置信息。
• velocity(uint8_t instance)：返回指定实例的速度信息。
• velocity()：返回主传感器的速度信息。

2.5 其他方法

• 提供了对各种GPS数据的访问，如垂直速度、水平精度、垂直精度、HDOP、最后消息时间、卫星数量、滞后时间、速度精度和GPS航向角度等。

2.6 消息处理

• handle_message(const log_RGPH &msg)：处理 log_RGPH 类型的消息。
• handle_message(const log_RGPI &msg)：处理 log_RGPI 类型的消息。
• handle_message(const log_RGPJ &msg)：处理 log_RGPJ 类型的消息，并更新临时位置数据。

2.7 私有成员变量

• 这些私有成员变量用于存储GPS数据，包括日志结构 _RGPH、_RGPI 和 _RGPJ，以及临时位置数组 tmp_location。

3. 重要例程

3.1 构造和状态

3.1.1 AP_DAL_GPS

构造函数，初始化GPS模块序号

AP_DAL_GPS::AP_DAL_GPS()
{
    for (uint8_t i=0; i<ARRAY_SIZE(_RGPI); i++) {
        _RGPI[i].instance = i;
        _RGPJ[i].instance = i;
    }
}

3.1.2 status

获取指定GPS模块状态 或 主GPS模块状态

    GPS_Status status(uint8_t sensor_id) const {
        return (GPS_Status)_RGPI[sensor_id].status;
    }
    GPS_Status status() const {
        return status(primary_sensor());
    }

3.1.3 primary_sensor

获取主GPS模块传感器序号

    uint8_t primary_sensor(void) const {
        return _RGPH.primary_sensor;
    }

3.2 位置和速度

3.2.1 location

获取指定GPS模块位置信息 或 主GPS模块位置信息

    const Location &location(uint8_t instance) const {
        return tmp_location[instance];
    }

    // TODO: decide if this really, really should be here!
    const Location &location() const {
        return location(_RGPH.primary_sensor);
    }

3.2.2 velocity

获取指定GPS模块速度矢量 或 主GPS模块速度矢量

    const Vector3f &velocity(uint8_t instance) const {
        return _RGPJ[instance].velocity;
    }
    const Vector3f &velocity() const {
        return velocity(primary_sensor());
    }

3.3 辅助函数

3.3.1 have_vertical_velocity

指定GPS模块是否有垂直速度 或 主GPS模块是否有垂直速度

    bool have_vertical_velocity(uint8_t instance) const {
        return _RGPI[instance].have_vertical_velocity;
    }
    bool have_vertical_velocity() const {
        return have_vertical_velocity(primary_sensor());
    }

3.3.2 horizontal_accuracy

获取指定GPS模块水平精度 或 主GPS模块水平精度

    bool horizontal_accuracy(uint8_t instance, float &hacc) const {
        hacc = _RGPJ[instance].hacc;
        return _RGPI[instance].horizontal_accuracy_returncode;
    }
    bool horizontal_accuracy(float &hacc) const {
        return horizontal_accuracy(primary_sensor(),hacc);
    }

3.3.3 vertical_accuracy

获取指定GPS模块垂直精度 或 主GPS模块垂直精度

    bool vertical_accuracy(uint8_t instance, float &vacc) const {
        vacc = _RGPJ[instance].vacc;
        return _RGPI[instance].vertical_accuracy_returncode;
    }
    bool vertical_accuracy(float &vacc) const {
        return vertical_accuracy(primary_sensor(), vacc);
    }

3.3.4 get_hdop

获取指定GPS模块HDOP值 或 主GPS模块HDOP值

    uint16_t get_hdop(uint8_t instance) const {
        return _RGPJ[instance].hdop;
    }
    uint16_t get_hdop() const {
        return get_hdop(primary_sensor());
    }

3.3.5 last_message_time_ms

获取最近一次消息的时间戳

    uint32_t last_message_time_ms(uint8_t instance) const {
        return _RGPJ[instance].last_message_time_ms;
    }

3.3.6 num_sats

获取指定GPS模块卫星数量 或 主GPS模块卫星数量

    uint8_t num_sats(uint8_t instance) const {
        return _RGPI[instance].num_sats;
    }
    uint8_t num_sats() const {
        return num_sats(primary_sensor());
    }

3.3.7 get_lag

获取指定GPS模块滞后时间 或 主GPS模块滞后时间

    bool get_lag(uint8_t instance, float &lag_sec) const {
        lag_sec = _RGPI[instance].lag_sec;
        return _RGPI[instance].get_lag_returncode;
    }
    bool get_lag(float &lag_sec) const {
        return get_lag(primary_sensor(), lag_sec);
    }

3.3.8 speed_accuracy

获取指定GPS模块速度精度 或 主GPS模块速度精度

    bool speed_accuracy(uint8_t instance, float &sacc) const {
        sacc = _RGPJ[instance].sacc;
        return _RGPI[instance].speed_accuracy_returncode;
    }
    bool speed_accuracy(float &sacc) const {
        return speed_accuracy(primary_sensor(), sacc);
    }

3.3.9 num_sensors

获取GPS模块数量

    uint8_t num_sensors(void) const {
        return _RGPH.num_sensors;
    }

3.3.10 gps_yaw_deg

获取指定GPS模块yaw角度

    bool gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg, uint32_t &time_ms) const {
        yaw_deg = _RGPJ[instance].yaw_deg;
        accuracy_deg = _RGPJ[instance].yaw_accuracy_deg;
        time_ms = _RGPJ[instance].yaw_deg_time_ms;
        return _RGPI[instance].gps_yaw_deg_returncode;
    }

3.3.11 get_antenna_offset

获取指定GPS模块位置相对偏移

    // return a 3D vector defining the offset of the GPS antenna in meters relative to the body frame origin
    const Vector3f &get_antenna_offset(uint8_t instance) const {
        return _RGPI[instance].antenna_offset;
    }

3.4 其他函数

3.4.1 start_frame

AP_DAL::start_frame
 └──> AP_DAL_GPS::start_frame

void AP_DAL_GPS::start_frame()
{
    // 获取GPS对象
    const auto &gps = AP::gps();

    // 备份旧的RGPH日志
    const log_RGPH old_RGPH = _RGPH;
    // 更新RGPH中的主传感器和传感器数量
    _RGPH.primary_sensor = gps.primary_sensor();
    _RGPH.num_sensors = gps.num_sensors();

    // 如果RGPH发生变化，写入重放块
    WRITE_REPLAY_BLOCK_IFCHANGED(RGPH, _RGPH, old_RGPH);

    // 遍历所有GPS传感器
    for (uint8_t i=0; i<ARRAY_SIZE(_RGPI); i++) {
        log_RGPI &RGPI = _RGPI[i];  // 获取当前传感器的RGPI日志
        log_RGPJ &RGPJ = _RGPJ[i];  // 获取当前传感器的RGPJ日志
        const log_RGPI old_RGPI = RGPI;  // 备份旧的RGPI日志
        const log_RGPJ old_RGPJ = RGPJ;  // 备份旧的RGPJ日志

        // 更新RGPI和RGPJ中的数据
        RGPI.status = (GPS_Status)gps.status(i);  // 更新传感器状态
        RGPI.antenna_offset = gps.get_antenna_offset(i);  // 更新天线偏移

        const Location &loc = gps.location(i);  // 获取传感器位置
        RGPJ.last_message_time_ms = gps.last_message_time_ms(i);  // 更新最后消息时间
        RGPJ.lat = loc.lat;  // 更新纬度
        RGPJ.lng = loc.lng;  // 更新经度
        RGPJ.alt = loc.alt;  // 更新高度
        RGPI.have_vertical_velocity = gps.have_vertical_velocity(i);  // 更新是否有垂直速度

        // 更新水平和垂直精度
        RGPI.horizontal_accuracy_returncode = gps.horizontal_accuracy(i, RGPJ.hacc);
        RGPI.vertical_accuracy_returncode = gps.vertical_accuracy(i, RGPJ.vacc);
        RGPJ.hdop = gps.get_hdop(i);  // 更新HDOP
        RGPI.num_sats = gps.num_sats(i);  // 更新卫星数量
        RGPI.get_lag_returncode = gps.get_lag(i, RGPI.lag_sec);  // 更新延迟

        RGPJ.velocity = gps.velocity(i);  // 更新速度
        RGPI.speed_accuracy_returncode = gps.speed_accuracy(i, RGPJ.sacc);  // 更新速度精度
        RGPI.gps_yaw_deg_returncode = gps.gps_yaw_deg(i, RGPJ.yaw_deg, RGPJ.yaw_accuracy_deg, RGPJ.yaw_deg_time_ms);  // 更新航向角

        // 如果RGPI和RGPJ发生变化，写入重放块
        WRITE_REPLAY_BLOCK_IFCHANGED(RGPI, RGPI, old_RGPI);
        WRITE_REPLAY_BLOCK_IFCHANGED(RGPJ, RGPJ, old_RGPJ);

        // 更新临时位置
        tmp_location[i].lat = RGPJ.lat;
        tmp_location[i].lng = RGPJ.lng;
        tmp_location[i].alt = RGPJ.alt;
    }
}

3.4.2 handle_message

AP_DAL::handle_message
 └──> AP_DAL_Baro::handle_message

    void handle_message(const log_RGPH &msg) {
        _RGPH = msg;
    }
    void handle_message(const log_RGPI &msg) {
        _RGPI[msg.instance] = msg;
    }
    void handle_message(const log_RGPJ &msg) {
        _RGPJ[msg.instance] = msg;

        tmp_location[msg.instance].lat = msg.lat;
        tmp_location[msg.instance].lng = msg.lng;
        tmp_location[msg.instance].alt = msg.alt;
    }

4. 总结

AP_DAL_GPS主要功能是处理和管理GPS数据，并提供访问接口进行直接状态访问。

5. 参考资料

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