Rover: simple mode fixes

APMrover2/Rover.h

@@ -399,7 +399,6 @@ private:
     void one_second_loop(void);
     void update_GPS(void);
     void update_current_mode(void);
-    void init_simple_mode(void);
 
     // balance_bot.cpp
     void balancebot_pitch_control(float &throttle);

APMrover2/mode.cpp

@@ -148,16 +148,12 @@ void Mode::get_pilot_desired_lateral(float &lateral_out)
 // decode pilot's input and return heading_out (in cd) and speed_out (in m/s)
 void Mode::get_pilot_desired_heading_and_speed(float &heading_out, float &speed_out)
 {
-    float desired_steering;
-    float desired_throttle;
-    get_pilot_input(desired_steering, desired_throttle);
-
-    // scale down and limit throttle and steering to a -1 to +1 range
-    desired_throttle = constrain_float(desired_throttle / 100.0f, -1.0f, 1.0f);
-    desired_steering = constrain_float(desired_steering / 4500.0f, -1.0, 1.0f);
+    // get steering and throttle in the -1 to +1 range
+    float desired_steering = constrain_float(rover.channel_steer->get_control_in() / 4500.0f, -1.0, 1.0f);
+    float desired_throttle = constrain_float(rover.channel_throttle->get_control_in() / 100.0f, -1.0f, 1.0f);
 
     // calculate angle of input stick vector
-    heading_out = wrap_360_cd(atan2f(desired_steering,desired_throttle) * DEGX100);
+    heading_out = wrap_360_cd(atan2f(desired_steering, desired_throttle) * DEGX100);
 
     // calculate magnitude of input stick vector
     float magnitude_max = 1.0f;

APMrover2/mode.h

@@ -496,16 +496,17 @@ public:
 
     // methods that affect movement of the vehicle in this mode
     void update() override;
-    void init_simple_heading();
+    void init_heading();
 
-protected:
-
-    float simple_initial_heading;
+private:
 
     // simple type enum used for SIMPLE_TYPE parameter
     enum simple_type {
         Simple_InitialHeading = 0,
         Simple_CardinalDirections = 1,
     };
+
+    float _initial_heading_cd;  // vehicle heading (in centi-degrees) at moment vehicle was armed
+    float _desired_heading_cd;  // latest desired heading (in centi-degrees) from pilot
 };
 

APMrover2/mode_simple.cpp

@@ -1,41 +1,33 @@
 #include "mode.h"
 #include "Rover.h"
 
-void ModeSimple::init_simple_heading()
+void ModeSimple::init_heading()
 {
-    simple_initial_heading = ahrs.yaw;
+    _initial_heading_cd = ahrs.yaw_sensor;
+    _desired_heading_cd = ahrs.yaw_sensor;
 }
 
 void ModeSimple::update()
 {
-    float desired_heading, desired_steering, desired_speed;
-
-    // initial heading simple mode
-    if (g2.simple_type == Simple_InitialHeading) {
-
-        // get piot input
-        get_pilot_desired_steering_and_speed(desired_steering, desired_speed);
-
-        float simple_heading;
-        if (is_zero(desired_steering)) {
-            simple_heading = ((simple_initial_heading - ahrs.yaw) * 4500.0f);
-        } else {
-            simple_heading = desired_steering;
-        }
-
-        // run throttle and steering controllers
-        calc_steering_to_heading(simple_heading, false);
-        calc_throttle(desired_speed, true, false);
-    }
-
-    // cardinal directions simple mode
-    if (g2.simple_type == Simple_CardinalDirections) {
+    float desired_heading_cd, desired_speed;
 
     // get pilot input
-        get_pilot_desired_heading_and_speed(desired_heading, desired_speed);
+    get_pilot_desired_heading_and_speed(desired_heading_cd, desired_speed);
+
+    // rotate heading around based on initial heading
+    if (g2.simple_type == Simple_InitialHeading) {
+        desired_heading_cd = wrap_360_cd(_initial_heading_cd + desired_heading_cd);
+    }
+
+    // if sticks in middle, use previous desired heading (important when vehicle is slowing down)
+    if (!is_positive(desired_speed)) {
+        desired_heading_cd = _desired_heading_cd;
+    } else {
+        // record desired heading for next iteration
+        _desired_heading_cd = desired_heading_cd;
+    }
 
     // run throttle and steering controllers
-        calc_steering_to_heading(desired_heading, false);
+    calc_steering_to_heading(desired_heading_cd, 0.0f, false);
     calc_throttle(desired_speed, false, true);
-    }
 }

APMrover2/system.cpp

@@ -333,7 +333,7 @@ bool Rover::arm_motors(AP_Arming::ArmingMethod method)
     g2.smart_rtl.set_home(true);
 
     // initialize simple mode heading
-    rover.mode_simple.init_simple_heading();
+    rover.mode_simple.init_heading();
 
     change_arm_state();
     return true;