diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index e40379a48c..9422210eac 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -428,13 +428,6 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id) #endif break; - case MSG_FENCE_STATUS: -#if AC_FENCE == ENABLED - CHECK_PAYLOAD_SIZE(FENCE_STATUS); - sub.fence_send_mavlink_status(chan); -#endif - break; - case MSG_PID_TUNING: CHECK_PAYLOAD_SIZE(PID_TUNING); sub.send_pid_tuning(chan); diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index b0ad0c113f..04a582f2ab 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -579,7 +579,6 @@ private: void mainloop_failsafe_enable(); void mainloop_failsafe_disable(); void fence_check(); - void fence_send_mavlink_status(mavlink_channel_t chan); bool set_mode(control_mode_t mode, mode_reason_t reason); void update_flight_mode(); void exit_mode(control_mode_t old_control_mode, control_mode_t new_control_mode); diff --git a/ArduSub/fence.cpp b/ArduSub/fence.cpp index 66ed66d959..7dcf747625 100644 --- a/ArduSub/fence.cpp +++ b/ArduSub/fence.cpp @@ -49,27 +49,4 @@ void Sub::fence_check() } } -// fence_send_mavlink_status - send fence status to ground station -void Sub::fence_send_mavlink_status(mavlink_channel_t chan) -{ - if (fence.enabled()) { - // traslate fence library breach types to mavlink breach types - uint8_t mavlink_breach_type = FENCE_BREACH_NONE; - uint8_t breaches = fence.get_breaches(); - if ((breaches & AC_FENCE_TYPE_ALT_MAX) != 0) { - mavlink_breach_type = FENCE_BREACH_MAXALT; - } - if ((breaches & AC_FENCE_TYPE_CIRCLE) != 0) { - mavlink_breach_type = FENCE_BREACH_BOUNDARY; - } - - // send status - mavlink_msg_fence_status_send(chan, - (int8_t)(fence.get_breaches()!=0), - fence.get_breach_count(), - mavlink_breach_type, - fence.get_breach_time()); - } -} - #endif