diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 59888ae05e..3b615235fc 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -274,11 +274,6 @@ void Rover::send_pid_tuning(mavlink_channel_t chan) } } -void Rover::send_fence_status(mavlink_channel_t chan) -{ - fence_send_mavlink_status(chan); -} - void Rover::send_wheel_encoder(mavlink_channel_t chan) { // send wheel encoder data using rpm message @@ -363,11 +358,6 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id) rover.send_wheel_encoder_distance(chan); break; - case MSG_FENCE_STATUS: - CHECK_PAYLOAD_SIZE(FENCE_STATUS); - rover.send_fence_status(chan); - break; - case MSG_WIND: CHECK_PAYLOAD_SIZE(WIND); rover.g2.windvane.send_wind(chan); diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 8520330b7a..98f6a43e24 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -436,7 +436,6 @@ private: // fence.cpp void fence_check(); - void fence_send_mavlink_status(mavlink_channel_t chan); // GCS_Mavlink.cpp void send_sys_status(mavlink_channel_t chan); @@ -445,7 +444,6 @@ private: void send_pid_tuning(mavlink_channel_t chan); void send_wheel_encoder(mavlink_channel_t chan); void send_wheel_encoder_distance(mavlink_channel_t chan); - void send_fence_status(mavlink_channel_t chan); // Log.cpp void Log_Write_Arm_Disarm(); diff --git a/APMrover2/fence.cpp b/APMrover2/fence.cpp index 74709a690f..a3ee9c96c4 100644 --- a/APMrover2/fence.cpp +++ b/APMrover2/fence.cpp @@ -36,23 +36,3 @@ void Rover::fence_check() Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, ERROR_CODE_ERROR_RESOLVED); } } - -// fence_send_mavlink_status - send fence status to ground station -void Rover::fence_send_mavlink_status(mavlink_channel_t chan) -{ - if (g2.fence.enabled()) { - // traslate fence library breach types to mavlink breach types - uint8_t mavlink_breach_type = FENCE_BREACH_NONE; - const uint8_t breaches = g2.fence.get_breaches(); - if ((breaches & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) != 0) { - mavlink_breach_type = FENCE_BREACH_BOUNDARY; - } - - // send status - mavlink_msg_fence_status_send(chan, - static_cast(g2.fence.get_breaches() != 0), - g2.fence.get_breach_count(), - mavlink_breach_type, - g2.fence.get_breach_time()); - } -}