GCS_MAVLink: Add EAHRS commands handling

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();
 

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;