AP_AHRS: make home-relative functions AHRS-frontend only

libraries/AP_AHRS/AP_AHRS.cpp

@@ -51,8 +51,8 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = {
     AP_GROUPINFO("GPS_GAIN",  2, AP_AHRS, gps_gain, 1.0f),
 
     // @Param: GPS_USE
-    // @DisplayName: AHRS DCM use GPS for navigation
-    // @Description: This controls whether to use dead-reckoning or GPS based navigation. If set to 0 then the GPS won't be used for navigation, and only dead reckoning will be used. A value of zero should never be used for normal flight. Currently this affects only the DCM-based AHRS: the EKF uses GPS whenever it is available. A value of 2 means to use GPS for height as well as position in DCM.
+    // @DisplayName: AHRS use GPS for DCM navigation and position-down
+    // @Description: This controls whether to use dead-reckoning or GPS based navigation. If set to 0 then the GPS won't be used for navigation, and only dead reckoning will be used. A value of zero should never be used for normal flight. Currently this affects only the DCM-based AHRS: the EKF uses GPS according to its own parameters. A value of 2 means to use GPS for height as well as position - both in DCM estimation and when determining altitude-above-home.
     // @Values: 0:Disabled,1:Use GPS for DCM position,2:Use GPS for DCM position and height
     // @User: Advanced
     AP_GROUPINFO("GPS_USE",  3, AP_AHRS, _gps_use, float(GPSUse::Enable)),
@@ -1639,7 +1639,13 @@ void AP_AHRS::get_relative_position_D_home(float &posD) const
     float originD;
     if (!get_relative_position_D_origin(originD) ||
         !get_origin(originLLH)) {
-        AP_AHRS_DCM::get_relative_position_D_home(posD);
+        const auto &gps = AP::gps();
+        if (_gps_use == GPSUse::EnableWithHeight &&
+            gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+            posD = (get_home().alt - gps.location().alt) * 0.01;
+        } else {
+            posD = -AP::baro().get_altitude();
+        }
         return;
     }
 

libraries/AP_AHRS/AP_AHRS.h

@@ -174,17 +174,17 @@ public:
 
     // return the relative position NED to either home or origin
     // return true if the estimate is valid
-    bool get_relative_position_NED_home(Vector3f &vec) const override;
+    bool get_relative_position_NED_home(Vector3f &vec) const;
     bool get_relative_position_NED_origin(Vector3f &vec) const override;
 
     // return the relative position NE to either home or origin
     // return true if the estimate is valid
-    bool get_relative_position_NE_home(Vector2f &posNE) const override;
+    bool get_relative_position_NE_home(Vector2f &posNE) const;
     bool get_relative_position_NE_origin(Vector2f &posNE) const override;
 
     // return the relative position down to either home or origin
     // baro will be used for the _home relative one if the EKF isn't
-    void get_relative_position_D_home(float &posD) const override;
+    void get_relative_position_D_home(float &posD) const;
     bool get_relative_position_D_origin(float &posD) const override;
 
     // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops.

libraries/AP_AHRS/AP_AHRS_Backend.h

@@ -221,24 +221,12 @@ public:
         return false;
     }
 
-    // return a position relative to home in meters, North/East/Down
-    // order. This will only be accurate if have_inertial_nav() is
-    // true
-    virtual bool get_relative_position_NED_home(Vector3f &vec) const WARN_IF_UNUSED {
-        return false;
-    }
-
     // 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;
     }
-    // return a position relative to home in meters, North/East
-    // order. Return true if estimate is valid
-    virtual bool get_relative_position_NE_home(Vector2f &vecNE) const WARN_IF_UNUSED {
-        return false;
-    }
 
     // return a position relative to origin in meters, North/East
     // order. Return true if estimate is valid
@@ -246,10 +234,6 @@ public:
         return false;
     }
 
-    // return a Down position relative to home in meters
-    // if EKF is unavailable will return the baro altitude
-    virtual void get_relative_position_D_home(float &posD) const = 0;
-
     // 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 {
@@ -427,6 +411,14 @@ public:
 
 protected:
 
+    enum class GPSUse : uint8_t {
+        Disable = 0,
+        Enable  = 1,
+        EnableWithHeight = 2,
+    };
+
+    AP_Enum<GPSUse> _gps_use;
+
     // multi-thread access support
     HAL_Semaphore _rsem;
 

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -1141,18 +1141,6 @@ bool AP_AHRS::set_home(const Location &loc)
     return true;
 }
 
-//  a relative ground position to home in meters, Down
-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 = (AP::ahrs().get_home().alt - gps.location().alt) * 0.01;
-    } else {
-        posD = -AP::baro().get_altitude();
-    }
-}
-
 /*
   check if the AHRS subsystem is healthy
 */

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -82,8 +82,6 @@ public:
         return _wind;
     }
 
-    void get_relative_position_D_home(float &posD) const override;
-
     // return an airspeed estimate if available. return true
     // if we have an estimate
     bool airspeed_estimate(float &airspeed_ret) const override;
@@ -135,13 +133,6 @@ protected:
 
     AP_Float beta;
 
-    enum class GPSUse : uint8_t {
-        Disable = 0,
-        Enable  = 1,
-        EnableWithHeight = 2,
-    };
-
-    AP_Enum<GPSUse> _gps_use;
     AP_Int8 _gps_minsats;
 
 private:

libraries/AP_AHRS/AP_AHRS_Logging.cpp

@@ -84,7 +84,7 @@ void AP_AHRS_Backend::Write_POS() const
         return;
     }
     float home, origin;
-    get_relative_position_D_home(home);
+    AP::ahrs().get_relative_position_D_home(home);
     const struct log_POS pkt{
         LOG_PACKET_HEADER_INIT(LOG_POS_MSG),
         time_us        : AP_HAL::micros64(),