AP_AHRS: Add GNSS and data sending disable/enable

ArduPilot · Dec 26, 2024 · eed9c17 · eed9c17
1 parent 8c16b92
commit eed9c17
Show file tree

Hide file tree

Showing 5 changed files with 88 additions and 0 deletions.
diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -3629,6 +3629,50 @@ float AP_AHRS::get_air_density_ratio(void) const
     return 1.0 / sq(eas2tas);
 }
 
+void AP_AHRS::set_data_sending_state(DATA_SENDING_STATE data_sending_state)
+{
+    switch (active_EKF_type()) {
+#if AP_AHRS_EXTERNAL_ENABLED
+    case EKFType::EXTERNAL:
+        return external.set_data_sending_state(data_sending_state == DATA_SENDING_STATE::DISABLED ?
+                                               AP_AHRS_Backend::DATA_SENDING_STATE::DISABLED :
+                                               AP_AHRS_Backend::DATA_SENDING_STATE::ENABLED);
+#endif
+    default:
+        break;
+    }
+}
+
+void AP_AHRS::set_gps_state(GPS_STATE gps_state)
+{
+    switch (active_EKF_type()) {
+#if HAL_NAVEKF2_AVAILABLE
+    case EKFType::TWO:
+#if AP_GPS_ENABLED
+        AP::gps().force_disable(gps_state == GPS_STATE::DISABLED);
+#endif
+        return;
+#endif //HAL_NAVEKF2_AVAILABLE
+
+#if HAL_NAVEKF3_AVAILABLE
+    case EKFType::THREE:
+#if AP_GPS_ENABLED
+        AP::gps().force_disable(gps_state == GPS_STATE::DISABLED);
+#endif
+        return;
+#endif //HAL_NAVEKF3_AVAILABLE
+
+#if AP_AHRS_EXTERNAL_ENABLED
+    case EKFType::EXTERNAL:
+        return external.set_gps_state(gps_state == GPS_STATE::DISABLED ?
+                                      AP_AHRS_Backend::GPS_STATE::DISABLED :
+                                      AP_AHRS_Backend::GPS_STATE::ENABLED);
+#endif
+    default:
+        break;
+    }
+}
+
 // singleton instance
 AP_AHRS *AP_AHRS::_singleton;
 

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
@@ -683,6 +683,19 @@ class AP_AHRS {
     // get access to an EKFGSF_yaw estimator
     const EKFGSF_yaw *get_yaw_estimator(void) const;
 
+    enum class DATA_SENDING_STATE {
+        ENABLED,
+        DISABLED
+    };
+    // enable/disable sending data to autopilot. Can be used to simulate AHRS failure.
+    void set_data_sending_state(DATA_SENDING_STATE data_sending_state);
+
+    enum class GPS_STATE {
+        ENABLED,
+        DISABLED
+    };
+    void set_gps_state(GPS_STATE gps_state);
+
 private:
 
     // roll/pitch/yaw euler angles, all in radians

diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h
@@ -329,4 +329,17 @@ class AP_AHRS_Backend
     virtual void set_terrain_hgt_stable(bool stable) {}
 
     virtual void get_control_limits(float &ekfGndSpdLimit, float &controlScaleXY) const = 0;
+
+    enum class DATA_SENDING_STATE {
+        ENABLED,
+        DISABLED
+    };
+    // enable/disable sending data to autopilot. Can be used to simulate AHRS failure.
+    virtual void set_data_sending_state(DATA_SENDING_STATE state) {};
+
+    enum class GPS_STATE {
+        ENABLED,
+        DISABLED
+    };
+    virtual void set_gps_state(GPS_STATE state) {};
 };
diff --git a/libraries/AP_AHRS/AP_AHRS_External.cpp b/libraries/AP_AHRS/AP_AHRS_External.cpp
@@ -137,4 +137,18 @@ void AP_AHRS_External::get_control_limits(float &ekfGndSpdLimit, float &ekfNavVe
     ekfNavVelGainScaler = 0.5;
 }
 
+void AP_AHRS_External::set_data_sending_state(DATA_SENDING_STATE state)
+{
+    AP::externalAHRS().set_data_sending_state(state == DATA_SENDING_STATE::ENABLED ?
+                                              AP_ExternalAHRS::DATA_SENDING_STATE::ENABLED :
+                                              AP_ExternalAHRS::DATA_SENDING_STATE::DISABLED);
+}
+
+void AP_AHRS_External::set_gps_state(GPS_STATE state)
+{
+    AP::externalAHRS().set_gps_state(state == GPS_STATE::ENABLED ?
+                                     AP_ExternalAHRS::GPS_STATE::ENABLED :
+                                     AP_ExternalAHRS::GPS_STATE::DISABLED);
+}
+
 #endif
diff --git a/libraries/AP_AHRS/AP_AHRS_External.h b/libraries/AP_AHRS/AP_AHRS_External.h
@@ -87,6 +87,10 @@ class AP_AHRS_External : 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;
+
+    // enable/disable sending data to autopilot. Can be used to simulate AHRS failure.
+    void set_data_sending_state(DATA_SENDING_STATE state) override;
+    void set_gps_state(GPS_STATE state) override;
 };
 
 #endif