From 96ea35014323f31d95c4c9ed22b8b1344e61df1d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 31 Aug 2012 18:57:54 +1000 Subject: [PATCH] APM: added XTRK_MIN_DIST option this allows you to disable crosstrack correction on short legs --- ArduPlane/Parameters.h | 2 ++ ArduPlane/Parameters.pde | 11 ++++++++++- ArduPlane/navigation.pde | 3 ++- 3 files changed, 14 insertions(+), 2 deletions(-) diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 406875f731..9a5d8c69f9 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -73,6 +73,7 @@ public: k_param_reset_mission_chan, k_param_land_flare_alt, k_param_land_flare_sec, + k_param_crosstrack_min_distance, // 110: Telemetry control // @@ -237,6 +238,7 @@ public: AP_Float crosstrack_gain; AP_Int16 crosstrack_entry_angle; AP_Int8 crosstrack_use_wind; + AP_Int16 crosstrack_min_distance; // Estimation // diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index da96f274f8..ce352826c6 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -126,7 +126,16 @@ const AP_Param::Info var_info[] PROGMEM = { // @Description: If enabled, use wind estimation for navigation crosstrack when using a compass for yaw // @Values: 0:Disabled,1:Enabled // @User: Standard - GSCALAR(crosstrack_use_wind, "XTRK_USE_WIND", 1), + GSCALAR(crosstrack_use_wind, "XTRK_USE_WIND", 1), + + // @Param: XTRK_MIN_DIST + // @DisplayName: Crosstrack mininum distance + // @Description: Minimum distance in meters between waypoints to do crosstrack correction. + // @Units: Meters + // @Range: 0 32767 + // @Increment: 1 + // @User: Standard + GSCALAR(crosstrack_min_distance, "XTRK_MIN_DIST", 50), // @Param: ALT_MIX // @DisplayName: Gps to Baro Mix diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 3609b2a675..d039d5e8f9 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -189,7 +189,8 @@ static void update_crosstrack(void) // Crosstrack Error // ---------------- // If we are too far off or too close we don't do track following - if (abs(wrap_180_cd(target_bearing_cd - crosstrack_bearing_cd)) < 4500) { + if (wp_totalDistance >= g.crosstrack_min_distance && + abs(wrap_180_cd(target_bearing_cd - crosstrack_bearing_cd)) < 4500) { // Meters we are off track line crosstrack_error = sin(radians((target_bearing_cd - crosstrack_bearing_cd) * 0.01)) * wp_distance; nav_bearing_cd += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());