diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 7eca35ee6e..e78dfdd747 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -20,6 +20,7 @@ #include #include #include +#include #include // check if a message will fit in the payload space available @@ -231,6 +232,7 @@ protected: virtual class AP_Camera *get_camera() const = 0; virtual AP_ServoRelayEvents *get_servorelayevents() const = 0; virtual AP_AdvancedFailsafe *get_advanced_failsafe() const { return nullptr; }; + virtual AP_VisualOdom *get_visual_odom() const { return nullptr; } virtual bool set_mode(uint8_t mode) = 0; virtual const AP_FWVersion &get_fwver() const = 0; virtual void set_ekf_origin(const Location& loc) = 0; @@ -273,6 +275,7 @@ protected: void handle_gimbal_report(AP_Mount &mount, mavlink_message_t *msg) const; void handle_radio_status(mavlink_message_t *msg, DataFlash_Class &dataflash, bool log_radio); void handle_serial_control(const mavlink_message_t *msg); + void handle_vision_position_delta(mavlink_message_t *msg); void handle_common_message(mavlink_message_t *msg); void handle_set_gps_global_origin(const mavlink_message_t *msg); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 8c8248b1ec..627e63ef5c 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1863,7 +1863,15 @@ void GCS_MAVLINK::handle_data_packet(mavlink_message_t *msg) #endif } +void GCS_MAVLINK::handle_vision_position_delta(mavlink_message_t *msg) +{ + AP_VisualOdom *visual_odom = get_visual_odom(); + if (visual_odom == nullptr) { + return; + } + visual_odom->handle_msg(msg); +} /* handle messages which don't require vehicle specific data @@ -1983,6 +1991,10 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg) case MAVLINK_MSG_ID_DATA96: handle_data_packet(msg); break; + + case MAVLINK_MSG_ID_VISION_POSITION_DELTA: + handle_vision_position_delta(msg); + break; } }