From df3b6202f5443d8079fbe9406f95c0913f871e6b Mon Sep 17 00:00:00 2001 From: Raouf Date: Wed, 8 Aug 2018 14:18:13 +0900 Subject: [PATCH] Rover: support DO_SET_REVERSE commands in guided, RTL, SmartRTL --- APMrover2/GCS_Mavlink.cpp | 15 ++++++++------- APMrover2/commands_logic.cpp | 2 +- APMrover2/mode.h | 1 - APMrover2/mode_guided.cpp | 4 ++-- APMrover2/mode_rtl.cpp | 4 ++-- APMrover2/mode_smart_rtl.cpp | 8 ++++---- 6 files changed, 17 insertions(+), 17 deletions(-) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 77fe48bada..30e7768ca0 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -537,6 +537,14 @@ 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)) { @@ -693,13 +701,6 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l } return MAV_RESULT_ACCEPTED; - case MAV_CMD_DO_SET_REVERSE: - // param1 : Direction (0=Forward, 1=Reverse) - if(!rover.control_mode->set_reversed(packet.param1)) { - return MAV_RESULT_FAILED; - } - return MAV_RESULT_ACCEPTED; - case MAV_CMD_DO_SET_HOME: { // param1 : use current (1=use current location, 0=use specified location) diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index f5ac3bada9..84715816c6 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) { - mode_auto.set_reversed(cmd.p1 == 1); + rover.control_mode->set_reversed(cmd.p1 == 1); } diff --git a/APMrover2/mode.h b/APMrover2/mode.h index 4a75166e23..c5eece3139 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -251,7 +251,6 @@ public: // start RTL (within auto) void start_RTL(); - protected: bool _enter() override; diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp index 335e422d06..c4759a2231 100644 --- a/APMrover2/mode_guided.cpp +++ b/APMrover2/mode_guided.cpp @@ -29,8 +29,8 @@ void ModeGuided::update() // determine if we should keep navigating if (!_reached_destination || (rover.is_boat() && !near_wp)) { // drive towards destination - calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination); - calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, true); + calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed); + calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true); } else { stop_vehicle(); } diff --git a/APMrover2/mode_rtl.cpp b/APMrover2/mode_rtl.cpp index 6b9e792165..5f77e17ec9 100644 --- a/APMrover2/mode_rtl.cpp +++ b/APMrover2/mode_rtl.cpp @@ -31,8 +31,8 @@ void ModeRTL::update() // determine if we should keep navigating if (!_reached_destination || (rover.is_boat() && !near_wp)) { // continue driving towards destination - calc_steering_to_waypoint(_reached_destination ? rover.current_loc :_origin, _destination); - calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, false); + calc_steering_to_waypoint(_reached_destination ? rover.current_loc :_origin, _destination, _reversed); + calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, false); } else { // we've reached destination so stop stop_vehicle(); diff --git a/APMrover2/mode_smart_rtl.cpp b/APMrover2/mode_smart_rtl.cpp index a60a01dcb8..295385a143 100644 --- a/APMrover2/mode_smart_rtl.cpp +++ b/APMrover2/mode_smart_rtl.cpp @@ -64,8 +64,8 @@ void ModeSmartRTL::update() _load_point = true; } // continue driving towards destination - calc_steering_to_waypoint(_origin, _destination); - calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, false); + calc_steering_to_waypoint(_origin, _destination, _reversed); + calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, false); break; case SmartRTL_StopAtHome: @@ -73,8 +73,8 @@ void ModeSmartRTL::update() _reached_destination = true; if (rover.is_boat()) { // boats attempt to hold position at home - calc_steering_to_waypoint(rover.current_loc, _destination); - calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, false); + calc_steering_to_waypoint(rover.current_loc, _destination, _reversed); + calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, false); } else { // rovers stop stop_vehicle();