AP_AHRS: Add GNSS and data sending disable/enable

ArduPilot · Dec 17, 2024 · a28d836 · a28d836
1 parent af068b3
commit a28d836
Show file tree

Hide file tree

Showing 5 changed files with 44 additions and 0 deletions.
diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -3629,6 +3629,30 @@ float AP_AHRS::get_air_density_ratio(void) const
     return 1.0 / sq(eas2tas);
 }
 
+void AP_AHRS::set_data_sending_state(bool enabled)
+{
+    switch (active_EKF_type()) {
+#if AP_AHRS_EXTERNAL_ENABLED
+    case EKFType::EXTERNAL:
+        return external.set_data_sending_state(enabled);
+#endif
+    default:
+        break;
+    }
+}
+
+void AP_AHRS::set_gnss_state(bool enabled)
+{
+    switch (active_EKF_type()) {
+#if AP_AHRS_EXTERNAL_ENABLED
+    case EKFType::EXTERNAL:
+        return external.set_gnss_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,10 @@ class AP_AHRS {
     // get access to an EKFGSF_yaw estimator
     const EKFGSF_yaw *get_yaw_estimator(void) const;
 
+    // enable/disable sending data to autopilot
+    void set_data_sending_state(bool enabled);
+    void set_gnss_state(bool enabled);
+
 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,7 @@ class AP_AHRS_Backend
     virtual void set_terrain_hgt_stable(bool stable) {}
 
     virtual void get_control_limits(float &ekfGndSpdLimit, float &controlScaleXY) const = 0;
+
+    virtual void set_data_sending_state(bool enabled) {};
+    virtual void set_gnss_state(bool enabled) {};
 };
diff --git a/libraries/AP_AHRS/AP_AHRS_External.cpp b/libraries/AP_AHRS/AP_AHRS_External.cpp
@@ -137,4 +137,14 @@ void AP_AHRS_External::get_control_limits(float &ekfGndSpdLimit, float &ekfNavVe
     ekfNavVelGainScaler = 0.5;
 }
 
+void AP_AHRS_External::set_data_sending_state(bool enabled)
+{
+    AP::externalAHRS().set_data_sending_state(enabled);
+}
+
+void AP_AHRS_External::set_gnss_state(bool enabled)
+{
+    AP::externalAHRS().set_gnss_state(enabled);
+}
+
 #endif
diff --git a/libraries/AP_AHRS/AP_AHRS_External.h b/libraries/AP_AHRS/AP_AHRS_External.h
@@ -87,6 +87,9 @@ 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;
+
+    void set_data_sending_state(bool enabled) override;
+    void set_gnss_state(bool enabled) override;
 };
 
 #endif