diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 0ad41ec32b..671edf88d9 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -338,6 +338,7 @@ protected: void handle_rally_fetch_point(mavlink_message_t *msg); void handle_rally_point(mavlink_message_t *msg); virtual void handle_mount_message(const mavlink_message_t *msg); + void handle_fence_message(mavlink_message_t *msg); void handle_param_value(mavlink_message_t *msg); void handle_radio_status(mavlink_message_t *msg, AP_Logger &dataflash, bool log_radio); void handle_serial_control(const mavlink_message_t *msg); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 5db8ec820f..acf9c83180 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3023,6 +3023,11 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg) handle_command_int(msg); break; + case MAVLINK_MSG_ID_FENCE_POINT: + case MAVLINK_MSG_ID_FENCE_FETCH_POINT: + handle_fence_message(msg); + break; + case MAVLINK_MSG_ID_GIMBAL_REPORT: handle_mount_message(msg); break; diff --git a/libraries/GCS_MAVLink/GCS_Fence.cpp b/libraries/GCS_MAVLink/GCS_Fence.cpp index b4f4a60d67..22849dd61e 100644 --- a/libraries/GCS_MAVLink/GCS_Fence.cpp +++ b/libraries/GCS_MAVLink/GCS_Fence.cpp @@ -21,6 +21,26 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_fence_enable(const mavlink_command_lon } } +void GCS_MAVLINK::handle_fence_message(mavlink_message_t *msg) +{ + AC_Fence *fence = AP::fence(); + if (fence == nullptr) { + return; + } + // send or receive fence points with GCS + switch (msg->msgid) { + case MAVLINK_MSG_ID_FENCE_POINT: + case MAVLINK_MSG_ID_FENCE_FETCH_POINT: + fence->handle_msg(*this, msg); + break; + default: +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_HAL::panic("Unhandled common fence message"); +#endif + break; + } +} + // fence_send_mavlink_status - send fence status to ground station void GCS_MAVLINK::send_fence_status() const {