From 0b917cfd36a8230ef4bd6510940d887958d35ef5 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 10 Aug 2017 10:30:35 +0900 Subject: [PATCH] Rover: mode auto guided and rtl slow before destination new mode class member _desired_speed_final holds target speed at destination main vehicle code passes heading to next waypoint into auto mode. we do not provide heading when delaying at waypoint which signals we wish auto-mode calculates final speed at destination which allows vehicle to make turn within value of WP_OVERSHOOT parameter assuming vehicle turns at maximum lateral acceleration. --- APMrover2/Parameters.cpp | 9 +++++++++ APMrover2/Parameters.h | 2 ++ APMrover2/Rover.h | 3 --- APMrover2/commands_logic.cpp | 13 +++++++++---- APMrover2/mode.cpp | 30 ++++++++++++++++++++++++------ APMrover2/mode.h | 11 ++++++++--- APMrover2/mode_auto.cpp | 6 +++--- 7 files changed, 55 insertions(+), 19 deletions(-) diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index c8fd738079..f3abf90903 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -314,6 +314,15 @@ const AP_Param::Info Rover::var_info[] = { // @User: Standard GSCALAR(waypoint_radius, "WP_RADIUS", 2.0f), + // @Param: WP_OVERSHOOT + // @DisplayName: Waypoint overshoot maximum + // @Description: Waypoint overshoot maximum in meters. The vehicle will attempt to stay within this many meters of the track as it completes one waypoint and moves to the next. + // @Units: m + // @Range: 0 10 + // @Increment: 0.1 + // @User: Standard + GSCALAR(waypoint_overshoot, "WP_OVERSHOOT", 2.0f), + // @Param: TURN_MAX_G // @DisplayName: Turning maximum G force // @Description: The maximum turning acceleration (in units of gravities) that the rover can handle while remaining stable. The navigation code will keep the lateral acceleration below this level to avoid rolling over or slipping the wheels in turns diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 576ee958a4..21f4237733 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -168,6 +168,7 @@ public: k_param_command_total = 220, // unused k_param_command_index, // unused k_param_waypoint_radius, + k_param_waypoint_overshoot, // // 230: camera control @@ -271,6 +272,7 @@ public: // Waypoints // AP_Float waypoint_radius; + AP_Float waypoint_overshoot; Parameters() {} }; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 90381060a3..f124aa3397 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -259,9 +259,6 @@ private: // true if we have a position estimate from AHRS bool have_position; - // angle of our next navigation waypoint - int32_t next_navigation_leg_cd; - // receiver RSSI uint8_t receiver_rssi; diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index cb16655d60..21e27a0cde 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -22,9 +22,6 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd) gcs().send_text(MAV_SEVERITY_INFO, "Executing command ID #%i", cmd.id); - // remember the course of our next navigation leg - next_navigation_leg_cd = mission.get_next_ground_course_cd(0); - switch (cmd.id) { case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint do_nav_wp(cmd, false); @@ -228,9 +225,17 @@ void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool stay_active_a // this is the delay, stored in seconds loiter_duration = cmd.p1; + // get heading to following waypoint (auto mode reduces speed to allowing 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) { + 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, stay_active_at_dest); + 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) diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 9d2a13fab7..9006eb8d6a 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -26,7 +26,7 @@ bool Mode::enter() } // set desired location -void Mode::set_desired_location(const struct Location& destination) +void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd) { // record targets _origin = rover.current_loc; @@ -36,6 +36,21 @@ void Mode::set_desired_location(const struct Location& destination) // initialise distance _distance_to_destination = get_distance(_origin, _destination); _reached_destination = false; + + // set final desired speed + _desired_speed_final = 0.0f; + if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) { + // if not turning can continue at full speed + if (is_zero(next_leg_bearing_cd)) { + _desired_speed_final = _desired_speed; + } else { + // calculate maximum speed that keeps overshoot within bounds + const float curr_leg_bearing_cd = get_bearing_cd(_origin, _destination); + const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd); + const float radius_m = fabsf(g.waypoint_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f)); + _desired_speed_final = MIN(_desired_speed, safe_sqrt(g.turn_max_g * GRAVITY_MSS * radius_m)); + } + } } // set desired heading and speed @@ -148,14 +163,17 @@ float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed) pivot_speed_scaling = 0.0f; } - // calculate a waypoint distance based scaling (default to no reduction) - float distance_speed_scaling = 1.0f; - if (is_positive(distance_to_waypoint)) { - distance_speed_scaling = 1.0f - yaw_error_ratio; + // scaled speed + float speed_scaled = desired_speed * MIN(lateral_accel_speed_scaling, pivot_speed_scaling); + + // limit speed based on distance to waypoint and max acceleration/deceleration + if (is_positive(distance_to_waypoint) && is_positive(attitude_control.get_accel_max())) { + const float speed_max = safe_sqrt(2.0f * distance_to_waypoint * attitude_control.get_accel_max() + sq(_desired_speed_final)); + speed_scaled = constrain_float(speed_scaled, -speed_max, speed_max); } // return minimum speed - return desired_speed * MIN(MIN(lateral_accel_speed_scaling, distance_speed_scaling), pivot_speed_scaling); + return speed_scaled; } // calculate the lateral acceleration target to cause the vehicle to drive along the path from origin to destination diff --git a/APMrover2/mode.h b/APMrover2/mode.h index 0f56c28a47..72d085473c 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -4,6 +4,8 @@ #include // for MAV_SEVERITY #include "defines.h" +#define MODE_NEXT_HEADING_UNKNOWN 99999.0f // used to indicate to set_desired_location method that next leg's heading is unknown + class Mode { public: @@ -62,7 +64,9 @@ public: virtual float get_distance_to_destination() const { return 0.0f; } // set desired location and speed (used in RTL, Guided, Auto) - virtual void set_desired_location(const struct Location& destination); + // next_leg_bearing_cd should be heading to the following waypoint (used to slow the vehicle in order to make the turn) + virtual void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN); + // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck virtual bool reached_destination() { return true; } @@ -126,6 +130,7 @@ protected: float _desired_yaw_cd; // desired yaw in centi-degrees float _yaw_error_cd; // error between desired yaw and actual yaw in centi-degrees float _desired_speed; // desired speed in m/s + float _desired_speed_final; // desired speed in m/s when we reach the destination float _speed_error; // ground speed error in m/s }; @@ -149,7 +154,7 @@ public: // 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, bool stay_active_at_dest); + void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN, bool stay_active_at_dest = false); bool reached_destination() override; // heading and speed control @@ -199,7 +204,7 @@ public: float get_distance_to_destination() const override; // set desired location, heading and speed - void set_desired_location(const struct Location& destination) override; + void set_desired_location(const struct Location& destination); void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) override; // set desired heading-delta, turn-rate and speed diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp index 16b042c25c..796fcdb573 100644 --- a/APMrover2/mode_auto.cpp +++ b/APMrover2/mode_auto.cpp @@ -10,7 +10,7 @@ bool ModeAuto::_enter() } // init location target - set_desired_location(rover.current_loc, false); + set_desired_location(rover.current_loc); // other initialisation auto_triggered = false; @@ -81,10 +81,10 @@ void ModeAuto::update() } // set desired location to drive to -void ModeAuto::set_desired_location(const struct Location& destination, bool stay_active_at_dest) +void ModeAuto::set_desired_location(const struct Location& destination, float next_leg_bearing_cd, bool stay_active_at_dest) { // call parent - Mode::set_desired_location(destination); + Mode::set_desired_location(destination, next_leg_bearing_cd); _submode = Auto_WP; _stay_active_at_dest = stay_active_at_dest;