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

AC_WP_Nav: Convert S-Curves to use maximum Snap to remove minimum time between waypoints

This commit is contained in:
Leonard Hall 2022-03-03 09:23:11 +10:30 committed by Randy Mackay
parent 7d9c7a8780
commit c16e736bc6
2 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -177,7 +177,7 @@ void AC_WPNav::wp_and_spline_init(float speed_cms)
if (!is_positive(_wp_jerk)) { if (!is_positive(_wp_jerk)) {
_wp_jerk = _wp_accel_cmss; _wp_jerk = _wp_accel_cmss;
} }
calc_scurve_jerk_and_jerk_time(); calc_scurve_jerk_and_snap();
_scurve_prev_leg.init(); _scurve_prev_leg.init();
_scurve_this_leg.init(); _scurve_this_leg.init();
@ -334,7 +334,7 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt)
_scurve_this_leg.calculate_track(_origin, _destination, _scurve_this_leg.calculate_track(_origin, _destination,
_pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(), _pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(),
_wp_accel_cmss, _wp_accel_z_cmss, _wp_accel_cmss, _wp_accel_z_cmss,
_scurve_jerk_time, _scurve_jerk * 100.0f); _scurve_snap * 100.0f, _scurve_jerk * 100.0f);
if (!is_zero(origin_speed)) { if (!is_zero(origin_speed)) {
// rebuild start of scurve if we have a non-zero origin speed // rebuild start of scurve if we have a non-zero origin speed
_scurve_this_leg.set_origin_speed_max(origin_speed); _scurve_this_leg.set_origin_speed_max(origin_speed);
@ -362,7 +362,7 @@ bool AC_WPNav::set_wp_destination_next(const Vector3f& destination, bool terrain
_scurve_next_leg.calculate_track(_destination, destination, _scurve_next_leg.calculate_track(_destination, destination,
_pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(), _pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(),
_wp_accel_cmss, _wp_accel_z_cmss, _wp_accel_cmss, _wp_accel_z_cmss,
_scurve_jerk_time, _scurve_jerk * 100.0f); _scurve_snap * 100.0f, _scurve_jerk * 100.0);
if (_this_leg_is_spline) { if (_this_leg_is_spline) {
const float this_leg_dest_speed_max = _spline_this_leg.get_destination_speed_max(); const float this_leg_dest_speed_max = _spline_this_leg.get_destination_speed_max();
const float next_leg_origin_speed_max = _scurve_next_leg.set_origin_speed_max(this_leg_dest_speed_max); const float next_leg_origin_speed_max = _scurve_next_leg.set_origin_speed_max(this_leg_dest_speed_max);
@ -857,8 +857,8 @@ bool AC_WPNav::get_vector_NEU(const Location &loc, Vector3f &vec, bool &terrain_
} }
// helper function to calculate scurve jerk and jerk_time values // helper function to calculate scurve jerk and jerk_time values
// updates _scurve_jerk and _scurve_jerk_time // updates _scurve_jerk and _scurve_snap
void AC_WPNav::calc_scurve_jerk_and_jerk_time() void AC_WPNav::calc_scurve_jerk_and_snap()
{ {
// calculate jerk // calculate jerk
_scurve_jerk = MIN(_attitude_control.get_ang_vel_roll_max_rads() * GRAVITY_MSS, _attitude_control.get_ang_vel_pitch_max_rads() * GRAVITY_MSS); _scurve_jerk = MIN(_attitude_control.get_ang_vel_roll_max_rads() * GRAVITY_MSS, _attitude_control.get_ang_vel_pitch_max_rads() * GRAVITY_MSS);
@ -868,14 +868,14 @@ void AC_WPNav::calc_scurve_jerk_and_jerk_time()
_scurve_jerk = MIN(_scurve_jerk, _wp_jerk); _scurve_jerk = MIN(_scurve_jerk, _wp_jerk);
} }
// calculate jerk time // calculate maximum snap
// Jounce (the rate of change of jerk) uses the attitude control input time constant because multicopters // Snap (the rate of change of jerk) uses the attitude control input time constant because multicopters
// lean to accelerate. This means the change in angle is equivalent to the change in acceleration // lean to accelerate. This means the change in angle is equivalent to the change in acceleration
const float jounce = MIN(_attitude_control.get_accel_roll_max_radss() * GRAVITY_MSS, _attitude_control.get_accel_pitch_max_radss() * GRAVITY_MSS); _scurve_snap = (_scurve_jerk * M_PI) / (2.0 * MAX(_attitude_control.get_input_tc(), 0.1f));
if (is_positive(jounce)) { const float snap = MIN(_attitude_control.get_accel_roll_max_radss(), _attitude_control.get_accel_pitch_max_radss()) * GRAVITY_MSS;
_scurve_jerk_time = MAX(_attitude_control.get_input_tc(), 0.5f * _scurve_jerk * M_PI / jounce); if (is_positive(snap)) {
} else { _scurve_snap = MIN(_scurve_snap, snap);
_scurve_jerk_time = MAX(_attitude_control.get_input_tc(), 0.1f);
} }
_scurve_jerk_time *= 2.0f; // reduce maximum snap by a factor of two from what the aircraft is capable of
_scurve_snap *= 0.5;
} }

6
libraries/AC_WPNav/AC_WPNav.h
View File

@ -213,8 +213,8 @@ protected:
} _flags; } _flags;
// helper function to calculate scurve jerk and jerk_time values // helper function to calculate scurve jerk and jerk_time values
// updates _scurve_jerk and _scurve_jerk_time // updates _scurve_jerk and _scurve_snap
void calc_scurve_jerk_and_jerk_time(); void calc_scurve_jerk_and_snap();
// references and pointers to external libraries // references and pointers to external libraries
const AP_InertialNav& _inav; const AP_InertialNav& _inav;
@ -241,7 +241,7 @@ protected:
SCurve _scurve_this_leg; // current scurve trajectory SCurve _scurve_this_leg; // current scurve trajectory
SCurve _scurve_next_leg; // next scurve trajectory used to blend with current scurve trajectory SCurve _scurve_next_leg; // next scurve trajectory used to blend with current scurve trajectory
float _scurve_jerk; // scurve jerk max in m/s/s/s float _scurve_jerk; // scurve jerk max in m/s/s/s
float _scurve_jerk_time; // scurve jerk time (time in seconds for jerk to increase from zero _scurve_jerk) float _scurve_snap; // scurve snap in m/s/s/s/s
// spline curves // spline curves
SplineCurve _spline_this_leg; // spline curve for current segment SplineCurve _spline_this_leg; // spline curve for current segment