From c16e736bc6fc0457a4c8a386a63f1e04d592c9a7 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 3 Mar 2022 09:23:11 +1030 Subject: [PATCH] AC_WP_Nav: Convert S-Curves to use maximum Snap to remove minimum time between waypoints --- libraries/AC_WPNav/AC_WPNav.cpp | 26 +++++++++++++------------- libraries/AC_WPNav/AC_WPNav.h | 6 +++--- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 2fb251efd2..2854d38be5 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -177,7 +177,7 @@ void AC_WPNav::wp_and_spline_init(float speed_cms) if (!is_positive(_wp_jerk)) { _wp_jerk = _wp_accel_cmss; } - calc_scurve_jerk_and_jerk_time(); + calc_scurve_jerk_and_snap(); _scurve_prev_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, _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, - _scurve_jerk_time, _scurve_jerk * 100.0f); + _scurve_snap * 100.0f, _scurve_jerk * 100.0f); if (!is_zero(origin_speed)) { // rebuild start of scurve if we have a non-zero 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, _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, - _scurve_jerk_time, _scurve_jerk * 100.0f); + _scurve_snap * 100.0f, _scurve_jerk * 100.0); if (_this_leg_is_spline) { 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); @@ -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 -// updates _scurve_jerk and _scurve_jerk_time -void AC_WPNav::calc_scurve_jerk_and_jerk_time() +// updates _scurve_jerk and _scurve_snap +void AC_WPNav::calc_scurve_jerk_and_snap() { // 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); @@ -868,14 +868,14 @@ void AC_WPNav::calc_scurve_jerk_and_jerk_time() _scurve_jerk = MIN(_scurve_jerk, _wp_jerk); } - // calculate jerk time - // Jounce (the rate of change of jerk) uses the attitude control input time constant because multicopters + // calculate maximum snap + // 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 - const float jounce = MIN(_attitude_control.get_accel_roll_max_radss() * GRAVITY_MSS, _attitude_control.get_accel_pitch_max_radss() * GRAVITY_MSS); - if (is_positive(jounce)) { - _scurve_jerk_time = MAX(_attitude_control.get_input_tc(), 0.5f * _scurve_jerk * M_PI / jounce); - } else { - _scurve_jerk_time = MAX(_attitude_control.get_input_tc(), 0.1f); + _scurve_snap = (_scurve_jerk * M_PI) / (2.0 * MAX(_attitude_control.get_input_tc(), 0.1f)); + const float snap = MIN(_attitude_control.get_accel_roll_max_radss(), _attitude_control.get_accel_pitch_max_radss()) * GRAVITY_MSS; + if (is_positive(snap)) { + _scurve_snap = MIN(_scurve_snap, snap); } - _scurve_jerk_time *= 2.0f; + // reduce maximum snap by a factor of two from what the aircraft is capable of + _scurve_snap *= 0.5; } diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 4a4432aff8..f2c050566b 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -213,8 +213,8 @@ protected: } _flags; // helper function to calculate scurve jerk and jerk_time values - // updates _scurve_jerk and _scurve_jerk_time - void calc_scurve_jerk_and_jerk_time(); + // updates _scurve_jerk and _scurve_snap + void calc_scurve_jerk_and_snap(); // references and pointers to external libraries const AP_InertialNav& _inav; @@ -241,7 +241,7 @@ protected: SCurve _scurve_this_leg; // 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_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 SplineCurve _spline_this_leg; // spline curve for current segment