From fc0b50dd82cd0aed6afd6d20d253a0e3cfc770c0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 2 Mar 2013 11:03:15 +1100
Subject: [PATCH] Rover: reduce throttle more directly

this gives better throttle control in turns and waypoints
---
 APMrover2/APMrover2.pde |  2 +-
 APMrover2/Steering.pde  | 59 ++++++++++++++++++++++++-----------------
 2 files changed, 35 insertions(+), 26 deletions(-)

diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index 0be4edaefc..4788d33694 100644
--- a/APMrover2/APMrover2.pde
+++ b/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.  
diff --git a/APMrover2/Steering.pde b/APMrover2/Steering.pde
index dfa8c4a412..ffbd21e31f 100644
--- a/APMrover2/Steering.pde
+++ b/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;
+    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;
 
-   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;
-       }
-   }
+    groundspeed_error = target_speed - ground_speed; 
+    
+    throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
 
-   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());
+    // 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());
 }
 
 /*****************************************