GCS_MAVLink: support align-vision-position-local-ned

rmackay9 · Nov 20, 2024 · da6989b · da6989b
1 parent 2dcd7e2
commit da6989b
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 0 deletions.
diff --git 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
@@ -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;