Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AR_WPNav: enable_overspeed added to improve sailboat support

This commit is contained in:
Randy Mackay 2022-01-05 14:52:06 +09:00
parent 2f6e098f23
commit 653afbeb15
2 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -27,6 +27,7 @@ extern const AP_HAL::HAL& hal;
#define AR_WPNAV_TIMEOUT_MS 100 #define AR_WPNAV_TIMEOUT_MS 100
#define AR_WPNAV_SPEED_DEFAULT 2.0f #define AR_WPNAV_SPEED_DEFAULT 2.0f
#define AR_WPNAV_RADIUS_DEFAULT 2.0f #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
const AP_Param::GroupInfo AR_WPNav::var_info[] = { const AP_Param::GroupInfo AR_WPNav::var_info[] = {
@ -419,7 +420,8 @@ void AR_WPNav::advance_wp_target_along_track(const Location &current_loc, float
const float track_error = _pos_control.get_pos_error().tofloat().dot(track_direction); const float track_error = _pos_control.get_pos_error().tofloat().dot(track_direction);
float track_velocity = curr_vel_NED.xy().dot(track_direction); float track_velocity = curr_vel_NED.xy().dot(track_direction);
// set time scaler to be consistent with the achievable vehicle speed with a 5% buffer for short term variation. // set time scaler to be consistent with the achievable vehicle speed with a 5% buffer for short term variation.
track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_p().kP() * track_error) / curr_target_vel.length(), 0.1f, 1.0f); const float time_scaler_dt_max = _overspeed_enabled ? AR_WPNAV_OVERSPEED_RATIO_MAX : 1.0f;
track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_p().kP() * track_error) / curr_target_vel.length(), 0.1f, time_scaler_dt_max);
} }
// change s-curve time speed with a time constant of maximum acceleration / maximum jerk // change s-curve time speed with a time constant of maximum acceleration / maximum jerk
float track_scaler_tc = 1.0f; float track_scaler_tc = 1.0f;

6
libraries/AR_WPNav/AR_WPNav.h
View File

@ -88,6 +88,11 @@ public:
// settor to allow vehicle code to provide turn related param values to this library (should be updated regularly) // settor to allow vehicle code to provide turn related param values to this library (should be updated regularly)
void set_turn_params(float turn_radius, bool pivot_possible); void set_turn_params(float turn_radius, bool pivot_possible);
// enable speeding up position target to catch-up with vehicles travelling faster than WP_SPEED
// designed to support sailboats that do not have precise speed control
// only supported when using SCurves and not when using position controller input shaping
void enable_overspeed(bool enable) { _overspeed_enabled = enable; }
// accessors for parameter values // accessors for parameter values
float get_default_speed() const { return _speed_max; } float get_default_speed() const { return _speed_max; }
float get_radius() const { return _radius; } float get_radius() const { return _radius; }
@ -160,6 +165,7 @@ protected:
float _scurve_jerk; // scurve jerk max in m/s/s/s float _scurve_jerk; // scurve jerk max in m/s/s/s
bool _fast_waypoint; // true if vehicle will stop at the next waypoint bool _fast_waypoint; // true if vehicle will stop at the next waypoint
bool _pivot_at_next_wp; // true if vehicle should pivot at next waypoint bool _pivot_at_next_wp; // true if vehicle should pivot at next waypoint
bool _overspeed_enabled; // if true scurve's position target will speedup to catch vehicles travelling faster than WP_SPEED
float _track_scalar_dt; // time scaler to ensure scurve target doesn't get too far ahead of vehicle float _track_scalar_dt; // time scaler to ensure scurve target doesn't get too far ahead of vehicle
// variables held in vehicle code (for now) // variables held in vehicle code (for now)