From 8eb991b6107cefebc9de50a780626550620fd534 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 7 Dec 2017 11:42:34 +0900 Subject: [PATCH] Rover: boats say active at destination in auto previously the user would use the LOITER_UNLIMITED or LOITER_TIME mission commands to specify that the vehicle should remain active at the destination. This was cumbersome and not the best way to specify this behaviour because these two commands are valid for regular rovers that should not try to remain active at the destination. --- APMrover2/Rover.h | 4 +--- APMrover2/commands_logic.cpp | 28 ++++++---------------------- APMrover2/mode.h | 4 +--- APMrover2/mode_auto.cpp | 9 +++------ 4 files changed, 11 insertions(+), 34 deletions(-) diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index a5a8534839..250c2de741 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -423,9 +423,7 @@ private: bool verify_command_callback(const AP_Mission::Mission_Command& cmd); bool verify_command(const AP_Mission::Mission_Command& cmd); void do_RTL(void); - void do_nav_wp(const AP_Mission::Mission_Command& cmd, bool stay_active_at_dest); - void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd); - void do_loiter_time(const AP_Mission::Mission_Command& cmd); + void do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination); void do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd); bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); bool verify_RTL(); diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index fa849fcb2b..0096486d55 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -32,12 +32,9 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd) do_RTL(); break; - case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely - do_loiter_unlimited(cmd); - break; - - case MAV_CMD_NAV_LOITER_TIME: - do_loiter_time(cmd); + case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely + case MAV_CMD_NAV_LOITER_TIME: // Loiter for specified time + do_nav_wp(cmd, true); break; case MAV_CMD_NAV_SET_YAW_SPEED: @@ -216,7 +213,7 @@ void Rover::do_RTL(void) mode_auto.start_RTL(); } -void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool stay_active_at_dest) +void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination) { // just starting so we haven't previously reached the waypoint previously_reached_wp = false; @@ -230,27 +227,14 @@ void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool stay_active_a // get heading to following waypoint (auto mode reduces speed to allow corning without large overshoot) // in case of non-zero loiter duration, we provide heading-unknown to signal we should stop at the point float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN; - if (loiter_duration == 0) { + if (!always_stop_at_destination && loiter_duration == 0) { next_leg_bearing_cd = mission.get_next_ground_course_cd(MODE_NEXT_HEADING_UNKNOWN); } // retrieve and sanitize target location Location cmdloc = cmd.content.location; location_sanitize(current_loc, cmdloc); - mode_auto.set_desired_location(cmdloc, next_leg_bearing_cd, stay_active_at_dest); -} - -void Rover::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) -{ - do_nav_wp(cmd, true); -} - -// do_loiter_time - initiate loitering at a point for a given time period -// if the vehicle is moved off the loiter point (i.e. a boat in a current) -// then the vehicle will actively return to the loiter coords. -void Rover::do_loiter_time(const AP_Mission::Mission_Command& cmd) -{ - do_nav_wp(cmd, true); + mode_auto.set_desired_location(cmdloc, next_leg_bearing_cd); } // do_set_yaw_speed - turn to a specified heading and achieve and given speed diff --git a/APMrover2/mode.h b/APMrover2/mode.h index ee3ac95c72..842dbd3ff8 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -200,8 +200,7 @@ public: float get_distance_to_destination() const override { return _distance_to_destination; } // set desired location, heading and speed - // set stay_active_at_dest if the vehicle should attempt to maintain it's position at the destination (mostly for boats) - void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN, bool stay_active_at_dest = false); + void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN); bool reached_destination() override; // heading and speed control @@ -236,7 +235,6 @@ private: bool auto_triggered; 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 }; diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp index bb16b9e2a2..0d2dba32f5 100644 --- a/APMrover2/mode_auto.cpp +++ b/APMrover2/mode_auto.cpp @@ -53,11 +53,9 @@ void ModeAuto::update() _reached_destination = true; } } - // stay active at destination if caller requested this behaviour and outside the waypoint radius - bool active_at_destination = _reached_destination && _stay_active_at_dest && (_distance_to_destination > rover.g.waypoint_radius); - if (!_reached_destination || active_at_destination) { + if (!_reached_destination || rover.is_boat()) { // continue driving towards destination - calc_steering_to_waypoint(active_at_destination ? rover.current_loc : _origin, _destination, _reversed); + 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); } else { // we have reached the destination so stop @@ -90,13 +88,12 @@ void ModeAuto::update() } // set desired location to drive to -void ModeAuto::set_desired_location(const struct Location& destination, float next_leg_bearing_cd, bool stay_active_at_dest) +void ModeAuto::set_desired_location(const struct Location& destination, float next_leg_bearing_cd) { // call parent Mode::set_desired_location(destination, next_leg_bearing_cd); _submode = Auto_WP; - _stay_active_at_dest = stay_active_at_dest; } // return true if vehicle has reached or even passed destination