AP_AHRS: move home and origin methods to frontend

libraries/AP_AHRS/AP_AHRS.cpp

@@ -270,7 +270,13 @@ void AP_AHRS::update(bool skip_ins_update)
 {
     // support locked access functions to AHRS data
     WITH_SEMAPHORE(_rsem);
-    
+
+    // see if we have to restore home after a watchdog reset:
+    if (!_checked_watchdog_home) {
+        load_watchdog_home();
+        _checked_watchdog_home = true;
+    }
+
     // drop back to normal priority if we were boosted by the INS
     // calling delay_microseconds_boost()
     hal.scheduler->boost_end();
@@ -654,9 +660,10 @@ void AP_AHRS::reset(bool recover_eulers)
 {
     // support locked access functions to AHRS data
     WITH_SEMAPHORE(_rsem);
-    
+
     AP_AHRS_DCM::reset(recover_eulers);
     _dcm_attitude = {roll, pitch, yaw};
+
 #if HAL_NAVEKF2_AVAILABLE
     if (_ekf2_started) {
         _ekf2_started = EKF2.InitialiseFilter();

libraries/AP_AHRS/AP_AHRS.h

@@ -144,10 +144,10 @@ public:
     // set the EKF's origin location in 10e7 degrees.  This should only
     // be called when the EKF has no absolute position reference (i.e. GPS)
     // from which to decide the origin on its own
-    bool set_origin(const Location &loc) override WARN_IF_UNUSED;
+    bool set_origin(const Location &loc) WARN_IF_UNUSED;
 
     // returns the inertial navigation origin in lat/lon/alt
-    bool get_origin(Location &ret) const override WARN_IF_UNUSED;
+    bool get_origin(Location &ret) const WARN_IF_UNUSED;
 
     bool have_inertial_nav() const override;
 
@@ -337,6 +337,37 @@ public:
     // return SSA
     float getSSA(void) const { return _SSA; }
 
+    /*
+     * home-related functionality
+     */
+
+    // get the home location. This is const to prevent any changes to
+    // home without telling AHRS about the change
+    const struct Location &get_home(void) const {
+        return _home;
+    }
+
+    // functions to handle locking of home.  Some vehicles use this to
+    // allow GCS to lock in a home location.
+    void lock_home() {
+        _home_locked = true;
+    }
+    bool home_is_locked() const {
+        return _home_locked;
+    }
+
+    // returns true if home is set
+    bool home_is_set(void) const {
+        return _home_is_set;
+    }
+
+    // set the home location in 10e7 degrees. This should be called
+    // when the vehicle is at this position. It is assumed that the
+    // current barometer and GPS altitudes correspond to this altitude
+    bool set_home(const Location &loc) WARN_IF_UNUSED;
+
+    void Log_Write_Home_And_Origin();
+
 protected:
     // optional view class
     AP_AHRS_View *_view;
@@ -396,6 +427,15 @@ private:
     // get the index of the current primary IMU
     uint8_t get_primary_IMU_index(void) const;
 
+    /*
+     * home-related state
+     */
+    void load_watchdog_home();
+    bool _checked_watchdog_home;
+    struct Location _home;
+    bool _home_is_set :1;
+    bool _home_locked :1;
+
     // avoid setting current state repeatedly across all cores on all EKFs:
     enum class TriState {
         False = 0,

libraries/AP_AHRS/AP_AHRS_Backend.cpp

@@ -304,7 +304,7 @@ Vector2f AP_AHRS_Backend::body_to_earth2D(const Vector2f &bf) const
 }
 
 // log ahrs home and EKF origin
-void AP_AHRS_Backend::Log_Write_Home_And_Origin()
+void AP_AHRS::Log_Write_Home_And_Origin()
 {
     AP_Logger *logger = AP_Logger::get_singleton();
     if (logger == nullptr) {

libraries/AP_AHRS/AP_AHRS_Backend.h

@@ -386,41 +386,6 @@ public:
         return false;
     }
 
-    // get the home location. This is const to prevent any changes to
-    // home without telling AHRS about the change
-    const struct Location &get_home(void) const {
-        return _home;
-    }
-
-    // functions to handle locking of home.  Some vehicles use this to
-    // allow GCS to lock in a home location.
-    void lock_home() {
-        _home_locked = true;
-    }
-    bool home_is_locked() const {
-        return _home_locked;
-    }
-
-    // returns true if home is set
-    bool home_is_set(void) const {
-        return _home_is_set;
-    }
-
-    // set the home location in 10e7 degrees. This should be called
-    // when the vehicle is at this position. It is assumed that the
-    // current barometer and GPS altitudes correspond to this altitude
-    virtual bool set_home(const Location &loc) WARN_IF_UNUSED = 0;
-
-    // set the EKF's origin location in 10e7 degrees.  This should only
-    // be called when the EKF has no absolute position reference (i.e. GPS)
-    // from which to decide the origin on its own
-    virtual bool set_origin(const Location &loc) WARN_IF_UNUSED { return false; }
-
-    // returns the inertial navigation origin in lat/lon/alt
-    virtual bool get_origin(Location &ret) const  WARN_IF_UNUSED { return false; }
-
-    void Log_Write_Home_And_Origin();
-
     // return true if the AHRS object supports inertial navigation,
     // with very accurate position and velocity
     virtual bool have_inertial_nav(void) const {
@@ -629,11 +594,6 @@ protected:
     Vector2f _hp; // ground vector high-pass filter
     Vector2f _lastGndVelADS; // previous HPF input
 
-    // reference position for NED positions
-    struct Location _home;
-    bool _home_is_set :1;
-    bool _home_locked :1;
-
     // helper trig variables
     float _cos_roll{1.0f};
     float _cos_pitch{1.0f};

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -50,7 +50,7 @@ AP_AHRS_DCM::reset_gyro_drift(void)
 
 
 /* if this was a watchdog reset then get home from backup registers */
-void AP_AHRS_DCM::load_watchdog_home()
+void AP_AHRS::load_watchdog_home()
 {
     const AP_HAL::Util::PersistentData &pd = hal.util->persistent_data;
     if (hal.util->was_watchdog_reset() && (pd.home_lat != 0 || pd.home_lon != 0)) {
@@ -70,11 +70,6 @@ AP_AHRS_DCM::update(bool skip_ins_update)
     // support locked access functions to AHRS data
     WITH_SEMAPHORE(_rsem);
 
-    if (_last_startup_ms == 0) {
-        _last_startup_ms = AP_HAL::millis();
-        load_watchdog_home();
-    }
-
     AP_InertialSensor &_ins = AP::ins();
 
     if (!skip_ins_update) {
@@ -230,9 +225,6 @@ AP_AHRS_DCM::reset(bool recover_eulers)
 
     }
 
-    if (_last_startup_ms == 0) {
-        load_watchdog_home();
-    }
     _last_startup_ms = AP_HAL::millis();
 }
 
@@ -1038,7 +1030,7 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const
         gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
         loc.alt = gps.location().alt;
     } else {
-        loc.alt = baro.get_altitude() * 100 + _home.alt;
+        loc.alt = baro.get_altitude() * 100 + AP::ahrs().get_home().alt;
     }
     loc.relative_alt = 0;
     loc.terrain_alt = 0;
@@ -1094,7 +1086,7 @@ bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret)
     return true;
 }
 
-bool AP_AHRS_DCM::set_home(const Location &loc)
+bool AP_AHRS::set_home(const Location &loc)
 {
     WITH_SEMAPHORE(_rsem);
     // check location is valid
@@ -1135,7 +1127,7 @@ void AP_AHRS_DCM::get_relative_position_D_home(float &posD) const
     const auto &gps = AP::gps();
     if (_gps_use == GPSUse::EnableWithHeight &&
         gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
-        posD = (_home.alt - gps.location().alt) * 0.01;
+        posD = (AP::ahrs().get_home().alt - gps.location().alt) * 0.01;
     } else {
         posD = -AP::baro().get_altitude();
     }

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -102,7 +102,6 @@ public:
     // return the quaternion defining the rotation from NED to XYZ (body) axes
     bool get_quaternion(Quaternion &quat) const override WARN_IF_UNUSED;
 
-    bool set_home(const Location &loc) override WARN_IF_UNUSED;
     void estimate_wind(void);
 
     // is the AHRS subsystem healthy?
@@ -132,7 +131,6 @@ private:
     void            euler_angles(void);
     bool            have_gps(void) const;
     bool            use_fast_gains(void) const;
-    void            load_watchdog_home();
     void            backup_attitude(void);
 
     // primary representation of attitude of board used for all inertial calculations