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 構造函數

    // ConstructorAP_AHRS_Backend() {}

3.1.2 析構函數

    // empty virtual destructorvirtual ~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 selectedvirtual 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 sensorvirtual uint8_t get_primary_accel_index(void) const {
#if AP_INERTIALSENSOR_ENABLEDreturn AP::ins().get_first_usable_accel();
#elsereturn 0;
#endif}

3.2.3 AP_AHRS_Backend::get_primary_gyro_index

當前使用的陀螺儀傳感器，從0開始代表第一個IMU。

    // get the index of the current primary gyro sensorvirtual uint8_t get_primary_gyro_index(void) const {
#if AP_INERTIALSENSOR_ENABLEDreturn AP::ins().get_first_usable_gyro();
#elsereturn 0;
#endif}

3.3 重要方法

3.3.1 AP_AHRS_Backend::update

定期更新的AHRS方法，繼承類必須重載該函數實現。

    // Methodsvirtual 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 checkedvirtual 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 initialisationvirtual 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) axesvirtual 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 recalculatedvirtual void reset_gyro_drift(void) = 0;

3.4.2 AP_AHRS_Backend::reset

重置IMU，繼承類必須重載該函數實現。

    // reset the current attitude, used on new IMU calibrationvirtual 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 occurredvirtual 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 occurredvirtual 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 occurredvirtual 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 occurredvirtual 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 falsevirtual 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/svirtual 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 estimatevirtual 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 estimatebool 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 unavailablevirtual 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// truevirtual 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 ratiostatic 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// truevirtual 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 validvirtual 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 validvirtual 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 requiredvirtual 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 followingvirtual 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 flagvirtual 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 yawvirtual bool use_compass(void) = 0;

3.7.2 AP_AHRS_Backend::get_mag_field_correction

獲取機體坐標系的磁力修正矢量。

    // returns the estimated magnetic field offsets in body framevirtual 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 instancevirtual 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 validbool horiz_vel          : 1; // 1 - true if horizontal velocity estimate is validbool vert_vel           : 1; // 2 - true if the vertical velocity estimate is validbool horiz_pos_rel      : 1; // 3 - true if the relative horizontal position estimate is validbool horiz_pos_abs      : 1; // 4 - true if the absolute horizontal position estimate is validbool vert_pos           : 1; // 5 - true if the vertical position estimate is validbool terrain_alt        : 1; // 6 - true if the terrain height estimate is validbool const_pos_mode     : 1; // 7 - true if we are in const position modebool pred_horiz_pos_rel : 1; // 8 - true if filter expects it can produce a good relative horizontal position estimate - used before takeoffbool pred_horiz_pos_abs : 1; // 9 - true if filter expects it can produce a good absolute horizontal position estimate - used before takeoffbool takeoff_detected   : 1; // 10 - true if optical flow takeoff has been detectedbool takeoff            : 1; // 11 - true if filter is compensating for baro errors during takeoffbool touchdown          : 1; // 12 - true if filter is compensating for baro errors during touchdownbool using_gps          : 1; // 13 - true if we are using GPS positionbool gps_glitching      : 1; // 14 - true if GPS glitching is affecting navigation accuracybool gps_quality_good   : 1; // 15 - true if we can use GPS for navigationbool initalized         : 1; // 16 - true if the EKF has ever been healthybool rejecting_airspeed : 1; // 17 - true if we are rejecting airspeed databool 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 availablevirtual 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 argumentsvirtual 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 checksvirtual 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 failsafevirtual 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 bypassedvirtual 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 yawvirtual 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 failsafevirtual 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=tertiaryvirtual 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 velocityvirtual 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 estimatestatic bool airspeed_sensor_enabled(void) {#if AP_AIRSPEED_ENABLEDconst AP_Airspeed *_airspeed = AP::airspeed();return _airspeed != nullptr && _airspeed->use() && _airspeed->healthy();#elsereturn false;#endif}// return true if airspeed comes from a specific airspeed sensor, as// opposed to an IMU estimatestatic bool airspeed_sensor_enabled(uint8_t airspeed_index) {#if AP_AIRSPEED_ENABLEDconst AP_Airspeed *_airspeed = AP::airspeed();return _airspeed != nullptr && _airspeed->use(airspeed_index) && _airspeed->healthy(airspeed_index);#elsereturn 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設計