From da6989ba29f1520a04873e5df82fb937874094e3 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 21 Feb 2024 21:22:43 +0900
Subject: [PATCH] GCS_MAVLink: support align-vision-position-local-ned

---
 libraries/GCS_MAVLink/GCS.h          |  1 +
 libraries/GCS_MAVLink/GCS_Common.cpp | 20 ++++++++++++++++++++
 2 files changed, 21 insertions(+)

diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 8bb1108de3d189..a73479a6350a36 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -1064,6 +1064,7 @@ class GCS_MAVLINK
                                                      const uint8_t reset_counter,
                                                      const uint16_t payload_size);
     void handle_vision_speed_estimate(const mavlink_message_t &msg);
+    MAV_RESULT handle_visualodom_command(const mavlink_command_int_t &packet);
     void handle_landing_target(const mavlink_message_t &msg);
 
     void lock_channel(const mavlink_channel_t chan, bool lock);
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index ed467585171d74..72a5e81972d06a 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -3976,6 +3976,22 @@ void GCS_MAVLINK::handle_vision_speed_estimate(const mavlink_message_t &msg)
     uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.usec, PAYLOAD_SIZE(chan, VISION_SPEED_ESTIMATE));
     visual_odom->handle_vision_speed_estimate(m.usec, timestamp_ms, vel, m.reset_counter, 0);
 }
+
+MAV_RESULT GCS_MAVLINK::handle_visualodom_command(const mavlink_command_int_t &packet)
+{
+    // sanity check messages passed in (this should never fail)
+    if (packet.command != MAV_CMD_ALIGN_VISION_POSITION_LOCAL_NED) {
+        return MAV_RESULT_UNSUPPORTED;
+    }
+
+    AP_VisualOdom *visual_odom = AP::visualodom();
+    if (visual_odom == nullptr) {
+        return MAV_RESULT_FAILED;
+    }
+
+    visual_odom->align_position_and_yaw(Vector3f{packet.param1, packet.param2, packet.param3}, packet.y);
+    return MAV_RESULT_ACCEPTED;
+}
 #endif  // HAL_VISUALODOM_ENABLED
 
 void GCS_MAVLINK::handle_command_ack(const mavlink_message_t &msg)
@@ -5633,6 +5649,10 @@ 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 HAL_VISUALODOM_ENABLED
+    case MAV_CMD_ALIGN_VISION_POSITION_LOCAL_NED:
+        return handle_visualodom_command(packet);
+#endif
     }
 
     return MAV_RESULT_UNSUPPORTED;