From 3ad08b7ffc2ff4d3f02614e04a71f66a3a1dbb05 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 10 Aug 2012 10:02:32 -0700 Subject: [PATCH] ACM : Added larger WP radius for fast waypoints so we don't loose speed angling in on the WP at the last second. --- ArduCopter/ArduCopter.pde | 2 +- ArduCopter/Attitude.pde | 3 +++ ArduCopter/commands_logic.pde | 2 +- ArduCopter/navigation.pde | 2 ++ 4 files changed, 7 insertions(+), 2 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index a070d8ba65..cbc2d0d732 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -563,7 +563,7 @@ static int16_t waypoint_speed_gov; static int32_t long_error, lat_error; // Are we navigating while holding a positon? This is set to false once the speed drops below 1m/s static boolean loiter_override; - +static int16_t waypoint_radius; //////////////////////////////////////////////////////////////////////////////// // Orientation diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index bcb49d604b..2ce6553412 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -403,6 +403,9 @@ static void reset_nav_params(void) // make sure we stick to Nav yaw on takeoff auto_yaw = nav_yaw; + + // revert to smaller radius set in params + waypoint_radius = g.waypoint_radius; } /* diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 8c988c6160..b60c78b51e 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -452,7 +452,7 @@ static bool verify_nav_wp() } // Did we pass the WP? // Distance checking - if((wp_distance <= (g.waypoint_radius * 100)) || check_missed_wp()){ + if((wp_distance <= (waypoint_radius * 100)) || check_missed_wp()){ // if we have a distance calc error, wp_distance may be less than 0 if(wp_distance > 0){ diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index c5c32ca0d6..734a6f4081 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -260,8 +260,10 @@ static int16_t get_desired_speed(int16_t max_speed, bool _slow) */ if(fast_corner){ + waypoint_radius = g.waypoint_radius * 2; //max_speed = max_speed; }else{ + waypoint_radius = g.waypoint_radius; max_speed = min(max_speed, (wp_distance - g.waypoint_radius) / 3); max_speed = max(max_speed, WAYPOINT_SPEED_MIN); // go at least 100cm/s }