ArduPilot开源代码之AP_AHRS_Backend

1. 源由

前面我们简单研读了《ArduPilot开源飞控之AP_AHRS》，问题是该类需要比较多的数学知识，且其复杂度比较高。

因此，始终没有比较全面的进行理解。为此，我们还是用非常简单的原则，一步一个脚印，逐一的突破。

在《ArduPilot开源代码之AP_AHRS_View》中，通过另一个视角，我们了解了AP_AHRS会有哪些结果可以呈现。

本章我们将从AP_AHRS内部用到的各个成员类的共性(AP_AHRS_Backend)入手研读。

2. 类继承关系

AP_AHRS_Backend
 ├──> AP_AHRS_DCM
 ├──> AP_AHRS_External
 └──> AP_AHRS_SIM

3. 框架设计

AP_AHRS_Backend 类用作姿态和航向参考系统 (AHRS) 后端的抽象基类。作为实现不同 AHRS 后端的蓝图，确保接口一致性同时允许根据不同的传感器配置或系统要求实现灵活性。每个派生类将实现纯虚方法以提供特定于不同 AHRS 功能的实现。

2.1 构造函数和析构函数

• 类具有默认构造函数，并且有一个虚拟的空析构函数 (virtual ~AP_AHRS_Backend() {})，表明它设计用于继承和动态多态性。

2.2 不可复制

• 包含一个宏 CLASS_NO_COPY(AP_AHRS_Backend);，防止实例的复制，确保单例类似的行为或唯一性。

2.3 嵌套结构和枚举

• Estimates：一个结构体，包含与方向相关的各种估算值（如滚转、俯仰、偏航角）、方向矩阵、陀螺仪估算、加速度计数据、位置信息以及位置有效性标志。它还提供了 get_location 等方法来获取位置数据。

2.4 虚方法

• virtual void init();：初始化 AHRS 后端。
• virtual void update() = 0;：纯虚方法，用于更新 AHRS 数据。
• virtual void get_results(Estimates &results) = 0;：纯虚方法，用于获取 AHRS 的估算结果。
• 其他虚方法定义了与传感器索引、预装载检查、姿态一致性检查、车道切换、偏航处理、传感器来源、重置（陀螺仪漂移、姿态）、空速估算、地速、位置、速度以及导航和 AHRS 健康状态相关的功能。

2.5 静态方法

• static float get_EAS2TAS();：将等效空速转换为真空速。

2.6 实用方法

• groundspeed_vector()、groundspeed() 等方法提供了用于检索地速、位置数据和状态检查的实用功能。

2.7 纯虚方法

• 诸如 virtual bool get_quaternion(Quaternion &quat) const = 0; 和 virtual bool healthy() const = 0; 的方法强制派生类实现特定的 AHRS 功能。

2.8 条件编译

• 几个方法使用条件编译 (#if 指令) 根据预处理器定义（如 AP_INERTIALSENSOR_ENABLED 和 AP_AIRSPEED_ENABLED）启用或禁用功能。

3. 虚方法设计

整个源代码非常奇怪。

1. 大部分虚函数接口，感觉更像API设计定义；
2. 仅有非常少量的抽象共性函数实现；
3. AP_AHRS_Backend.cpp源代码文件里面，实现的函数属于AP_AHRS和AP_AHRS_View的；

3.1 初始化

非常简洁，因为抽象实现很少；更像API设计，因此没有太多的初始化。

3.1.1 构造函数

    // Constructor
    AP_AHRS_Backend() {}

3.1.2 析构函数

    // empty virtual destructor
    virtual ~AP_AHRS_Backend() {}

3.1.3 AP_AHRS_Backend::init

void AP_AHRS_Backend::init()
{
}

3.2 IMU传感

3.2.1 AP_AHRS_Backend::get_primary_core_index

• EKFType::DCM //0
• EKFType::SIM //0
• EKFType::EXTERNAL //0
• EKFType::TWO //current healthy IMU core
• EKFType::THREE //current healthy IMU core

    // return the index of the primary core or -1 if no primary core selected
    virtual int8_t get_primary_core_index() const { return -1; }

3.2.2 AP_AHRS_Backend::get_primary_accel_index

当前使用的加速度传感器序号，从0开始代表第一个IMU。

    // get the index of the current primary accelerometer sensor
    virtual uint8_t get_primary_accel_index(void) const {
#if AP_INERTIALSENSOR_ENABLED
        return AP::ins().get_first_usable_accel();
#else
        return 0;
#endif
    }

3.2.3 AP_AHRS_Backend::get_primary_gyro_index

当前使用的陀螺仪传感器，从0开始代表第一个IMU。

    // get the index of the current primary gyro sensor
    virtual uint8_t get_primary_gyro_index(void) const {
#if AP_INERTIALSENSOR_ENABLED
        return AP::ins().get_first_usable_gyro();
#else
        return 0;
#endif
    }

3.3 重要方法

3.3.1 AP_AHRS_Backend::update

定期更新的AHRS方法，继承类必须重载该函数实现。

    // Methods
    virtual void update() = 0;

3.3.2 AP_AHRS_Backend::get_results

获取当前姿态、速度、位置估计数值，继承类必须重载该函数实现。

    virtual void get_results(Estimates &results) = 0;

3.3.3 AP_AHRS_Backend::pre_arm_check

检查AHRS是否健康，继承类必须重载该函数实现。

    // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
    // requires_position should be true if horizontal position configuration should be checked
    virtual bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const = 0;

3.3.4 AP_AHRS_Backend::healthy

判断是否健康，继承类必须重载该函数实现。

    // is the AHRS subsystem healthy?
    virtual bool healthy(void) const = 0;

3.3.5 AP_AHRS_Backend::initialised

判断初始化是否完成。

    // true if the AHRS has completed initialisation
    virtual bool initialised(void) const {
        return true;
    };

    virtual bool started(void) const {
        return initialised();
    };

注：详见，AP_AHRS::initialised

3.3.6 AP_AHRS_Backend::get_quaternion

获取四元数，继承类必须重载该函数实现。

    // return the quaternion defining the rotation from NED to XYZ (body) axes
    virtual bool get_quaternion(Quaternion &quat) const WARN_IF_UNUSED = 0;

3.4 重置函数

3.4.1 AP_AHRS_Backend::reset_gyro_drift

重置陀螺仪漂移，继承类必须重载该函数实现。

    // reset the current gyro drift estimate
    //  should be called if gyro offsets are recalculated
    virtual void reset_gyro_drift(void) = 0;

3.4.2 AP_AHRS_Backend::reset

重置IMU，继承类必须重载该函数实现。

    // reset the current attitude, used on new IMU calibration
    virtual void reset() = 0;

3.4.3 AP_AHRS_Backend::getLastYawResetAngle

获取最近一次Yaw重置时间。

    // return the amount of yaw angle change due to the last yaw angle reset in radians
    // returns the time of the last yaw angle reset or 0 if no reset has ever occurred
    virtual uint32_t getLastYawResetAngle(float &yawAng) {
        return 0;
    };

注：详见，AP_AHRS::getLastYawResetAngle

3.4.4 AP_AHRS_Backend::getLastPosNorthEastReset

获取最近一次NE坐标下位置重置时间。

    // return the amount of NE position change in metres due to the last reset
    // returns the time of the last reset or 0 if no reset has ever occurred
    virtual uint32_t getLastPosNorthEastReset(Vector2f &pos) WARN_IF_UNUSED {
        return 0;
    };

注：详见，AP_AHRS::getLastPosNorthEastReset

3.4.5 AP_AHRS_Backend::getLastVelNorthEastReset

获取最近一次NE坐标下速度重置时间。

    // return the amount of NE velocity change in metres/sec due to the last reset
    // returns the time of the last reset or 0 if no reset has ever occurred
    virtual uint32_t getLastVelNorthEastReset(Vector2f &vel) const WARN_IF_UNUSED {
        return 0;
    };

注：详见，AP_AHRS::getLastVelNorthEastReset

3.4.6 AP_AHRS_Backend::getLastPosDownReset

获取最近一次Z坐标下位置重置时间。

    // return the amount of vertical position change due to the last reset in meters
    // returns the time of the last reset or 0 if no reset has ever occurred
    virtual uint32_t getLastPosDownReset(float &posDelta) WARN_IF_UNUSED {
        return 0;
    };

注：详见，AP_AHRS::getLastPosDownReset

3.4.7 AP_AHRS_Backend::resetHeightDatum

重置高度。

    // Resets the baro so that it reads zero at the current height
    // Resets the EKF height to zero
    // Adjusts the EKf origin height so that the EKF height + origin height is the same as before
    // Returns true if the height datum reset has been performed
    // If using a range finder for height no reset is performed and it returns false
    virtual bool resetHeightDatum(void) WARN_IF_UNUSED {
        return false;
    }

注：详见，AP_AHRS::resetHeightDatum

3.5 速度函数

3.5.1 AP_AHRS_Backend::wind_estimate

获取风速，继承类必须重载该函数实现。

    // return a wind estimation vector, in m/s
    virtual bool wind_estimate(Vector3f &wind) const = 0;

3.5.2 AP_AHRS_Backend::airspeed_estimate

获取空速，继承类必须重载该函数实现。

    // return an airspeed estimate if available. return true
    // if we have an estimate
    virtual bool airspeed_estimate(float &airspeed_ret) const WARN_IF_UNUSED { return false; }
    virtual bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const { return false; }

3.5.3 AP_AHRS_Backend::airspeed_estimate_true

获取真空速，不同的继承类有不同的实现方式：

• AP_AHRS::airspeed_estimate_true
• AP_AHRS_View::airspeed_estimate_true

    // return a true airspeed estimate (navigation airspeed) if
    // available. return true if we have an estimate
    bool airspeed_estimate_true(float &airspeed_ret) const WARN_IF_UNUSED {
        if (!airspeed_estimate(airspeed_ret)) {
            return false;
        }
        airspeed_ret *= get_EAS2TAS();
        return true;
    }

3.5.4 AP_AHRS_Backend::airspeed_vector_true

dummy function，获取空速矢量。

    // return estimate of true airspeed vector in body frame in m/s
    // returns false if estimate is unavailable
    virtual bool airspeed_vector_true(Vector3f &vec) const WARN_IF_UNUSED {
        return false;
    }

3.5.5 AP_AHRS_Backend::get_velocity_NED

dummy function，获取NED坐标下的速度矢量。

    // return a ground velocity in meters/second, North/East/Down
    // order. This will only be accurate if have_inertial_nav() is
    // true
    virtual bool get_velocity_NED(Vector3f &vec) const WARN_IF_UNUSED {
        return false;
    }

3.5.6 AP_AHRS_Backend::get_vert_pos_rate_D

获取垂直速度导数，不同于垂直速度，继承类必须重载该函数实现。

    // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops.
    // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for.
    virtual bool get_vert_pos_rate_D(float &velocity) const = 0;

3.5.7 AP_AHRS_Backend::get_control_limits

获取速度限制和XY导航增益比例因子，继承类必须重载该函数实现。

    virtual void get_control_limits(float &ekfGndSpdLimit, float &controlScaleXY) const = 0;

3.5.8 AP_AHRS_Backend::get_EAS2TAS

获取真空速比率，不同的继承类有不同的实现方式：

• AP_AHRS::get_EAS2TAS
• AP_AHRS_View::get_EAS2TAS

    // get apparent to true airspeed ratio
    static float get_EAS2TAS(void);

// get apparent to true airspeed ratio
float AP_AHRS_Backend::get_EAS2TAS(void) {
    return AP::baro()._get_EAS2TAS();
}

3.6 位置函数

3.6.1 AP_AHRS_Backend::set_origin/get_origin

设置/获取起始点位置，继承类必须重载该函数实现。

    virtual bool set_origin(const Location &loc) {
        return false;
    }
    virtual bool get_origin(Location &ret) const = 0;

3.6.2 AP_AHRS_Backend::get_relative_position_NED_origin

获取NED坐标下的起始点，继承类必须重载该函数实现。

    // return a position relative to origin in meters, North/East/Down
    // order. This will only be accurate if have_inertial_nav() is
    // true
    virtual bool get_relative_position_NED_origin(Vector3f &vec) const WARN_IF_UNUSED {
        return false;
    }

3.6.3 AP_AHRS_Backend::get_relative_position_NE_origin

获取NE坐标下的起始点，继承类必须重载该函数实现。

    // return a position relative to origin in meters, North/East
    // order. Return true if estimate is valid
    virtual bool get_relative_position_NE_origin(Vector2f &vecNE) const WARN_IF_UNUSED {
        return false;
    }

3.6.4 AP_AHRS_Backend::get_relative_position_D_origin

获取Z坐标下的起始点，继承类必须重载该函数实现。

    // return a Down position relative to origin in meters
    // Return true if estimate is valid
    virtual bool get_relative_position_D_origin(float &posD) const WARN_IF_UNUSED {
        return false;
    }

3.6.5 AP_AHRS_Backend::get_hgt_ctrl_limit

获取控制回路中要观察的最大高度（以米为单位）及其有效性标志。

    // get_hgt_ctrl_limit - get maximum height to be observed by the
    // control loops in meters and a validity flag.  It will return
    // false when no limiting is required
    virtual bool get_hgt_ctrl_limit(float &limit) const WARN_IF_UNUSED { return false; };

3.6.6 AP_AHRS_Backend::set_terrain_hgt_stable

设置地面参考高度稳定。

    // 如果地面稳定到足以用作高度参考，则设置为true
    // 这与地形跟随无关
    // Set to true if the terrain underneath is stable enough to be used as a height reference
    // this is not related to terrain following
    virtual void set_terrain_hgt_stable(bool stable) {}

3.6.7 AP_AHRS_Backend::get_hagl

获取获取最近一次估计的高度，并返回有效bool。

    // get latest altitude estimate above ground level in meters and validity flag
    virtual bool get_hagl(float &height) const WARN_IF_UNUSED { return false; }

注：详见，AP_AHRS::get_hagl

3.7 磁力计函数

3.7.1 AP_AHRS_Backend::use_compass

判断是否使用磁力计，继承类必须重载该函数实现。

    // return true if we will use compass for yaw
    virtual bool use_compass(void) = 0;

3.7.2 AP_AHRS_Backend::get_mag_field_correction

获取机体坐标系的磁力修正矢量。

    // returns the estimated magnetic field offsets in body frame
    virtual bool get_mag_field_correction(Vector3f &ret) const WARN_IF_UNUSED {
        return false;
    }

注：详见，AP_AHRS::get_mag_field_correction

3.7.3 AP_AHRS_Backend::get_mag_field_NED

获取NED坐标系的磁力场矢量。

    virtual bool get_mag_field_NED(Vector3f &vec) const {
        return false;
    }

注：详见，AP_AHRS::get_mag_field_NED

3.7.4 AP_AHRS_Backend::get_mag_offsets

获取磁力场偏置参数。

    virtual bool get_mag_offsets(uint8_t mag_idx, Vector3f &magOffsets) const {
        return false;
    }

注：详见，AP_AHRS::get_mag_offsets

3.8 创新量函数

3.8.1 AP_AHRS_Backend::get_innovations

获取滤波器（通常是扩展卡尔曼滤波器 EKF）的创新量。创新量指的是滤波器中测量与预测之间的残差或偏差，用于评估滤波器对系统状态的估计质量以及测量的一致性。通过分析创新量，可以评估滤波器对系统状态的准确性和系统中误差的校正程度。

    // return the innovations for the specified instance
    // An out of range instance (eg -1) returns data for the primary instance
    virtual bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const {
        return false;
    }

注：详见，AP_AHRS::get_innovations

3.8.2 AP_AHRS_Backend::get_filter_status

获取滤波器状态。

    virtual bool get_filter_status(union nav_filter_status &status) const {
        return false;
    }

union nav_filter_status {
    struct {
        bool attitude           : 1; // 0 - true if attitude estimate is valid
        bool horiz_vel          : 1; // 1 - true if horizontal velocity estimate is valid
        bool vert_vel           : 1; // 2 - true if the vertical velocity estimate is valid
        bool horiz_pos_rel      : 1; // 3 - true if the relative horizontal position estimate is valid
        bool horiz_pos_abs      : 1; // 4 - true if the absolute horizontal position estimate is valid
        bool vert_pos           : 1; // 5 - true if the vertical position estimate is valid
        bool terrain_alt        : 1; // 6 - true if the terrain height estimate is valid
        bool const_pos_mode     : 1; // 7 - true if we are in const position mode
        bool pred_horiz_pos_rel : 1; // 8 - true if filter expects it can produce a good relative horizontal position estimate - used before takeoff
        bool pred_horiz_pos_abs : 1; // 9 - true if filter expects it can produce a good absolute horizontal position estimate - used before takeoff
        bool takeoff_detected   : 1; // 10 - true if optical flow takeoff has been detected
        bool takeoff            : 1; // 11 - true if filter is compensating for baro errors during takeoff
        bool touchdown          : 1; // 12 - true if filter is compensating for baro errors during touchdown
        bool using_gps          : 1; // 13 - true if we are using GPS position
        bool gps_glitching      : 1; // 14 - true if GPS glitching is affecting navigation accuracy
        bool gps_quality_good   : 1; // 15 - true if we can use GPS for navigation
        bool initalized         : 1; // 16 - true if the EKF has ever been healthy
        bool rejecting_airspeed : 1; // 17 - true if we are rejecting airspeed data
        bool dead_reckoning     : 1; // 18 - true if we are dead reckoning (e.g. no position or velocity source)
    } flags;
    uint32_t value;
};

注：详见，AP_AHRS::get_filter_status

3.8.3 AP_AHRS_Backend::get_variances

获取创新方差归一化的偏差，其中值为0表示测量与EKF解决方案完全一致，值为1表示滤波器接受的最大不一致性。

    // get_variances - provides the innovations normalised using the innovation variance where a value of 0
    // indicates perfect consistency between the measurement and the EKF solution and a value of 1 is the maximum
    // inconsistency that will be accepted by the filter
    // boolean false is returned if variances are not available
    virtual bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const {
        return false;
    }

注：详见，AP_AHRS::get_variances

3.8.4 AP_AHRS_Backend::get_vel_innovations_and_variances_for_source

获取创新量和偏差值。

    // get a source's velocity innovations.  source should be from 0 to 7 (see AP_NavEKF_Source::SourceXY)
    // returns true on success and results are placed in innovations and variances arguments
    virtual bool get_vel_innovations_and_variances_for_source(uint8_t source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED {
        return false;
    }
	
    enum class SourceXY : uint8_t {
        NONE = 0,
        // BARO = 1 (not applicable)
        // RANGEFINDER = 2 (not applicable)
        GPS = 3,
        BEACON = 4,
        OPTFLOW = 5,
        EXTNAV = 6,
        WHEEL_ENCODER = 7
    };

注：详见，AP_AHRS::get_vel_innovations_and_variances_for_source

3.3.30 AP_AHRS_Backend::send_ekf_status_report

发送EKF状态报告，继承类必须重载该函数实现。

    virtual void send_ekf_status_report(class GCS_MAVLINK &link) const = 0;

3.9 辅助函数

3.9.1 AP_AHRS_Backend::attitudes_consistent

默认姿态一致性正常。

• EKFType::DCM //重载
• EKFType::SIM //默认
• EKFType::EXTERNAL //默认
• EKFType::TWO //重载
• EKFType::THREE //重载

注：详见，AP_AHRS::attitudes_consistent

    // check all cores providing consistent attitudes for prearm checks
    virtual bool attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const { return true; }

3.9.2 AP_AHRS_Backend::check_lane_switch

dummy function.

    // see if EKF lane switching is possible to avoid EKF failsafe
    virtual void check_lane_switch(void) {}

注：详见，AP_AHRS::check_lane_switch

3.9.3 AP_AHRS_Backend::using_noncompass_for_yaw

默认不需要使用磁力计来确定机头方向。

    // check if non-compass sensor is providing yaw.  Allows compass pre-arm checks to be bypassed
    virtual bool using_noncompass_for_yaw(void) const { return false; }

注：详见，AP_AHRS::using_noncompass_for_yaw

3.9.4 AP_AHRS_Backend::using_extnav_for_yaw

默认不需要使用外部导航来确定机头方向。

    // check if external nav is providing yaw
    virtual bool using_extnav_for_yaw(void) const { return false; }

注：详见，AP_AHRS::using_extnav_for_yaw

3.9.5 AP_AHRS_Backend::request_yaw_reset

dummy function.

    // request EKF yaw reset to try and avoid the need for an EKF lane switch or failsafe
    virtual void request_yaw_reset(void) {}

注：详见，AP_AHRS::request_yaw_reset

3.9.6 AP_AHRS_Backend::set_posvelyaw_source_set

dummy function.

    // set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
    virtual void set_posvelyaw_source_set(uint8_t source_set_idx) {}

注：详见，AP_AHRS::set_posvelyaw_source_set

3.9.7 AP_AHRS_Backend::have_inertial_nav

判断是否有具有惯性导航。

    // return true if the AHRS object supports inertial navigation,
    // with very accurate position and velocity
    virtual bool have_inertial_nav(void) const {
        return false;
    }

注：详见，AP_AHRS::have_inertial_nav

3.10 共性抽象

3.10.1 AP_AHRS_Backend::airspeed_sensor_enabled

空速传感使能判断函数。

    // return true if airspeed comes from an airspeed sensor, as
    // opposed to an IMU estimate
    static bool airspeed_sensor_enabled(void) {
    #if AP_AIRSPEED_ENABLED
        const AP_Airspeed *_airspeed = AP::airspeed();
        return _airspeed != nullptr && _airspeed->use() && _airspeed->healthy();
    #else
        return false;
    #endif
    }

    // return true if airspeed comes from a specific airspeed sensor, as
    // opposed to an IMU estimate
    static bool airspeed_sensor_enabled(uint8_t airspeed_index) {
    #if AP_AIRSPEED_ENABLED
        const AP_Airspeed *_airspeed = AP::airspeed();
        return _airspeed != nullptr && _airspeed->use(airspeed_index) && _airspeed->healthy(airspeed_index);
    #else
        return false;
    #endif
    }

3.10.2 AP_AHRS_Backend::groundspeed

获取地面一维速度。

    // return ground speed estimate in meters/second. Used by ground vehicles.
    float groundspeed(void) {
        return groundspeed_vector().length();
    }

4. 总结

结合上面AP_AHRS_Backend类的API研读：

• 从设计的角度来说，是一个后端驱动模版；
• 从抽象的角度看，是设计共性标准化（虚函数，共性实现）；
• 从代码角度，有些离散，可能是SIM、DCM、External、EKF2、EKF3场景和算法的一个历史变化（后续可能还会有UKF算法）；

通过走读代码，可以逐步的熟悉代码历史以及设计思想。很多历史问题是无法避免的和回避的。

正如苏格拉底所说的：“我知道我一无所知。”（“I know that I know nothing.”）

“苏格拉底怀疑主义”是分析问题，认识世界非常好的手段！

5. 参考资料

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