GCS_MAVLink: Add EAHRS commands handling

ArduPilot · Dec 15, 2024 · 3f21e61 · 3f21e61
1 parent 69f7eb2
commit 3f21e61
Show file tree

Hide file tree

Showing 2 changed files with 43 additions and 0 deletions.
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
@@ -632,6 +632,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
@@ -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_send_disable(false);
+            return MAV_RESULT_ACCEPTED;
+        }
+        case MAV_CMD_AHRS_STOP:
+        {
+            AP::ahrs().set_data_send_disable(true);
+            return MAV_RESULT_ACCEPTED;
+        }
+
+        case MAV_CMD_AHRS_ENABLE_GNSS:
+        {
+            AP::ahrs().set_gnss_disable(false);
+            return MAV_RESULT_ACCEPTED;
+        }
+
+        case MAV_CMD_AHRS_DISABLE_GNSS:
+        {
+            AP::ahrs().set_gnss_disable(true);
+            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;