GCS_MAVLink: support MAV_CMD_SET_EKF_SOURCE_SET command

this allows external systems to set the active EKF source set
ArduPilot · Aug 16, 2021 · 611a8a1 · 611a8a1
1 parent d26158d
commit 611a8a1
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 0 deletions.
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
@@ -506,6 +506,7 @@ class GCS_MAVLINK
     MAV_RESULT handle_command_get_home_position(const mavlink_command_long_t &packet);
     MAV_RESULT handle_command_do_fence_enable(const mavlink_command_long_t &packet);
     MAV_RESULT handle_command_debug_trap(const mavlink_command_long_t &packet);
+    MAV_RESULT handle_command_set_ekf_source_set(const mavlink_command_long_t &packet);
 
     void handle_optical_flow(const mavlink_message_t &msg);
 

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -3973,6 +3973,18 @@ MAV_RESULT GCS_MAVLINK::handle_command_debug_trap(const mavlink_command_long_t &
     return MAV_RESULT_UNSUPPORTED;
 }
 
+MAV_RESULT GCS_MAVLINK::handle_command_set_ekf_source_set(const mavlink_command_long_t &packet)
+{
+    // source set must be between 1 and 3
+    uint32_t source_set = uint32_t(packet.param1);
+    if ((source_set >= 1) && (source_set <= 3)) {
+        // mavlink command uses range 1 to 3 while ahrs interface accepts 0 to 2
+        AP::ahrs().set_posvelyaw_source_set(source_set-1);
+        return MAV_RESULT_ACCEPTED;
+    }
+    return MAV_RESULT_DENIED;
+}
+
 MAV_RESULT GCS_MAVLINK::handle_command_do_gripper(const mavlink_command_long_t &packet)
 {
     AP_Gripper *gripper = AP::gripper();
@@ -4209,6 +4221,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
         result = handle_command_debug_trap(packet);
         break;
 
+    case MAV_CMD_SET_EKF_SOURCE_SET:
+        result = handle_command_set_ekf_source_set(packet);
+        break;
+
     case MAV_CMD_PREFLIGHT_STORAGE:
         if (is_equal(packet.param1, 2.0f)) {
             AP_Param::erase_all();