From 151e710668c9d5eb3765c41715931879a90d66c3 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 19 Feb 2012 13:13:21 -0800 Subject: [PATCH] Added slow wp option in calc_desired_speed --- ArduCopter/navigation.pde | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 2afa10bd34..ef534425fc 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -265,7 +265,7 @@ static void calc_loiter_pitch_roll() auto_pitch = -auto_pitch; } -static int16_t calc_desired_speed(int16_t max_speed) +static int16_t calc_desired_speed(int16_t max_speed, bool _slow) { /* |< WP Radius @@ -277,14 +277,13 @@ static int16_t calc_desired_speed(int16_t max_speed) */ // max_speed is default 600 or 6m/s - // (wp_distance * .5) = 1/2 of the distance converted to speed - // wp_distance is always in m/s and not cm/s - I know it's stupid that way - // for example 4m from target = 200cm/s speed - // we choose the lowest speed based on disance - max_speed = min(max_speed, wp_distance); - - // go at least 100cm/s - max_speed = max(max_speed, WAYPOINT_SPEED_MIN); + if(_slow){ + max_speed = min(max_speed, wp_distance / 2); + max_speed = max(max_speed, 0); + }else{ + max_speed = min(max_speed, wp_distance); + max_speed = max(max_speed, WAYPOINT_SPEED_MIN); // go at least 100cm/s + } // limit the ramp up of the speed // waypoint_speed_gov is reset to 0 at each new WP command