From 3645e182438b1a39b2895d3b10aafde67fc7e765 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sun, 6 May 2018 10:48:26 +0900
Subject: [PATCH] Rover: acro and steering use
 get_pilot_desired_steering_and_speed

---
 APMrover2/mode_acro.cpp     | 39 +++++++++++++++++--------------------
 APMrover2/mode_steering.cpp | 15 ++++++--------
 2 files changed, 24 insertions(+), 30 deletions(-)

diff --git a/APMrover2/mode_acro.cpp b/APMrover2/mode_acro.cpp
index cfb7c1349c..6f9cd16125 100644
--- a/APMrover2/mode_acro.cpp
+++ b/APMrover2/mode_acro.cpp
@@ -3,14 +3,24 @@
 
 void ModeAcro::update()
 {
-
-    // convert pilot stick input into desired steering and throttle
-    float desired_steering, desired_throttle;
-    get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
-
-    // set reverse flag backing up
-    const bool reversed = is_negative(desired_throttle);
-    rover.set_reverse(reversed);
+    // get speed forward
+    float speed, desired_steering;
+    if (!attitude_control.get_forward_speed(speed)) {
+        float desired_throttle;
+        // convert pilot stick input into desired steering and throttle
+        get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
+        // no valid speed, just use the provided throttle
+        g2.motors.set_throttle(desired_throttle);
+        // set reverse flag if negative throttle
+        rover.set_reverse(is_negative(desired_throttle));
+    } else {
+        float desired_speed;
+        // convert pilot stick input into desired steering and speed
+        get_pilot_desired_steering_and_speed(desired_steering, desired_speed);
+        calc_throttle(desired_speed, false, true);
+        // set reverse flag if negative desired speed
+        rover.set_reverse(is_negative(desired_speed));
+    }
 
     // convert pilot steering input to desired turn rate in radians/sec
     const float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);
@@ -22,19 +32,6 @@ void ModeAcro::update()
                                                                     g2.motors.limit.steer_right);
 
     g2.motors.set_steering(steering_out * 4500.0f);
-
-    // get speed forward
-    float speed;
-    if (!attitude_control.get_forward_speed(speed)) {
-        // no valid speed, just use the provided throttle
-        g2.motors.set_throttle(desired_throttle);
-    } else {
-
-        // convert pilot throttle input to desired speed
-        float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
-
-        calc_throttle(target_speed, false, true);
-    }
 }
 
 bool ModeAcro::requires_velocity() const
diff --git a/APMrover2/mode_steering.cpp b/APMrover2/mode_steering.cpp
index 34fb2d6e1e..9f01f9d7f2 100644
--- a/APMrover2/mode_steering.cpp
+++ b/APMrover2/mode_steering.cpp
@@ -3,12 +3,6 @@
 
 void ModeSteering::update()
 {
-    float desired_steering, desired_throttle;
-    get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
-
-    // convert pilot throttle input to desired speed (up to twice the cruise speed)
-    float target_speed = desired_throttle * 0.01f * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
-
     // get speed forward
     float speed;
     if (!attitude_control.get_forward_speed(speed)) {
@@ -18,8 +12,11 @@ void ModeSteering::update()
         return;
     }
 
+    float desired_steering, desired_speed;
+    get_pilot_desired_steering_and_speed(desired_steering, desired_speed);
+
     // determine if pilot is requesting pivot turn
-    bool is_pivot_turning = g2.motors.have_skid_steering() && is_zero(target_speed) && (!is_zero(desired_steering));
+    bool is_pivot_turning = g2.motors.have_skid_steering() && is_zero(desired_speed) && (!is_zero(desired_steering));
 
     // In steering mode we control lateral acceleration directly.
     // For pivot steering vehicles we use the TURN_MAX_G parameter
@@ -39,7 +36,7 @@ void ModeSteering::update()
 
     // reverse target lateral acceleration if backing up
     bool reversed = false;
-    if (is_negative(target_speed)) {
+    if (is_negative(desired_speed)) {
         reversed = true;
         desired_lat_accel = -desired_lat_accel;
     }
@@ -51,5 +48,5 @@ void ModeSteering::update()
     calc_steering_from_lateral_acceleration(desired_lat_accel, reversed);
 
     // run speed to throttle controller
-    calc_throttle(target_speed, false, true);
+    calc_throttle(desired_speed, false, true);
 }