Rover: reduce throttle more directly

this gives better throttle control in turns and waypoints

APMrover2/APMrover2.pde

@@ -348,7 +348,7 @@ static int32_t target_bearing;
 // deg * 100 : 0 to 360
 static int32_t crosstrack_bearing;
 // A gain scaler to account for ground speed/headwind/tailwind
-static float	nav_gain_scaler 		= 1;		
+static float	nav_gain_scaler 		= 1.0f;		
 static bool rtl_complete = false;
 
 // There may be two active commands in Auto mode.  

APMrover2/Steering.pde

@@ -17,38 +17,47 @@ static void throttle_slew_limit(int16_t last_throttle)
     }
 }
 
+/*
+  calculate the throtte for auto-throttle modes
+ */
 static void calc_throttle(float target_speed)
 {  
-   if (target_speed <= 0) {
-       // cope with zero requested speed
-       g.channel_throttle.servo_out = g.throttle_min.get();
-       return;
-   }
+    if (target_speed <= 0) {
+        // cope with zero requested speed
+        g.channel_throttle.servo_out = g.throttle_min.get();
+        return;
+    }
 
-   int throttle_target = g.throttle_cruise + throttle_nudge;  
+    int throttle_target = g.throttle_cruise + throttle_nudge;  
 
-   /*
-     reduce target speed in proportion to turning rate, up to the
-     SPEED_TURN_GAIN percentage.
+    /*
+      reduce target speed in proportion to turning rate, up to the
+      SPEED_TURN_GAIN percentage.
     */
-   float steer_rate = fabsf(nav_steer / (float)SERVO_MAX);
-   steer_rate = constrain(steer_rate, 0.0, 1.0);
-   float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01;
-
-   if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
-       // in auto-modes we reduce speed when approaching waypoints
-       float reduction2 = 1.0 - (100-g.speed_turn_gain)*0.01*((g.speed_turn_dist - wp_distance)/g.speed_turn_dist);
-       if (reduction2 < reduction) {
-           reduction = reduction2;
-       }
-   }
-
-   target_speed *= reduction;
-   
-   groundspeed_error = target_speed - ground_speed; 
-        
-   throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
-   g.channel_throttle.servo_out = constrain_int16(throttle, g.throttle_min.get(), g.throttle_max.get());
+    float steer_rate = fabsf((nav_steer/nav_gain_scaler) / (float)SERVO_MAX);
+    steer_rate = constrain(steer_rate, 0.0, 1.0);
+    float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01;
+    
+    if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
+        // in auto-modes we reduce speed when approaching waypoints
+        float reduction2 = 1.0 - (100-g.speed_turn_gain)*0.01*((g.speed_turn_dist - wp_distance)/g.speed_turn_dist);
+        if (reduction2 < reduction) {
+            reduction = reduction2;
+        }
+    }
+    
+    // reduce the target speed by the reduction factor
+    target_speed *= reduction;
+
+    groundspeed_error = target_speed - ground_speed; 
+    
+    throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
+
+    // also reduce the throttle by the reduction factor. This gives a
+    // much faster response in turns
+    throttle *= reduction;
+
+    g.channel_throttle.servo_out = constrain_int16(throttle, g.throttle_min.get(), g.throttle_max.get());
 }
 
 /*****************************************