Rover: much simpler SPEED_TURN_GAIN implementation

as soon as we hit the SPEED_TURN_DIST we lower target speed to the
specified gain
This commit is contained in:
Andrew Tridgell 2014-05-18 21:15:07 +10:00
parent 69b5f352f6
commit fc0d703b9b

View File

@ -109,7 +109,7 @@ static void calc_throttle(float target_speed)
if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
// in auto-modes we reduce speed when approaching waypoints
float reduction2 = 1.0 - speed_turn_reduction*((g.speed_turn_dist - wp_distance)/g.speed_turn_dist);
float reduction2 = 1.0 - speed_turn_reduction;
if (reduction2 < reduction) {
reduction = reduction2;
}