diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 4aab083209..eee3848617 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -104,8 +104,6 @@ void Rover::setup() // load the default values of variables listed in var_info[] AP_Param::setup_sketch_defaults(); - in_auto_reverse = false; - init_ardupilot(); // initialise the main loop scheduler diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index c9318bc939..fb4fd6c96a 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -351,9 +351,6 @@ private: // set if we are driving backwards bool in_reverse; - // set if the users asks for auto reverse - bool in_auto_reverse; - // true if pivoting (set by use_pivot_steering) bool pivot_steering_active; diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index d8f3176a77..13cbce5ec8 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -423,11 +423,6 @@ void Rover::do_digicam_control(const AP_Mission::Mission_Command& cmd) void Rover::do_set_reverse(const AP_Mission::Mission_Command& cmd) { - if (cmd.p1 == 1) { - in_auto_reverse = true; - set_reverse(true); - } else { - in_auto_reverse = false; - set_reverse(false); - } + mode_auto.set_reversed(cmd.p1 == 1); + set_reverse(cmd.p1 == 1); } diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 622ede421f..5ca41857ca 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -276,10 +276,6 @@ void Rover::set_reverse(bool reverse) if (in_reverse == reverse) { return; } - g.pidSpeedThrottle.reset_I(); - steerController.reset_I(); - nav_controller->set_reverse(reverse); - steerController.set_reverse(reverse); in_reverse = reverse; }