diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 30e7768ca0..66917b6d5d 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -537,14 +537,6 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_in } return MAV_RESULT_ACCEPTED; - case MAV_CMD_DO_SET_REVERSE: - // param1 : Direction (0=Forward, 1=Reverse) - if (is_equal(packet.param1, 1.0f)) { - rover.control_mode->set_reversed(1); - return MAV_RESULT_ACCEPTED; - } - return MAV_RESULT_FAILED; - case MAV_CMD_DO_SET_HOME: { // assume failure if (is_equal(packet.param1, 1.0f)) { @@ -587,6 +579,11 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_in return MAV_RESULT_ACCEPTED; } + case MAV_CMD_DO_SET_REVERSE: + // param1 : Direction (0=Forward, 1=Reverse) + rover.control_mode->set_reversed(is_equal(packet.param1,1.0f)); + return MAV_RESULT_ACCEPTED; + #if MOUNT == ENABLED case MAV_CMD_DO_SET_ROI: { // param1 : /* Region of interest mode (not used)*/ @@ -727,6 +724,11 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l return MAV_RESULT_FAILED; } + case MAV_CMD_DO_SET_REVERSE: + // param1 : Direction (0=Forward, 1=Reverse) + rover.control_mode->set_reversed(is_equal(packet.param1,1.0f)); + return MAV_RESULT_ACCEPTED; + case MAV_CMD_NAV_SET_YAW_SPEED: { // param1 : yaw angle to adjust direction by in centidegress diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index 84715816c6..6d1453ea9b 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -418,5 +418,5 @@ void Rover::do_digicam_control(const AP_Mission::Mission_Command& cmd) void Rover::do_set_reverse(const AP_Mission::Mission_Command& cmd) { - rover.control_mode->set_reversed(cmd.p1 == 1); + control_mode->set_reversed(cmd.p1 == 1); } diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 826aee0457..5f202ff97c 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -229,9 +229,7 @@ bool Mode::set_desired_speed(float speed) // execute the mission in reverse (i.e. backing up) void Mode::set_reversed(bool value) { - if (_reversed != value) { - _reversed = value; - } + _reversed = value; } void Mode::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled)