diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index 66134963a2..6240f32552 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -23,7 +23,6 @@ extern const AP_HAL::HAL& hal; #define AP_FOLLOW_TIMEOUT_MS 3000 // position estimate timeout after 1 second #define AP_FOLLOW_SYSID_TIMEOUT_MS 10000 // forget sysid we are following if we haave not heard from them in 10 seconds -#define AP_GCS_INTERVAL_MS 1000 // interval between updating GCS on position of vehicle #define AP_FOLLOW_OFFSET_TYPE_NED 0 // offsets are in north-east-down frame #define AP_FOLLOW_OFFSET_TYPE_RELATIVE 1 // offsets are relative to lead vehicle's heading @@ -293,14 +292,6 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg) _sysid.set(msg.sysid); _automatic_sysid = true; } - if ((now - _last_location_sent_to_gcs) > AP_GCS_INTERVAL_MS) { - _last_location_sent_to_gcs = now; - gcs().send_text(MAV_SEVERITY_INFO, "Foll: %u %ld %ld %4.2f\n", - (unsigned)_sysid.get(), - (long)_target_location.lat, - (long)_target_location.lng, - (double)(_target_location.alt * 0.01f)); // cm to m - } // log lead's estimated vs reported position DataFlash_Class::instance()->Log_Write("FOLL", diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h index 145fce44f3..4612a63767 100644 --- a/libraries/AP_Follow/AP_Follow.h +++ b/libraries/AP_Follow/AP_Follow.h @@ -108,7 +108,5 @@ private: Vector3f _target_accel_ned; // last known acceleration of target in NED frame in m/s/s uint32_t _last_heading_update_ms; // system time of last heading update float _target_heading; // heading in degrees - uint32_t _last_location_sent_to_gcs; // last time GCS was told position - bool _automatic_sysid; // did we lock onto a sysid automatically? };