From a28d8362f04cdd828b8b9ce0f0b70ca013d947e2 Mon Sep 17 00:00:00 2001
From: Valentin Bugrov <valentin.bugrov@inertiallabs.com>
Date: Sun, 15 Dec 2024 18:00:03 -0500
Subject: [PATCH] AP_AHRS: Add GNSS and data sending disable/enable

---
 libraries/AP_AHRS/AP_AHRS.cpp          | 24 ++++++++++++++++++++++++
 libraries/AP_AHRS/AP_AHRS.h            |  4 ++++
 libraries/AP_AHRS/AP_AHRS_Backend.h    |  3 +++
 libraries/AP_AHRS/AP_AHRS_External.cpp | 10 ++++++++++
 libraries/AP_AHRS/AP_AHRS_External.h   |  3 +++
 5 files changed, 44 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
index e8fa5cd623a61c..c1b6d47b18e3d0 100644
--- 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
index 04ce489dad7e39..4aab96d0482ace 100644
--- 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
index 99ad8f2dc82c96..03b609c0e510b5 100644
--- 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
index 435bda69b471a9..984360244b9ec8 100644
--- 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
index 6ca69f73f8f4ac..304b49655abcd5 100644
--- 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