From 3d32630be31a1845fcdbbc27901907bf059b8451 Mon Sep 17 00:00:00 2001
From: Valentin Bugrov <valentin.bugrov@inertiallabs.com>
Date: Tue, 19 Nov 2024 18:01:25 -0500
Subject: [PATCH] GCS_MAVLink: Add EAHRS commands handling

---
 libraries/GCS_MAVLink/GCS.h          |  3 +++
 libraries/GCS_MAVLink/GCS_Common.cpp | 33 ++++++++++++++++++++++++++++
 2 files changed, 36 insertions(+)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index aa67f61e8cedd..cd98872c1543d 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -631,6 +631,9 @@ class GCS_MAVLINK
 #if AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED
     MAV_RESULT handle_command_request_autopilot_capabilities(const mavlink_command_int_t &packet);
 #endif
+#if AP_AHRS_ENABLED
+    MAV_RESULT handle_AHRS_message(const mavlink_command_int_t &packet);
+#endif
 
     virtual void send_banner();
 
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 8fb6bc3f8f40b..9e1d386f1416b 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -4878,6 +4878,32 @@ MAV_RESULT GCS_MAVLINK::handle_command_request_autopilot_capabilities(const mavl
 }
 #endif
 
+#if AP_AHRS_ENABLED
+MAV_RESULT GCS_MAVLINK::handle_AHRS_message(const mavlink_command_int_t &packet)
+{
+    switch (packet.command) {
+        case MAV_CMD_AHRS_START:
+            AP::ahrs().set_data_sending_state(AP_AHRS::DATA_SENDING_STATE::ENABLED);
+            return MAV_RESULT_ACCEPTED;
+
+        case MAV_CMD_AHRS_STOP:
+            AP::ahrs().set_data_sending_state(AP_AHRS::DATA_SENDING_STATE::DISABLED);
+            return MAV_RESULT_ACCEPTED;
+
+        case MAV_CMD_AHRS_GPS_ENABLE:
+            AP::ahrs().set_gps_state(AP_AHRS::GPS_STATE::ENABLED);
+            return MAV_RESULT_ACCEPTED;
+
+        case MAV_CMD_AHRS_GPS_DISABLE:
+            AP::ahrs().set_gps_state(AP_AHRS::GPS_STATE::DISABLED);
+            return MAV_RESULT_ACCEPTED;
+
+        default:
+            return MAV_RESULT_FAILED;
+    }
+}
+#endif
+
 MAV_RESULT GCS_MAVLINK::handle_command_do_set_mode(const mavlink_command_int_t &packet)
 {
     const MAV_MODE _base_mode = (MAV_MODE)packet.param1;
@@ -5638,6 +5664,13 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p
     case MAV_CMD_REQUEST_MESSAGE:
         return handle_command_request_message(packet);
 
+#if AP_AHRS_ENABLED
+    case MAV_CMD_AHRS_START:
+    case MAV_CMD_AHRS_STOP:
+    case MAV_CMD_AHRS_GPS_ENABLE:
+    case MAV_CMD_AHRS_GPS_DISABLE:
+        return handle_AHRS_message(packet);
+#endif
     }
 
     return MAV_RESULT_UNSUPPORTED;