diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 7f33b270a7..8cfbe14ea0 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -724,11 +724,9 @@ private: // fence.cpp void fence_check(); - void fence_send_mavlink_status(mavlink_channel_t chan); // GCS_Mavlink.cpp void gcs_send_heartbeat(void); - void send_fence_status(mavlink_channel_t chan); void send_sys_status(mavlink_channel_t chan); void send_nav_controller_output(mavlink_channel_t chan); void send_rpm(mavlink_channel_t chan); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index e3f58d4270..9c605e26f8 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -118,14 +118,6 @@ void GCS_MAVLINK_Copter::send_position_target_global_int() 0.0f); // yaw_rate } -#if AC_FENCE == ENABLED -NOINLINE void Copter::send_fence_status(mavlink_channel_t chan) -{ - fence_send_mavlink_status(chan); -} -#endif - - NOINLINE void Copter::send_sys_status(mavlink_channel_t chan) { int16_t battery_current = -1; @@ -306,13 +298,6 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id) #endif break; - case MSG_FENCE_STATUS: -#if AC_FENCE == ENABLED - CHECK_PAYLOAD_SIZE(FENCE_STATUS); - copter.send_fence_status(chan); -#endif - break; - case MSG_WIND: case MSG_SERVO_OUT: case MSG_AOA_SSA: diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 432313dccb..7a1d460a52 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -78,27 +78,4 @@ void Copter::fence_check() } } -// fence_send_mavlink_status - send fence status to ground station -void Copter::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 | AC_FENCE_TYPE_POLYGON)) != 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