Clean up AP_AHRS_Backend interface

libraries/AP_AHRS/AP_AHRS.h

@@ -725,6 +725,7 @@ class AP_AHRS {
     AP_Int8 _gps_minsats;
 
     EKFType active_EKF_type(void) const { return state.active_EKF; }
+    AP_AHRS_Backend::Estimates *active_estimates;
 
     bool always_use_EKF() const {
         return _ekf_flags & FLAG_ALWAYS_USE_EKF;

libraries/AP_AHRS/AP_AHRS_Backend.h

@@ -51,47 +51,160 @@ class AP_AHRS_Backend
 
     // structure to retrieve results from backends:
     struct Estimates {
+        friend class AP_AHRS_DCM;
+        friend class AP_AHRS_External;
+        friend class AP_AHRS_SIM;
+
+        bool initialised;
+        bool healthy;
+
+        uint8_t primary_imu_index;
+
         float roll_rad;
         float pitch_rad;
         float yaw_rad;
         Matrix3f dcm_matrix;
+        Quaternion quat;
+
+        bool attitude_valid;
+
+        // return the quaternion defining the rotation from NED to XYZ
+        // (body) axes
+        bool get_quaternion(Quaternion &_quat) const WARN_IF_UNUSED {
+            _quat = quat;
+            return attitude_valid;
+        }
+
         Vector3f gyro_estimate;
         Vector3f gyro_drift;
         Vector3f accel_ef;
         Vector3f accel_bias;
 
-        Location location;
-        bool location_valid;
+        bool get_velocity_NED(Vector3f &vel) const WARN_IF_UNUSED {
+            if (velocity_NED_valid) {
+                vel = velocity_NED;
+            }
+            return velocity_NED_valid;
+        };
+        bool get_vert_pos_rate_D(float &velocity) const WARN_IF_UNUSED {
+            velocity = vert_pos_rate_D;
+            return vert_pos_rate_D_valid;
+        }
+
+        // ground vector estimate in meters/second, in North/East order
+        Vector2f groundspeed_vector;
 
-        bool get_location(Location &loc) const {
+        bool get_location(Location &loc) const WARN_IF_UNUSED {
             loc = location;
             return location_valid;
         };
-    };
 
-    // init sets up INS board orientation
-    virtual void init();
+        // origin-relative movement:
+        bool get_origin(Location &ret) const {
+            ret = origin;
+            return origin_valid;
+        }
+        bool  get_relative_position_NED_origin(Vector3f &vec) const WARN_IF_UNUSED {
+            vec = relative_position_NED_origin;
+            return relative_position_NED_origin_valid;
+        }
 
-    // return the index of the primary core or -1 if no primary core selected
-    virtual int8_t get_primary_core_index() const { return -1; }
+        bool  get_relative_position_NE_origin(Vector2f &vec) const WARN_IF_UNUSED {
+            vec = relative_position_NE_origin;
+            return relative_position_NE_origin_valid;
+        }
 
-    // get the index of the current primary accelerometer sensor
-    virtual uint8_t get_primary_accel_index(void) const {
-#if AP_INERTIALSENSOR_ENABLED
-        return AP::ins().get_first_usable_accel();
-#else
-        return 0;
-#endif
-    }
+        bool  get_relative_position_D_origin(float &down) const WARN_IF_UNUSED {
+            down = relative_position_D_origin;
+            return relative_position_D_origin_valid;
+        }
 
-    // get the index of the current primary gyro sensor
-    virtual uint8_t get_primary_gyro_index(void) const {
-#if AP_INERTIALSENSOR_ENABLED
-        return AP::ins().get_first_usable_gyro();
-#else
-        return 0;
-#endif
-    }
+        bool get_hagl(float &height) const WARN_IF_UNUSED {
+            height = hagl;
+            return hagl_valid;
+        }
+
+        bool wind_estimate(Vector3f &_wind) const WARN_IF_UNUSED {
+            _wind = wind;
+            return wind_valid;
+        }
+
+        void get_control_limits(float &_ekfGndSpdLimit, float &_controlScaleXY) const {
+            _ekfGndSpdLimit = ekfGndSpdLimit;
+            _controlScaleXY = controlScaleXY;
+        }
+
+        // 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 occurred
+        uint32_t getLastYawResetAngle(float &yawAng) {
+            return 0;
+        };
+
+        // 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 occurred
+        uint32_t getLastPosNorthEastReset(Vector2f &pos) WARN_IF_UNUSED {
+            return 0;
+        };
+
+        // 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 occurred
+        uint32_t getLastVelNorthEastReset(Vector2f &vel) const WARN_IF_UNUSED {
+            return 0;
+        };
+
+        // 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 occurred
+        uint32_t getLastPosDownReset(float &posDelta) WARN_IF_UNUSED {
+            return 0;
+        };
+
+    private:
+
+        Vector3f velocity_NED;
+        bool velocity_NED_valid;
+
+        // 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:
+        float vert_pos_rate_D;
+        bool vert_pos_rate_D_valid;
+
+        Location location;
+        bool location_valid;
+
+        // origin for local position:
+        Location origin;
+        bool origin_valid;
+
+        // position relative to origin in meters, North/East/Down
+        // order. This will only be accurate if have_inertial_nav() is
+        // true
+        Vector3f relative_position_NED_origin;
+        bool relative_position_NED_origin_valid;
+        // a position relative to origin in meters, North/East
+        // order
+        Vector2f relative_position_NE_origin;
+        bool relative_position_NE_origin_valid;
+        float relative_position_D_origin;
+        bool relative_position_D_origin_valid;
+
+        float hagl;  // in metres
+        bool hagl_valid;
+
+        // wind estimate, earth frame, metres/second
+        Vector3f wind;
+        bool wind_valid;
+
+        // control limits (with defaults):
+        float ekfGndSpdLimit;
+        float controlScaleXY;
+    };
+
+    // init sets up INS board orientation
+    virtual void init();
 
     // Methods
     virtual void update() = 0;
@@ -127,12 +240,6 @@ class AP_AHRS_Backend
     // reset the current attitude, used on new IMU calibration
     virtual void reset() = 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; }
-
-    // return a wind estimation vector, in m/s
-    virtual bool wind_estimate(Vector3f &wind) const = 0;
-
     // return an airspeed estimate if available. return true
     // if we have an estimate
     virtual bool airspeed_EAS(float &airspeed_ret) const WARN_IF_UNUSED { return false; }
@@ -148,12 +255,6 @@ class AP_AHRS_Backend
         return true;
     }
 
-    // return estimate of true airspeed vector in body frame in m/s
-    // returns false if estimate is unavailable
-    virtual bool airspeed_vector_true(Vector3f &vec) const WARN_IF_UNUSED {
-        return false;
-    }
-
     // get apparent to true airspeed ratio
     static float get_EAS2TAS(void);
     static float get_TAS2EAS(void) { return 1.0/get_EAS2TAS(); }
@@ -180,110 +281,14 @@ class AP_AHRS_Backend
     #endif
     }
 
-    // return a ground vector estimate in meters/second, in North/East order
-    virtual Vector2f groundspeed_vector(void) = 0;
-
-    // return a ground velocity in meters/second, North/East/Down
-    // order. This will only be accurate if have_inertial_nav() is
-    // true
-    virtual bool get_velocity_NED(Vector3f &vec) const WARN_IF_UNUSED {
-        return false;
-    }
-
-    // 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;
-
-    // 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;
     }
-    virtual bool get_origin(Location &ret) const = 0;
-
-    // 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 origin in meters, North/East
-    // order. Return true if estimate is valid
-    virtual bool get_relative_position_NE_origin(Vector2f &vecNE) const WARN_IF_UNUSED {
-        return false;
-    }
-
-    // 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 {
-        return false;
-    }
-
-    // return ground speed estimate in meters/second. Used by ground vehicles.
-    float groundspeed(void) {
-        return groundspeed_vector().length();
-    }
 
     // return true if we will use compass for yaw
     virtual bool use_compass(void) = 0;
 
-    // return the quaternion defining the rotation from NED to XYZ (body) axes
-    virtual bool get_quaternion(Quaternion &quat) const WARN_IF_UNUSED = 0;
-
-    // return true if the AHRS object supports inertial navigation,
-    // with very accurate position and velocity
-    virtual bool have_inertial_nav(void) const {
-        return false;
-    }
-
-    // is the AHRS subsystem healthy?
-    virtual bool healthy(void) const = 0;
-
-    // true if the AHRS has completed initialisation
-    virtual bool initialised(void) const {
-        return true;
-    };
-    virtual bool started(void) const {
-        return initialised();
-    };
-
-    // 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 occurred
-    virtual uint32_t getLastYawResetAngle(float &yawAng) {
-        return 0;
-    };
-
-    // 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 occurred
-    virtual uint32_t getLastPosNorthEastReset(Vector2f &pos) WARN_IF_UNUSED {
-        return 0;
-    };
-
-    // 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 occurred
-    virtual uint32_t getLastVelNorthEastReset(Vector2f &vel) const WARN_IF_UNUSED {
-        return 0;
-    };
-
-    // 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 occurred
-    virtual uint32_t getLastPosDownReset(float &posDelta) WARN_IF_UNUSED {
-        return 0;
-    };
-
     // 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
@@ -328,5 +333,4 @@ class AP_AHRS_Backend
     // this is not related to terrain following
     virtual void set_terrain_hgt_stable(bool stable) {}
 
-    virtual void get_control_limits(float &ekfGndSpdLimit, float &controlScaleXY) const = 0;
 };