From 4b6db7c0dda76dd97ebe13527bd266fa82340001 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 8 Dec 2018 13:18:41 +0900 Subject: [PATCH] Copter: remove follow mode debug --- ArduCopter/mode_follow.cpp | 23 ++--------------------- 1 file changed, 2 insertions(+), 21 deletions(-) diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index 86e65b9397..782b4369af 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -13,25 +13,14 @@ * TODO: ensure AC_AVOID_ENABLED is true because we rely on it velocity limiting functions */ -#if 1 -#define Debug(fmt, args ...) do { \ - gcs().send_text(MAV_SEVERITY_WARNING, fmt, ## args); \ - } while(0) -#elif 0 -#define Debug(fmt, args ...) do { \ - ::fprintf(stderr, "%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); -#else -#define Debug(fmt, args ...) -#endif - // initialise follow mode bool Copter::ModeFollow::init(const bool ignore_checks) { - // re-use guided mode - gcs().send_text(MAV_SEVERITY_WARNING, "Follow-mode init"); if (!g2.follow.enabled()) { + gcs().send_text(MAV_SEVERITY_WARNING, "Set FOLL_ENABLE = 1"); return false; } + // re-use guided mode return Copter::ModeGuided::init(ignore_checks); } @@ -56,14 +45,6 @@ void Copter::ModeFollow::run() Vector3f dist_vec_offs; // vector to lead vehicle + offset Vector3f vel_of_target; // velocity of lead vehicle if (g2.follow.get_target_dist_and_vel_ned(dist_vec, dist_vec_offs, vel_of_target)) { - // debug - static uint16_t counter = 0; - counter++; - if (counter >= 400) { - counter = 0; - Debug("dist to veh: %f %f %f", (double)dist_vec.x, (double)dist_vec.y, (double)dist_vec.z); - } - // convert dist_vec_offs to cm in NEU const Vector3f dist_vec_offs_neu(dist_vec_offs.x * 100.0f, dist_vec_offs.y * 100.0f, -dist_vec_offs.z * 100.0f);