From f4f04ead68b0ed0426ce46586ec57f6f59fb4d01 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 31 Mar 2014 08:44:19 +1100
Subject: [PATCH] Rover: added BRAKING_PERCENT parameter

this allows for reverse throttle to be applied for braking in corners
---
 APMrover2/Parameters.h   |  6 ++++++
 APMrover2/Parameters.pde |  9 +++++++++
 APMrover2/Steering.pde   | 13 +++++++++++++
 3 files changed, 28 insertions(+)

diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
index 1f5a1b3736..f3a930eda6 100644
--- a/APMrover2/Parameters.h
+++ b/APMrover2/Parameters.h
@@ -44,6 +44,9 @@ public:
         k_param_battery_volt_pin,
         k_param_battery_curr_pin,
 
+        // braking
+        k_param_braking_percent = 30,
+
 
         // 110: Telemetry control
         //
@@ -189,6 +192,9 @@ public:
     // IO pins
     AP_Int8     rssi_pin;
 
+    // braking
+    AP_Int8     braking_percent;
+
 	// Telemetry control
 	//
 	AP_Int16    sysid_this_mav;
diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde
index 3689312987..86876b823f 100644
--- a/APMrover2/Parameters.pde
+++ b/APMrover2/Parameters.pde
@@ -145,6 +145,15 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
 	GSCALAR(speed_turn_dist,    "SPEED_TURN_DIST",  2.0f),
 
+    // @Param: BRAKING_PERCENT
+    // @DisplayName: Percentage braking to apply
+    // @Description: The maximum reverse throttle braking percentage to apply when cornering
+    // @Units: percent
+    // @Range: 0 100
+    // @Increment: 1
+    // @User: Standard
+	GSCALAR(braking_percent,    "BRAKING_PERCENT",  0),
+
     // @Param: PIVOT_TURN_ANGLE
     // @DisplayName: Pivot turn angle
     // @Description: Navigation angle threshold in degrees to switch to pivot steering when SKID_STEER_OUT is 1. This allows you to setup a skid steering rover to turn on the spot in auto mode when the angle it needs to turn it greater than this angle. An angle of zero means to disable pivot turning. Note that you will probably also want to set a low value for WP_RADIUS to get neat turns.
diff --git a/APMrover2/Steering.pde b/APMrover2/Steering.pde
index d8e6ba8add..2a31f234c5 100644
--- a/APMrover2/Steering.pde
+++ b/APMrover2/Steering.pde
@@ -124,6 +124,19 @@ static void calc_throttle(float target_speed)
     } else {
         channel_throttle->servo_out = constrain_int16(throttle, g.throttle_min, g.throttle_max);
     }
+
+    if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.speed_cruise/2) {
+        // the user has asked to use reverse throttle to brake. Apply
+        // it in proportion to the ground speed error, but only when
+        // our ground speed error is more than half our cruise speed
+        float brake_gain = constrain_float(-groundspeed_error / (float)g.speed_cruise, 0, 1);
+        int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
+        channel_throttle->servo_out = constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min);
+        
+        // temporarily set us in reverse to allow the PWM setting to
+        // go negative
+        set_reverse(true);
+    }
     
     if (use_pivot_steering()) {
         channel_throttle->servo_out = 0;