AP_AHRS: move control limits into results structure

ArduPilot · Nov 22, 2023 · e223d23 · e223d23
1 parent 5665e16
commit e223d23
Show file tree

Hide file tree

Showing 8 changed files with 25 additions and 31 deletions.
diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -2299,7 +2299,7 @@ void AP_AHRS::getControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler
     switch (active_EKF_type()) {
 #if AP_AHRS_DCM_ENABLED
     case EKFType::DCM:
-        dcm.get_control_limits(ekfGndSpdLimit, ekfNavVelGainScaler);
+        dcm_estimates.get_control_limits(ekfGndSpdLimit, ekfNavVelGainScaler);
         break;
 #endif
 
@@ -2317,14 +2317,13 @@ void AP_AHRS::getControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler
 
 #if AP_AHRS_SIM_ENABLED
     case EKFType::SIM:
-        sim.get_control_limits(ekfGndSpdLimit, ekfNavVelGainScaler);
+        sim_estimates.get_control_limits(ekfGndSpdLimit, ekfNavVelGainScaler);
         break;
 #endif
 #if HAL_EXTERNAL_AHRS_ENABLED
     case EKFType::EXTERNAL:
         // no limit on gains, large vel limit
-        ekfGndSpdLimit = 400;
-        ekfNavVelGainScaler = 1;
+        external_estimates.get_control_limits(ekfGndSpdLimit, ekfNavVelGainScaler);
         break;
 #endif
     }

diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h
@@ -128,6 +128,11 @@ class AP_AHRS_Backend
             return wind_valid;
         }
 
+        void get_control_limits(float &_ekfGndSpdLimit, float &_controlScaleXY) const {
+            _ekfGndSpdLimit = ekfGndSpdLimit;
+            _controlScaleXY = controlScaleXY;
+        }
+
     private:
 
         Vector3f velocity_NED;
@@ -167,6 +172,10 @@ class AP_AHRS_Backend
         // 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
@@ -341,5 +350,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;
 };
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -157,6 +157,11 @@ void AP_AHRS_DCM::get_results(AP_AHRS_Backend::Estimates &results)
     // wind estimation:
     results.wind = _wind;
     results.wind_valid = true;
+
+    // lower gains in VTOL controllers when flying on DCM:
+    results.ekfGndSpdLimit = 50.0;
+    results.controlScaleXY = 0.5;
+
 }
 
 /*
@@ -1262,11 +1267,4 @@ bool AP_AHRS_DCM::yaw_source_available(void) const
     return AP::compass().use_for_yaw();
 }
 
-void AP_AHRS_DCM::get_control_limits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
-{
-    // lower gains in VTOL controllers when flying on DCM
-    ekfGndSpdLimit = 50.0;
-    ekfNavVelGainScaler = 0.5;
-}
-
 #endif  // AP_AHRS_DCM_ENABLED
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -102,8 +102,6 @@ class AP_AHRS_DCM : public AP_AHRS_Backend {
     // return true if DCM has a yaw source
     bool yaw_source_available(void) const;
 
-    void get_control_limits(float &ekfGndSpdLimit, float &controlScaleXY) const override;
-
 private:
 
     bool get_relative_position_NED_origin(Vector3f &vec) const;

diff --git a/libraries/AP_AHRS/AP_AHRS_External.cpp b/libraries/AP_AHRS/AP_AHRS_External.cpp
@@ -48,6 +48,10 @@ void AP_AHRS_External::get_results(AP_AHRS_Backend::Estimates &results)
     results.relative_position_NE_origin_valid = results.relative_position_NED_origin_valid;
     results.relative_position_D_origin = results.relative_position_NED_origin.z;
     results.relative_position_D_origin_valid = results.relative_position_D_origin_valid;
+
+    // estimator limits on control:
+    results.ekfGndSpdLimit = 400.0;
+    results.controlScaleXY = 1;
 }
 
 bool AP_AHRS_External::get_relative_position_NED_origin(Vector3f &vec) const
@@ -80,11 +84,4 @@ void AP_AHRS_External::send_ekf_status_report(GCS_MAVLINK &link) const
     AP::externalAHRS().send_status_report(link);
 }
 
-void AP_AHRS_External::get_control_limits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
-{
-    // lower gains in VTOL controllers when flying on DCM
-    ekfGndSpdLimit = 50.0;
-    ekfNavVelGainScaler = 0.5;
-}
-
 #endif
diff --git a/libraries/AP_AHRS/AP_AHRS_External.h b/libraries/AP_AHRS/AP_AHRS_External.h
@@ -57,8 +57,6 @@ class AP_AHRS_External : public AP_AHRS_Backend {
     bool get_filter_status(nav_filter_status &status) const override;
     void send_ekf_status_report(class GCS_MAVLINK &link) const override;
 
-    void get_control_limits(float &ekfGndSpdLimit, float &controlScaleXY) const override;
-
 private:
 
     bool get_relative_position_NED_origin(Vector3f &vec) const;

diff --git 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;
 }
 
-void AP_AHRS_SIM::get_control_limits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
-{
-    // same as EKF2 for no optical flow
-    ekfGndSpdLimit = 400.0f;
-    ekfNavVelGainScaler = 1.0f;
-}
-
 bool AP_AHRS_SIM::get_mag_offsets(uint8_t mag_idx, Vector3f &magOffsets) const
 {
     magOffsets.zero();
@@ -224,6 +217,10 @@ void AP_AHRS_SIM::get_results(AP_AHRS_Backend::Estimates &results)
     results.wind = _sitl->state.wind_ef;
     results.wind_valid = true;
 
+    // estimator limits on control:
+    results.ekfGndSpdLimit = 400.0;
+    results.controlScaleXY = 1;
+
 #if HAL_NAVEKF3_AVAILABLE
     if (_sitl->odom_enable) {
         // use SITL states to write body frame odometry data at 20Hz

diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.h b/libraries/AP_AHRS/AP_AHRS_SIM.h
@@ -83,7 +83,6 @@ class AP_AHRS_SIM : public AP_AHRS_Backend {
 
     void send_ekf_status_report(class GCS_MAVLINK &link) const override;
 
-    void get_control_limits(float &ekfGndSpdLimit, float &controlScaleXY) const override;
     bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const override;
     bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;