diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 3353ba261962a2..36188922738438 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -2390,7 +2390,8 @@ bool AP_AHRS::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const #if AP_AHRS_SIM_ENABLED case EKFType::SIM: - return sim.get_mag_offsets(mag_idx, magOffsets); + magOffsets.zero(); + return true; #endif #if HAL_EXTERNAL_AHRS_ENABLED case EKFType::EXTERNAL: diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index 93df2997d5dece..3e8f1519f9b736 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -261,19 +261,6 @@ class AP_AHRS_Backend #endif } - // returns the estimated magnetic field offsets in body frame - virtual bool get_mag_field_correction(Vector3f &ret) const WARN_IF_UNUSED { - return false; - } - - virtual bool get_mag_field_NED(Vector3f &vec) const { - return false; - } - - virtual bool get_mag_offsets(uint8_t mag_idx, Vector3f &magOffsets) const { - return false; - } - // virtual bool set_origin(const Location &loc) { return false; diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.cpp b/libraries/AP_AHRS/AP_AHRS_SIM.cpp index 1f27d2b50e1bc6..852c8848e2d82d 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_SIM.cpp @@ -98,13 +98,6 @@ bool AP_AHRS_SIM::get_filter_status(nav_filter_status &status) const return true; } -bool AP_AHRS_SIM::get_mag_offsets(uint8_t mag_idx, Vector3f &magOffsets) const -{ - magOffsets.zero(); - - return true; -} - void AP_AHRS_SIM::send_ekf_status_report(GCS_MAVLINK &link) const { #if HAL_GCS_ENABLED diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.h b/libraries/AP_AHRS/AP_AHRS_SIM.h index 8ce0c569664f07..40c99a2c0cd27c 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.h +++ b/libraries/AP_AHRS/AP_AHRS_SIM.h @@ -77,10 +77,6 @@ class AP_AHRS_SIM : public AP_AHRS_Backend { // get_filter_status - returns filter status as a series of flags bool get_filter_status(nav_filter_status &status) const override; - // get compass offset estimates - // true if offsets are valid - bool get_mag_offsets(uint8_t mag_idx, Vector3f &magOffsets) const override; - void send_ekf_status_report(class GCS_MAVLINK &link) const override; bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const override;