AP_AHRS: return location inside backend_results structure

with the change to cache results inside AP::ahrs().state we no longer need to worry about the backend's attempts to project the last-known-location forwards according to amount of time elapsed since that last-known-location was calculated.

libraries/AP_AHRS/AP_AHRS.cpp

@@ -703,7 +703,7 @@ bool AP_AHRS::_get_location(Location &loc) const
 {
     switch (active_EKF_type()) {
     case EKFType::NONE:
-        return dcm.get_location(loc);
+        return dcm_estimates.get_location(loc);
 
 #if HAL_NAVEKF2_AVAILABLE
     case EKFType::TWO:
@@ -723,19 +723,18 @@ bool AP_AHRS::_get_location(Location &loc) const
 
 #if AP_AHRS_SIM_ENABLED
     case EKFType::SIM:
-        return sim.get_location(loc);
+        return sim_estimates.get_location(loc);
 #endif
 
 #if HAL_EXTERNAL_AHRS_ENABLED
-    case EKFType::EXTERNAL: {
+    case EKFType::EXTERNAL:
-        return external.get_location(loc);
+        return external_estimates.get_location(loc);
-    }
 #endif
     }
 
     // fall back to position from DCM
     if (!always_use_EKF()) {
-        return dcm.get_location(loc);
+        return dcm_estimates.get_location(loc);
     }
     return false;
 }
@@ -1178,7 +1177,10 @@ bool AP_AHRS::_get_secondary_position(Location &loc) const
 
     case EKFType::NONE:
         // return DCM position
-        dcm.get_location(loc);
+        loc = dcm_estimates.location;
+        // FIXME: we intentionally do not return whether location is
+        // actually valid here so we continue to send mavlink messages
+        // and log data:
         return true;
 
 #if HAL_NAVEKF2_AVAILABLE
@@ -1204,7 +1206,7 @@ bool AP_AHRS::_get_secondary_position(Location &loc) const
 #if HAL_EXTERNAL_AHRS_ENABLED
     case EKFType::EXTERNAL:
         // External is secondary
-        return external.get_location(loc);
+        return external_estimates.get_location(loc);
 #endif
     }
 

libraries/AP_AHRS/AP_AHRS.h

@@ -812,25 +812,6 @@ private:
     // poke AP_Notify based on values from status
     void update_notify_from_filter_status(const nav_filter_status &status);
 
-    /*
-     *  backends (and their results)
-     */
-    AP_AHRS_DCM dcm{_kp_yaw, _kp, gps_gain, beta, _gps_use, _gps_minsats};
-    struct AP_AHRS_Backend::Estimates dcm_estimates;
-#if AP_AHRS_SIM_ENABLED
-#if HAL_NAVEKF3_AVAILABLE
-    AP_AHRS_SIM sim{EKF3};
-#else
-    AP_AHRS_SIM sim;
-#endif
-    struct AP_AHRS_Backend::Estimates sim_estimates;
-#endif
-
-#if HAL_EXTERNAL_AHRS_ENABLED
-    AP_AHRS_External external;
-    struct AP_AHRS_Backend::Estimates external_estimates;
-#endif
-
     /*
      * copy results from a backend over AP_AHRS canonical results.
      * This updates member variables like roll and pitch, as well as
@@ -957,6 +938,26 @@ private:
         Vector3f velocity_NED;
         bool velocity_NED_ok;
     } state;
+
+    /*
+     *  backends (and their results)
+     */
+    AP_AHRS_DCM dcm{_kp_yaw, _kp, gps_gain, beta, _gps_use, _gps_minsats};
+    struct AP_AHRS_Backend::Estimates dcm_estimates;
+#if AP_AHRS_SIM_ENABLED
+#if HAL_NAVEKF3_AVAILABLE
+    AP_AHRS_SIM sim{EKF3};
+#else
+    AP_AHRS_SIM sim;
+#endif
+    struct AP_AHRS_Backend::Estimates sim_estimates;
+#endif
+
+#if HAL_EXTERNAL_AHRS_ENABLED
+    AP_AHRS_External external;
+    struct AP_AHRS_Backend::Estimates external_estimates;
+#endif
+
 };
 
 namespace AP {

libraries/AP_AHRS/AP_AHRS_Backend.h

@@ -24,6 +24,7 @@
 #include <inttypes.h>
 #include <AP_Airspeed/AP_Airspeed.h>
 #include <AP_InertialSensor/AP_InertialSensor.h>
+#include <AP_Common/Location.h>
 
 #define AP_AHRS_TRIM_LIMIT 10.0f        // maximum trim angle in degrees
 #define AP_AHRS_RP_P_MIN   0.05f        // minimum value for AHRS_RP_P parameter
@@ -57,6 +58,14 @@ public:
         Vector3f gyro_drift;
         Vector3f accel_ef;
         Vector3f accel_bias;
+
+        Location location;
+        bool location_valid;
+
+        bool get_location(Location &loc) const {
+            loc = location;
+            return location_valid;
+        };
     };
 
     // init sets up INS board orientation
@@ -109,10 +118,6 @@ public:
     // reset the current attitude, used on new IMU calibration
     virtual void reset() = 0;
 
-    // get our current position estimate. Return true if a position is available,
-    // otherwise false. This call fills in lat, lng and alt
-    virtual bool get_location(class Location &loc) const WARN_IF_UNUSED = 0;
-
     // get latest altitude estimate above ground level in meters and validity flag
     virtual bool get_hagl(float &height) const WARN_IF_UNUSED { return false; }
 

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -124,6 +124,8 @@ void AP_AHRS_DCM::get_results(AP_AHRS_Backend::Estimates &results)
     results.gyro_estimate = _omega;
     results.gyro_drift = _omega_I;
     results.accel_ef = _accel_ef;
+
+    results.location_valid = get_location(results.location);
 }
 
 /*

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -60,9 +60,6 @@ public:
         return have_initial_yaw;
     }
 
-    // dead-reckoning support
-    virtual bool get_location(Location &loc) const override;
-
     // status reporting
     float           get_error_rp() const {
         return _error_rp;
@@ -132,6 +129,9 @@ public:
 
 private:
 
+    // dead-reckoning support
+    bool get_location(Location &loc) const;
+
     // settable parameters
     AP_Float &_kp_yaw;
     AP_Float &_kp;

libraries/AP_AHRS/AP_AHRS_External.cpp

@@ -36,12 +36,8 @@ void AP_AHRS_External::get_results(AP_AHRS_Backend::Estimates &results)
     const Vector3f accel = AP::externalAHRS().get_accel();
     const Vector3f accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel;
     results.accel_ef = accel_ef;
-}
 
-
+    results.location_valid = AP::externalAHRS().get_location(results.location);
-bool AP_AHRS_External::get_location(struct Location &loc) const
-{
-    return AP::externalAHRS().get_location(loc);
 }
 
 bool AP_AHRS_External::get_quaternion(Quaternion &quat) const

libraries/AP_AHRS/AP_AHRS_External.h

@@ -43,9 +43,6 @@ public:
     void            get_results(Estimates &results) override;
     void            reset() override {}
 
-    // dead-reckoning support
-    virtual bool get_location(struct Location &loc) const override;
-
     // return a wind estimation vector, in m/s
     bool wind_estimate(Vector3f &ret) const override {
         return false;

libraries/AP_AHRS/AP_AHRS_SIM.cpp

@@ -255,6 +255,8 @@ void AP_AHRS_SIM::get_results(AP_AHRS_Backend::Estimates &results)
     const Vector3f &accel = _ins.get_accel();
     results.accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel;
 
+    results.location_valid = get_location(results.location);
+
 #if HAL_NAVEKF3_AVAILABLE
     if (_sitl->odom_enable) {
         // use SITL states to write body frame odometry data at 20Hz

libraries/AP_AHRS/AP_AHRS_SIM.h

@@ -60,9 +60,6 @@ public:
     void            get_results(Estimates &results) override;
     void            reset() override { return; }
 
-    // dead-reckoning support
-    virtual bool get_location(Location &loc) const override;
-
     // get latest altitude estimate above ground level in meters and validity flag
     bool get_hagl(float &hagl) const override WARN_IF_UNUSED;
 
@@ -119,6 +116,9 @@ public:
 
 private:
 
+    // dead-reckoning support
+    bool get_location(Location &loc) const;
+
 #if HAL_NAVEKF3_AVAILABLE
     // a reference to the EKF3 backend that we can use to send in
     // body-frame-odometry data into the EKF.  Rightfully there should