From 404df7fe7d9ef5aa8f016cbee8cc0521ca7e9e17 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 27 Apr 2015 11:24:47 +0900 Subject: [PATCH] Tracker: add DISTANCE_MIN parameter Vehicles will only be tracked if they are at least DISTANCE_MIN meters away from the tracker --- AntennaTracker/Parameters.h | 4 +++- AntennaTracker/Parameters.pde | 9 +++++++++ AntennaTracker/config.h | 3 +++ AntennaTracker/control_auto.pde | 8 ++++++-- 4 files changed, 21 insertions(+), 3 deletions(-) diff --git a/AntennaTracker/Parameters.h b/AntennaTracker/Parameters.h index 42002cb3ff..461edd4e2b 100644 --- a/AntennaTracker/Parameters.h +++ b/AntennaTracker/Parameters.h @@ -89,7 +89,8 @@ public: k_param_yaw_trim, k_param_pitch_trim, k_param_yaw_range, - k_param_pitch_range, // 136 + k_param_pitch_range, + k_param_distance_min, // 137 // // 150: Telemetry control @@ -138,6 +139,7 @@ public: AP_Float pitch_trim; AP_Int16 yaw_range; // yaw axis total range of motion in degrees AP_Int16 pitch_range; // pitch axis total range of motion in degrees + AP_Int16 distance_min; // target's must be at least this distance from tracker to be tracked // Waypoints // diff --git a/AntennaTracker/Parameters.pde b/AntennaTracker/Parameters.pde index d617cf1d70..cbff17d635 100644 --- a/AntennaTracker/Parameters.pde +++ b/AntennaTracker/Parameters.pde @@ -178,6 +178,15 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(pitch_range, "PITCH_RANGE", PITCH_RANGE_DEFAULT), + // @Param: DISTANCE_MIN + // @DisplayName: Distance minimum to target + // @Description: Tracker will track targets at least this distance away + // @Units: meters + // @Increment: 1 + // @Range: 0 100 + // @User: Standard + GSCALAR(distance_min, "DISTANCE_MIN", DISTANCE_MIN_DEFAULT), + // barometer ground calibration. The GND_ prefix is chosen for // compatibility with previous releases of ArduPlane // @Group: GND_ diff --git a/AntennaTracker/config.h b/AntennaTracker/config.h index 933c40d5b6..c00034acdc 100644 --- a/AntennaTracker/config.h +++ b/AntennaTracker/config.h @@ -53,6 +53,9 @@ #ifndef TRACKING_TIMEOUT_SEC # define TRACKING_TIMEOUT_SEC 5.0f // consider we've lost track of vehicle after 5 seconds with no position update. #endif +#ifndef DISTANCE_MIN_DEFAULT + # define DISTANCE_MIN_DEFAULT 5.0f // do not track targets within 5 meters +#endif ////////////////////////////////////////////////////////////////////////////// // Developer Items diff --git a/AntennaTracker/control_auto.pde b/AntennaTracker/control_auto.pde index 19b1a1d95a..7905692585 100644 --- a/AntennaTracker/control_auto.pde +++ b/AntennaTracker/control_auto.pde @@ -17,6 +17,10 @@ static void update_auto(void) float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100) * 0.01f; float pitch = constrain_float(nav_status.pitch+g.pitch_trim, -90, 90); - update_pitch_servo(pitch); - update_yaw_servo(yaw); + + // only move servos if target is at least distance_min away + if ((g.distance_min <= 0) || (nav_status.distance >= g.distance_min)) { + update_pitch_servo(pitch); + update_yaw_servo(yaw); + } }