Rover: mode fixes for reversing

APMrover2/mode.cpp

@@ -17,9 +17,6 @@ void Mode::exit()
 
     lateral_acceleration = 0.0f;
     rover.g.pidSpeedThrottle.reset_I();
-    if (!rover.in_auto_reverse) {
-        rover.set_reverse(false);
-    }
 }
 
 bool Mode::enter()
@@ -52,7 +49,7 @@ void Mode::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
     _desired_speed = target_speed;
 }
 
-void Mode::calc_throttle(float target_speed)
+void Mode::calc_throttle(float target_speed, bool reversed)
 {
     // get ground speed from vehicle
     const float &groundspeed = rover.ground_speed;
@@ -65,13 +62,13 @@ void Mode::calc_throttle(float target_speed)
 
     float throttle = throttle_target + (g.pidSpeedThrottle.get_pid(_speed_error * 100.0f) / 100.0f);
 
-    if (rover.in_reverse) {
+    if (reversed) {
         g2.motors.set_throttle(constrain_int16(-throttle, -g.throttle_max, -g.throttle_min));
     } else {
         g2.motors.set_throttle(constrain_int16(throttle, g.throttle_min, g.throttle_max));
     }
 
-    if (!rover.in_reverse && g.braking_percent != 0 && _speed_error < -g.braking_speederr) {
+    if (!reversed && g.braking_percent != 0 && _speed_error < -g.braking_speederr) {
         // 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 BRAKING_SPEEDERR.
@@ -82,10 +79,6 @@ void Mode::calc_throttle(float target_speed)
         const float brake_gain = constrain_float(((-_speed_error)-g.braking_speederr)/g.braking_speederr, 0.0f, 1.0f);
         const int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
         g2.motors.set_throttle(constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min));
-
-        // temporarily set us in reverse to allow the PWM setting to
-        // go negative
-        rover.set_reverse(true);
     }
 }
 
@@ -148,14 +141,19 @@ float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed)
 
 // calculate the lateral acceleration target to cause the vehicle to drive along the path from origin to destination
 // this function update lateral_acceleration and _yaw_error_cd members
-void Mode::calc_lateral_acceleration(const struct Location &origin, const struct Location &destination)
+void Mode::calc_lateral_acceleration(const struct Location &origin, const struct Location &destination, bool reversed)
 {
     // Calculate the required turn of the wheels
     // negative error = left turn
     // positive error = right turn
+    rover.nav_controller->set_reverse(reversed);
     rover.nav_controller->update_waypoint(origin, destination);
     lateral_acceleration = rover.nav_controller->lateral_acceleration();
-    _yaw_error_cd = wrap_180_cd(rover.nav_controller->target_bearing_cd() - ahrs.yaw_sensor);
+    if (reversed) {
+        _yaw_error_cd = wrap_180_cd(rover.nav_controller->target_bearing_cd() - ahrs.yaw_sensor + 18000);
+    } else {
+        _yaw_error_cd = wrap_180_cd(rover.nav_controller->target_bearing_cd() - ahrs.yaw_sensor);
+    }
     if (rover.use_pivot_steering(_yaw_error_cd)) {
         if (is_positive(_yaw_error_cd)) {
             lateral_acceleration = g.turn_max_g * GRAVITY_MSS;
@@ -169,16 +167,19 @@ void Mode::calc_lateral_acceleration(const struct Location &origin, const struct
 /*
     calculate steering angle given lateral_acceleration
 */
-void Mode::calc_nav_steer()
+void Mode::calc_nav_steer(bool reversed)
 {
     // add obstacle avoidance response to lateral acceleration target
-    if (!rover.in_reverse) {
+    if (!reversed) {
         lateral_acceleration += (rover.obstacle.turn_angle / 45.0f) * g.turn_max_g;
     }
 
     // constrain to max G force
     lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g * GRAVITY_MSS, g.turn_max_g * GRAVITY_MSS);
 
+    // set controller reversal
+    rover.steerController.set_reverse(reversed);
+
     // send final steering command to motor library
     g2.motors.set_steering(rover.steerController.get_steering_out_lat_accel(lateral_acceleration));
 }

APMrover2/mode.h

@@ -83,14 +83,14 @@ protected:
     virtual void _exit() { return; }
 
     // calculate steering angle given a desired lateral acceleration
-    virtual void calc_nav_steer();
+    void calc_nav_steer(bool reversed = false);
 
     // calculate desired lateral acceleration
-    void calc_lateral_acceleration(const struct Location &origin, const struct Location &destination);
+    void calc_lateral_acceleration(const struct Location &origin, const struct Location &destination, bool reversed = false);
 
     // calculates the amount of throttle that should be output based
     // on things like proximity to corners and current speed
-    virtual void calc_throttle(float target_speed);
+    virtual void calc_throttle(float target_speed, bool reversed = false);
 
     // calculate pilot input to nudge throttle up or down
     int16_t calc_throttle_nudge();
@@ -128,7 +128,7 @@ public:
 
     // methods that affect movement of the vehicle in this mode
     void update() override;
-    void calc_throttle(float target_speed) override;
+    void calc_throttle(float target_speed, bool reversed = false);
 
     // attributes of the mode
     bool is_autopilot_mode() const override { return true; }
@@ -146,6 +146,9 @@ public:
     void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) override;
     bool reached_heading();
 
+    // execute the mission in reverse (i.e. backing up)
+    void set_reversed(bool value);
+
 protected:
 
     bool _enter() override;
@@ -165,6 +168,7 @@ private:
 
     bool _reached_heading;      // true when vehicle has reached desired heading in TurnToHeading sub mode
     bool _stay_active_at_dest;  // true when we should actively maintain position even after reaching the destination
+    bool _reversed;             // execute the mission by backing up
 };
 
 

APMrover2/mode_auto.cpp

@@ -17,6 +17,9 @@ bool ModeAuto::_enter()
     auto_triggered = false;
     g2.motors.slew_limit_throttle(true);
 
+    // initialise reversed to be false
+    set_reversed(false);
+
     // restart mission processing
     mission.start_or_resume();
     return true;
@@ -24,9 +27,7 @@ bool ModeAuto::_enter()
 
 void ModeAuto::_exit()
 {
-    // If we are changing out of AUTO mode reset the loiter timer
-    rover.loiter_start_time = 0;
-    // ... and stop running the mission
+    // stop running the mission
     if (mission.state() == AP_Mission::MISSION_RUNNING) {
         mission.stop();
     }
@@ -34,10 +35,6 @@ void ModeAuto::_exit()
 
 void ModeAuto::update()
 {
-    if (!rover.in_auto_reverse) {
-        rover.set_reverse(false);
-    }
-
     switch (_submode) {
         case Auto_WP:
         {
@@ -53,9 +50,9 @@ void ModeAuto::update()
             bool active_at_destination = _reached_destination && _stay_active_at_dest && (_distance_to_destination > rover.g.waypoint_radius);
             if (!_reached_destination || active_at_destination) {
                 // continue driving towards destination
-                calc_lateral_acceleration(active_at_destination ? rover.current_loc : _origin, _destination);
-                calc_nav_steer();
-                calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed));
+                calc_lateral_acceleration(active_at_destination ? rover.current_loc : _origin, _destination, _reversed);
+                calc_nav_steer(_reversed);
+                calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), _reversed);
             } else {
                 // we have reached the destination so stop
                 g2.motors.set_throttle(g.throttle_min.get());
@@ -123,6 +120,15 @@ bool ModeAuto::reached_heading()
     return true;
 }
 
+// execute the mission in reverse (i.e. backing up)
+void ModeAuto::set_reversed(bool value)
+{
+    if (_reversed != value) {
+        _reversed = value;
+        rover.set_reverse(_reversed);
+    }
+}
+
 /*
     check for triggering of start of auto mode
 */
@@ -168,10 +174,9 @@ bool ModeAuto::check_trigger(void)
     return false;
 }
 
-void ModeAuto::calc_throttle(float target_speed)
+void ModeAuto::calc_throttle(float target_speed, bool reversed)
 {
-    // If not autostarting OR we are loitering at a waypoint
-    // then set the throttle to minimum
+    // If not autostarting set the throttle to minimum
     if (!check_trigger()) {
         g2.motors.set_throttle(g.throttle_min.get());
         // Stop rotation in case of loitering and skid steering
@@ -180,5 +185,5 @@ void ModeAuto::calc_throttle(float target_speed)
         }
         return;
     }
-    Mode::calc_throttle(target_speed);
+    Mode::calc_throttle(target_speed, reversed);
 }

APMrover2/mode_guided.cpp

@@ -9,16 +9,16 @@ bool ModeGuided::_enter()
     */
     lateral_acceleration = 0.0f;
     set_desired_location(rover.current_loc);
+
+    // guided mode never travels in reverse
+    rover.set_reverse(false);
+
     g2.motors.slew_limit_throttle(true);
     return true;
 }
 
 void ModeGuided::update()
 {
-    if (!rover.in_auto_reverse) {
-        rover.set_reverse(false);
-    }
-
     switch (_guided_mode) {
         case Guided_WP:
         {

APMrover2/mode_hold.cpp

@@ -6,7 +6,7 @@ void ModeHold::update()
     // hold position - stop motors and center steering
     g2.motors.set_throttle(0.0f);
     g2.motors.set_steering(0.0f);
-    if (!rover.in_auto_reverse) {
-        rover.set_reverse(false);
-    }
+
+    // hold mode never reverses
+    rover.set_reverse(false);
 }

APMrover2/mode_rtl.cpp

@@ -11,16 +11,15 @@ bool ModeRTL::_enter()
     // set destination
     set_desired_location(rover.home);
 
+    // RTL never reverses
+    rover.set_reverse(false);
+
     g2.motors.slew_limit_throttle(true);
     return true;
 }
 
 void ModeRTL::update()
 {
-    if (!rover.in_auto_reverse) {
-        rover.set_reverse(false);
-    }
-
     if (!_reached_destination) {
         // calculate distance to home
         _distance_to_destination = get_distance(rover.current_loc, _destination);

APMrover2/mode_steering.cpp

@@ -21,16 +21,19 @@ void ModeSteering::update()
     lateral_acceleration = max_g_force * (channel_steer->get_control_in() / 4500.0f);
 
     // reverse target lateral acceleration if backing up
+    bool reversed = false;
     if (is_negative(target_speed)) {
+        reversed = true;
         lateral_acceleration = -lateral_acceleration;
+        target_speed = fabsf(target_speed);
     }
 
-    // mark us as in_reverse when using a negative throttle to stop AHRS getting off
-    rover.set_reverse(is_negative(target_speed));
+    // mark us as in_reverse when using a negative throttle
+    rover.set_reverse(reversed);
 
     // run steering controller
-    calc_nav_steer();
+    calc_nav_steer(reversed);
 
     // run speed to throttle output controller
-    calc_throttle(target_speed);
+    calc_throttle(target_speed, reversed);
 }