From c6dffb990fe70155fe3c3b8009b7bd9016d5d5a9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 6 Jan 2022 13:07:03 +0900 Subject: [PATCH] AR_WPNav: add set_nudge_speed_max allows pilot to override speed_max from RC input --- libraries/AR_WPNav/AR_WPNav.cpp | 63 ++++++++++++++++++++++++--------- libraries/AR_WPNav/AR_WPNav.h | 17 +++++++-- 2 files changed, 62 insertions(+), 18 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 30eb35832b..ec7da187e0 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -27,6 +27,7 @@ extern const AP_HAL::HAL& hal; #define AR_WPNAV_TIMEOUT_MS 100 #define AR_WPNAV_SPEED_DEFAULT 2.0f #define AR_WPNAV_SPEED_MIN 0.05f // minimum speed between waypoints in m/s +#define AR_WPNAV_SPEED_UPDATE_MIN_MS 500 // max speed cannot be updated more than once in this many milliseconds #define AR_WPNAV_RADIUS_DEFAULT 2.0f #define AR_WPNAV_OVERSPEED_RATIO_MAX 5.0f // if _overspeed_enabled the vehicle may travel as quickly as 5x WP_SPEED @@ -94,16 +95,18 @@ AR_WPNav::AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control) : void AR_WPNav::init(float speed_max) { // determine max speed, acceleration and jerk - if (!is_positive(speed_max)) { - speed_max = _speed_max; + if (is_positive(speed_max)) { + _base_speed_max = speed_max; + } else { + _base_speed_max = _speed_max; } - speed_max = MAX(AR_WPNAV_SPEED_MIN, speed_max); + _base_speed_max = MAX(AR_WPNAV_SPEED_MIN, _base_speed_max); const float atc_accel_max = MIN(_atc.get_accel_max(), _atc.get_decel_max()); const float accel_max = is_positive(_accel_max) ? MIN(_accel_max, atc_accel_max) : atc_accel_max; const float jerk_max = is_positive(_jerk_max) ? _jerk_max : accel_max; // initialise position controller - _pos_control.set_limits(speed_max, accel_max, _atc.get_turn_lat_accel_max(), jerk_max); + _pos_control.set_limits(_base_speed_max, accel_max, _atc.get_turn_lat_accel_max(), jerk_max); _scurve_prev_leg.init(); _scurve_this_leg.init(); @@ -121,6 +124,9 @@ void AR_WPNav::init(float speed_max) // initialise origin and destination to stopping point _orig_and_dest_valid = false; set_origin_and_destination_to_stopping_point(); + + // initialise nudge speed to zero + set_nudge_speed_max(0); } // update navigation @@ -145,6 +151,9 @@ void AR_WPNav::update(float dt) update_distance_and_bearing_to_destination(); + // handle change in max speed + update_speed_max(); + // advance target along path unless vehicle is pivoting if (!_pivot.active()) { switch (_nav_control_type) { @@ -170,21 +179,17 @@ bool AR_WPNav::set_speed_max(float speed_max) return false; } - // ignore calls that do not change the speed - if (is_equal(speed_max, _pos_control.get_speed_max())) { - return true; - } - - // update position controller max speed - _pos_control.set_limits(speed_max, _pos_control.get_accel_max(), _pos_control.get_lat_accel_max(), _pos_control.get_jerk_max()); - - // change track speed - _scurve_this_leg.set_speed_max(_pos_control.get_speed_max(), _pos_control.get_speed_max(), _pos_control.get_speed_max()); - _scurve_next_leg.set_speed_max(_pos_control.get_speed_max(), _pos_control.get_speed_max(), _pos_control.get_speed_max()); - + _base_speed_max = speed_max; return true; } +// set speed nudge in m/s. this will have no effect unless nudge_speed_max > speed_max +// nudge_speed_max should always be positive regardless of whether the vehicle is travelling forward or reversing +void AR_WPNav::set_nudge_speed_max(float nudge_speed_max) +{ + _nudge_speed_max = nudge_speed_max; +} + // set desired location and (optionally) next_destination // next_destination should be provided if known to allow smooth cornering bool AR_WPNav::set_desired_location(const struct Location& destination, Location next_destination) @@ -586,3 +591,29 @@ bool AR_WPNav::set_origin_and_destination_to_stopping_point() _orig_and_dest_valid = true; return true; } + +// check for changes in _base_speed_max or _nudge_speed_max +// updates position controller limits and recalculate scurve path if required +void AR_WPNav::update_speed_max() +{ + const float speed_max = MAX(_base_speed_max, _nudge_speed_max); + + // ignore calls that do not change the speed + if (is_equal(speed_max, _pos_control.get_speed_max())) { + return; + } + + // protect against rapid updates + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - _last_speed_update_ms < AR_WPNAV_SPEED_UPDATE_MIN_MS) { + return; + } + _last_speed_update_ms = now_ms; + + // update position controller max speed + _pos_control.set_limits(speed_max, _pos_control.get_accel_max(), _pos_control.get_lat_accel_max(), _pos_control.get_jerk_max()); + + // change track speed + _scurve_this_leg.set_speed_max(_pos_control.get_speed_max(), _pos_control.get_speed_max(), _pos_control.get_speed_max()); + _scurve_next_leg.set_speed_max(_pos_control.get_speed_max(), _pos_control.get_speed_max(), _pos_control.get_speed_max()); +} diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index 594e8c0bb3..780c50481c 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -20,10 +20,14 @@ public: virtual void update(float dt); // get or set maximum speed in m/s - // set_speed_max should not be called at more than 3hz or else SCurve path planning may not advance properly - float get_speed_max() const { return _pos_control.get_speed_max(); } + // if set_speed_max is called in rapid succession changes in speed may be delayed by up to 0.5sec + float get_speed_max() const { return _base_speed_max; } bool set_speed_max(float speed_max); + // set speed nudge in m/s. this will have no effect unless nudge_speed_max > speed_max + // nudge_speed_max should always be positive regardless of whether the vehicle is travelling forward or reversing + void set_nudge_speed_max(float nudge_speed_max); + // execute the mission in reverse (i.e. drive backwards to destination) bool get_reversed() const { return _reversed; } void set_reversed(bool reversed) { _reversed = reversed; } @@ -131,6 +135,10 @@ protected: // set origin and destination to stopping point bool set_origin_and_destination_to_stopping_point(); + // check for changes in _base_speed_max or _nudge_speed_max + // updates position controller limits and recalculate scurve path if required + void update_speed_max(); + // parameters AP_Float _speed_max; // target speed between waypoints in m/s AP_Float _radius; // distance in meters from a waypoint when we consider the waypoint has been reached @@ -165,6 +173,11 @@ protected: NAV_PSC_INPUT_SHAPING // position controller input shaping used for navigation } _nav_control_type; // navigation controller that should be used to travel from _origin to _destination + // speed_max handling + float _base_speed_max; // speed max (in m/s) derived from parameters or passed into init + float _nudge_speed_max; // "nudge" speed max (in m/s) normally from the pilot. has no effect if less than _base_speed_max. always positive. + uint32_t _last_speed_update_ms; // system time that speed_max was last update. used to ensure speed_max is not update too quickly + // main outputs from navigation library float _desired_speed_limited; // desired speed (above) but accel/decel limited float _desired_turn_rate_rads; // desired turn-rate in rad/sec (negative is counter clockwise, positive is clockwise)