From 81ff249759aacd779721c23a2d4e3277f42ad935 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 | 40 ++++++++++++++++++++++++++++
 2 files changed, 43 insertions(+)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index aa67f61e8ceddc..cd98872c1543d9 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 91dfd299c3142d..3cfafcda0b5223 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -4878,6 +4878,39 @@ 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(/*enabled*/ true);
+            return MAV_RESULT_ACCEPTED;
+        }
+        case MAV_CMD_AHRS_STOP:
+        {
+            AP::ahrs().set_data_sending_state(/*enabled*/ false);
+            return MAV_RESULT_ACCEPTED;
+        }
+
+        case MAV_CMD_AHRS_ENABLE_GNSS:
+        {
+            AP::ahrs().set_gnss_state(/*enabled*/ true);
+            return MAV_RESULT_ACCEPTED;
+        }
+
+        case MAV_CMD_AHRS_DISABLE_GNSS:
+        {
+            AP::ahrs().set_gnss_state(/*enabled*/ false);
+            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 +5671,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_ENABLE_GNSS:
+    case MAV_CMD_AHRS_DISABLE_GNSS:
+        return handle_AHRS_message(packet);
+#endif
     }
 
     return MAV_RESULT_UNSUPPORTED;