From af7403f4e99fe9a727611d5482992437cc5eb581 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 4 Jan 2020 16:33:08 +1030 Subject: [PATCH] AP_Math: add SCurve library SCurve const more local variables rename update to get_jerk_vel_pos_at_time removed unused update debug output in SITL fixes to ensure finished at end of path fixes including validity check includes corrections from peer review --- libraries/AP_Math/SCurve.cpp | 971 +++++++++++++++++++++++++++++++++++ libraries/AP_Math/SCurve.h | 211 ++++++++ 2 files changed, 1182 insertions(+) create mode 100644 libraries/AP_Math/SCurve.cpp create mode 100644 libraries/AP_Math/SCurve.h diff --git a/libraries/AP_Math/SCurve.cpp b/libraries/AP_Math/SCurve.cpp new file mode 100644 index 0000000000..2a1506be46 --- /dev/null +++ b/libraries/AP_Math/SCurve.cpp @@ -0,0 +1,971 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include +#include +#include +#include "SCurve.h" + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#endif + +extern const AP_HAL::HAL &hal; + +#define SEG_INIT 0 +#define SEG_ACCEL_MAX 4 +#define SEG_ACCEL_END 7 +#define SEG_SPEED_CHANGE_END 14 +#define SEG_CONST 15 +#define SEG_DECEL_END 22 + +// constructor +SCurve::SCurve() +{ + init(); +} + +// initialise and clear the path +void SCurve::init() +{ + jerk_time = 0.0f; + jerk_max = 0.0f; + accel_max = 0.0f; + vel_max = 0.0f; + time = 0.0f; + num_segs = SEG_INIT; + add_segment(num_segs, 0.0f, SegmentType::CONSTANT_JERK, 0.0f, 0.0f, 0.0f, 0.0f); + track.zero(); + delta_unit.zero(); + position_sq = 0.0f; +} + +// generate a trigonometric track in 3D space that moves over a straight line +// between two points defined by the origin and destination +void SCurve::calculate_track(const Vector3f &origin, const Vector3f &destination, + float speed_xy, float speed_up, float speed_down, + float accel_xy, float accel_z, + float jerk_time_sec, float jerk_maximum) +{ + init(); + + // leave track as zero length if origin and destination are the same + if (origin == destination) { + return; + } + + // set jerk time and jerk max + jerk_time = jerk_time_sec; + jerk_max = jerk_maximum; + + // update speed and acceleration limits along path + set_kinematic_limits(origin, destination, + speed_xy, speed_up, speed_down, + accel_xy, accel_z); + + // avoid divide-by zeros. Path will be left as a zero length path + if (!is_positive(jerk_time) || !is_positive(jerk_max) || !is_positive(accel_max) || !is_positive(vel_max)) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + ::printf("SCurve::calculate_track created zero length path\n"); +#endif + INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result); + return; + } + + track = destination - origin; + const float track_length = track.length(); + if (is_zero(track_length)) { + // avoid possible divide by zero + delta_unit.zero(); + } else { + delta_unit = track.normalized(); + add_segments(track_length); + } + + // catch calculation errors + if (!valid()) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + ::printf("SCurve::calculate_track invalid path\n"); + debug(); +#endif + INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result); + init(); + } +} + +// set maximum velocity and re-calculate the path using these limits +void SCurve::set_speed_max(float speed_xy, float speed_up, float speed_down) +{ + // return immediately if zero length path + if (num_segs != segments_max) { + return; + } + + // segment accelerations can not be changed after segment creation. + const float track_speed_max = kinematic_limit(delta_unit, speed_xy, speed_up, fabsf(speed_down)); + + if (is_equal(vel_max, track_speed_max)) { + // new speed is equal to current speed maximum so no need to change anything + return; + } + + if (is_zero(track_speed_max)) { + // new speed is zero which is not supported + return; + } + vel_max = track_speed_max; + + if (time >= segment[SEG_CONST].end_time) { + return; + } + + // re-calculate the s-curve path based on update speeds + + const float Pend = segment[SEG_DECEL_END].end_pos; + float Vend = MIN(vel_max, segment[SEG_DECEL_END].end_vel); + + if (is_zero(time)) { + // path has not started so we can recompute the path + const float Vstart = MIN(vel_max, segment[SEG_INIT].end_vel); + num_segs = SEG_INIT; + add_segment(num_segs, 0.0f, SegmentType::CONSTANT_JERK, 0.0f, 0.0f, 0.0f, 0.0f); + add_segments(Pend); + set_origin_speed_max(Vstart); + set_destination_speed_max(Vend); + return; + } + + if ((time >= segment[SEG_ACCEL_END].end_time) && (time <= segment[SEG_SPEED_CHANGE_END].end_time)) { + // in the speed change phase + // move speed change phase to acceleration phase to provide room for further speed adjustments + + // set initial segment to last acceleration segment + segment[SEG_INIT].seg_type = SegmentType::CONSTANT_JERK; + segment[SEG_INIT].jerk_ref = 0.0f; + segment[SEG_INIT].end_time = segment[SEG_ACCEL_END].end_time; + segment[SEG_INIT].end_accel = segment[SEG_ACCEL_END].end_accel; + segment[SEG_INIT].end_vel = segment[SEG_ACCEL_END].end_vel; + segment[SEG_INIT].end_pos = segment[SEG_ACCEL_END].end_pos; + + // move speed change segments to acceleration segments + for (uint8_t i = SEG_INIT+1; i <= SEG_ACCEL_END; i++) { + segment[i] = segment[i+7]; + } + + // set change segments to last acceleration speed + for (uint8_t i = SEG_ACCEL_END+1; i <= SEG_SPEED_CHANGE_END; i++) { + segment[i].seg_type = SegmentType::CONSTANT_JERK; + segment[i].jerk_ref = 0.0f; + segment[i].end_time = segment[SEG_ACCEL_END].end_time; + segment[i].end_accel = 0.0f; + segment[i].end_vel = segment[SEG_ACCEL_END].end_vel; + segment[i].end_pos = segment[SEG_ACCEL_END].end_pos; + } + + } else if ((time > segment[SEG_SPEED_CHANGE_END].end_time) && (time <= segment[SEG_CONST].end_time)) { + // in the constant speed phase + // overwrite the acceleration and speed change phases with the current position and velocity + + // set initial segment to last acceleration segment + segment[SEG_INIT].seg_type = SegmentType::CONSTANT_JERK; + segment[SEG_INIT].jerk_ref = 0.0f; + segment[SEG_INIT].end_time = segment[SEG_SPEED_CHANGE_END].end_time; + segment[SEG_INIT].end_accel = 0.0f; + segment[SEG_INIT].end_vel = segment[SEG_SPEED_CHANGE_END].end_vel; + segment[SEG_INIT].end_pos = segment[SEG_SPEED_CHANGE_END].end_pos; + + // set acceleration and change segments to current constant speed + float Jt_out, At_out, Vt_out, Pt_out; + get_jerk_accel_vel_pos_at_time(time, Jt_out, At_out, Vt_out, Pt_out); + for (uint8_t i = SEG_INIT+1; i <= SEG_SPEED_CHANGE_END; i++) { + segment[i].seg_type = SegmentType::CONSTANT_JERK; + segment[i].jerk_ref = 0.0f; + segment[i].end_time = time; + segment[i].end_accel = 0.0f; + segment[i].end_vel = Vt_out; + segment[i].end_pos = Pt_out; + } + } + + // adjust the INIT and ACCEL segments for new speed + if ((time <= segment[SEG_ACCEL_MAX].end_time) && is_positive(segment[SEG_ACCEL_MAX].end_time - segment[SEG_ACCEL_MAX-1].end_time) && (vel_max < segment[SEG_ACCEL_END].end_vel) && is_positive(segment[SEG_ACCEL_MAX].end_accel) ) { + // path has not finished constant positive acceleration segment + // reduce velocity as close to target velocity as possible + + const float Vstart = segment[SEG_INIT].end_vel; + + // minimum velocity that can be obtained by shortening SEG_ACCEL_MAX + const float Vmin = segment[SEG_ACCEL_END].end_vel - segment[SEG_ACCEL_MAX].end_accel * (segment[SEG_ACCEL_MAX].end_time - MAX(time, segment[SEG_ACCEL_MAX-1].end_time)); + + float Jm, t2, t4, t6; + calculate_path(jerk_time, jerk_max, Vstart, accel_max, MAX(Vmin, vel_max), Pend * 0.5f, Jm, t2, t4, t6); + + uint8_t seg = SEG_INIT+1; + add_segments_jerk(seg, jerk_time, Jm, t2); + add_segment_const_jerk(seg, t4, 0.0f); + add_segments_jerk(seg, jerk_time, -Jm, t6); + + // remove numerical errors + segment[SEG_ACCEL_END].end_accel = 0.0f; + + // add empty speed adjust segments + for (uint8_t i = SEG_ACCEL_END+1; i <= SEG_CONST; i++) { + segment[i].seg_type = SegmentType::CONSTANT_JERK; + segment[i].jerk_ref = 0.0f; + segment[i].end_time = segment[SEG_ACCEL_END].end_time; + segment[i].end_accel = 0.0f; + segment[i].end_vel = segment[SEG_ACCEL_END].end_vel; + segment[i].end_pos = segment[SEG_ACCEL_END].end_pos; + } + + calculate_path(jerk_time, jerk_max, 0.0f, accel_max, MAX(Vmin, vel_max), Pend * 0.5f, Jm, t2, t4, t6); + + seg = SEG_CONST + 1; + add_segments_jerk(seg, jerk_time, -Jm, t6); + add_segment_const_jerk(seg, t4, 0.0f); + add_segments_jerk(seg, jerk_time, Jm, t2); + + // remove numerical errors + segment[SEG_DECEL_END].end_accel = 0.0f; + segment[SEG_DECEL_END].end_vel = MAX(0.0f, segment[SEG_DECEL_END].end_vel); + + // add to constant velocity segment to end at the correct position + const float dP = MAX(0.0f, Pend - segment[SEG_DECEL_END].end_pos); + const float t15 = dP / segment[SEG_CONST].end_vel; + for (uint8_t i = SEG_CONST; i <= SEG_DECEL_END; i++) { + segment[i].end_time += t15; + segment[i].end_pos += dP; + } + } + + // adjust the speed change segments (8 to 14) for new speed + // start with empty speed adjust segments + for (uint8_t i = SEG_ACCEL_END+1; i <= SEG_SPEED_CHANGE_END; i++) { + segment[i].seg_type = SegmentType::CONSTANT_JERK; + segment[i].jerk_ref = 0.0f; + segment[i].end_time = segment[SEG_ACCEL_END].end_time; + segment[i].end_accel = 0.0f; + segment[i].end_vel = segment[SEG_ACCEL_END].end_vel; + segment[i].end_pos = segment[SEG_ACCEL_END].end_pos; + } + if (!is_equal(vel_max, segment[SEG_ACCEL_END].end_vel)) { + // add velocity adjustment + // check there is enough time to make velocity change + // we use the approximation that the time will be distance/max_vel and 8 jerk segments + const float L = segment[SEG_CONST].end_pos - segment[SEG_ACCEL_END].end_pos; + float Jm = 0; + float t2 = 0; + float t4 = 0; + float t6 = 0; + if ((vel_max < segment[SEG_ACCEL_END].end_vel) && (jerk_time*12.0f < L/segment[SEG_ACCEL_END].end_vel)) { + // we have a problem here with small segments. + calculate_path(jerk_time, jerk_max, vel_max, accel_max, segment[SEG_ACCEL_END].end_vel, L * 0.5f, Jm, t6, t4, t2); + Jm = -Jm; + + } else if ((vel_max > segment[SEG_ACCEL_END].end_vel) && (L/(jerk_time*12.0f) > segment[SEG_ACCEL_END].end_vel)) { + float Vm = MIN(vel_max, L/(jerk_time*12.0f)); + calculate_path(jerk_time, jerk_max, segment[SEG_ACCEL_END].end_vel, accel_max, Vm, L * 0.5f, Jm, t2, t4, t6); + } + + uint8_t seg = SEG_ACCEL_END + 1; + if (!is_zero(Jm) && !is_negative(t2) && !is_negative(t4) && !is_negative(t6)) { + add_segments_jerk(seg, jerk_time, Jm, t2); + add_segment_const_jerk(seg, t4, 0.0f); + add_segments_jerk(seg, jerk_time, -Jm, t6); + + // remove numerical errors + segment[SEG_SPEED_CHANGE_END].end_accel = 0.0f; + } + } + + // add deceleration segments + // earlier check should ensure that we should always have sufficient time to stop + uint8_t seg = SEG_CONST; + Vend = MIN(Vend, segment[SEG_SPEED_CHANGE_END].end_vel); + add_segment_const_jerk(seg, 0.0f, 0.0f); + if (Vend < segment[SEG_SPEED_CHANGE_END].end_vel) { + float Jm, t2, t4, t6; + calculate_path(jerk_time, jerk_max, Vend, accel_max, segment[SEG_CONST].end_vel, Pend - segment[SEG_CONST].end_pos, Jm, t2, t4, t6); + add_segments_jerk(seg, jerk_time, -Jm, t6); + add_segment_const_jerk(seg, t4, 0.0f); + add_segments_jerk(seg, jerk_time, Jm, t2); + } else { + // No deceleration is required + for (uint8_t i = SEG_CONST+1; i <= SEG_DECEL_END; i++) { + segment[i].seg_type = SegmentType::CONSTANT_JERK; + segment[i].jerk_ref = 0.0f; + segment[i].end_time = segment[SEG_CONST].end_time; + segment[i].end_accel = 0.0f; + segment[i].end_vel = segment[SEG_CONST].end_vel; + segment[i].end_pos = segment[SEG_CONST].end_pos; + } + } + + // remove numerical errors + segment[SEG_DECEL_END].end_accel = 0.0f; + segment[SEG_DECEL_END].end_vel = MAX(0.0f, segment[SEG_DECEL_END].end_vel); + + // add to constant velocity segment to end at the correct position + const float dP = MAX(0.0f, Pend - segment[SEG_DECEL_END].end_pos); + const float t15 = dP / segment[SEG_CONST].end_vel; + for (uint8_t i = SEG_CONST; i <= SEG_DECEL_END; i++) { + segment[i].end_time += t15; + segment[i].end_pos += dP; + } + + // catch calculation errors + if (!valid()) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + ::printf("SCurve::set_speed_max invalid path\n"); + debug(); +#endif + INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result); + init(); + } +} + +// set the maximum vehicle speed at the origin +// returns the expected speed at the origin which will always be equal or lower than speed +float SCurve::set_origin_speed_max(float speed) +{ + // if path is zero length then start speed must be zero + if (num_segs != segments_max) { + return 0.0f; + } + + // avoid re-calculating if unnecessary + if (is_equal(segment[SEG_INIT].end_vel, speed)) { + return speed; + } + + const float Vm = segment[SEG_ACCEL_END].end_vel; + const float track_length = track.length(); + speed = MIN(speed, Vm); + + float Jm, t2, t4, t6; + calculate_path(jerk_time, jerk_max, speed, accel_max, Vm, track_length * 0.5f, Jm, t2, t4, t6); + + uint8_t seg = SEG_INIT; + add_segment(seg, 0.0f, SegmentType::CONSTANT_JERK, 0.0f, 0.0f, speed, 0.0f); + add_segments_jerk(seg, jerk_time, Jm, t2); + add_segment_const_jerk(seg, t4, 0.0f); + add_segments_jerk(seg, jerk_time, -Jm, t6); + + // remove numerical errors + segment[SEG_ACCEL_END].end_accel = 0.0f; + + // offset acceleration segment if we can't fit it all into half the original length + const float dPstart = MIN(0.0f, track_length * 0.5f - segment[SEG_ACCEL_END].end_pos); + const float dt = dPstart / segment[SEG_ACCEL_END].end_vel; + for (uint8_t i = SEG_INIT; i <= SEG_ACCEL_END; i++) { + segment[i].end_time += dt; + segment[i].end_pos += dPstart; + } + + // add empty speed change segments and constant speed segment + for (uint8_t i = SEG_ACCEL_END+1; i <= SEG_SPEED_CHANGE_END; i++) { + segment[i].seg_type = SegmentType::CONSTANT_JERK; + segment[i].jerk_ref = 0.0f; + segment[i].end_time = segment[SEG_ACCEL_END].end_time; + segment[i].end_accel = 0.0f; + segment[i].end_vel = segment[SEG_ACCEL_END].end_vel; + segment[i].end_pos = segment[SEG_ACCEL_END].end_pos; + } + + seg = SEG_CONST; + add_segment_const_jerk(seg, 0.0f, 0.0f); + + calculate_path(jerk_time, jerk_max, 0.0f, accel_max, segment[SEG_CONST].end_vel, track_length * 0.5f, Jm, t2, t4, t6); + + add_segments_jerk(seg, jerk_time, -Jm, t6); + add_segment_const_jerk(seg, t4, 0.0f); + add_segments_jerk(seg, jerk_time, Jm, t2); + + // remove numerical errors + segment[SEG_DECEL_END].end_accel = 0.0f; + segment[SEG_DECEL_END].end_vel = MAX(0.0f, segment[SEG_DECEL_END].end_vel); + + // add to constant velocity segment to end at the correct position + const float dP = MAX(0.0f, track_length - segment[SEG_DECEL_END].end_pos); + const float t15 = dP / segment[SEG_CONST].end_vel; + for (uint8_t i = SEG_CONST; i <= SEG_DECEL_END; i++) { + segment[i].end_time += t15; + segment[i].end_pos += dP; + } + + // catch calculation errors + if (!valid()) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + ::printf("SCurve::set_origin_speed_max invalid path\n"); + debug(); +#endif + INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result); + init(); + return 0.0f; + } + + return speed; +} + +// set the maximum vehicle speed at the destination +void SCurve::set_destination_speed_max(float speed) +{ + // if path is zero length then all speeds must be zero + if (num_segs != segments_max) { + return; + } + + // avoid re-calculating if unnecessary + if (is_equal(segment[segments_max-1].end_vel, speed)) { + return; + } + + const float Vm = segment[SEG_CONST].end_vel; + const float track_length = track.length(); + speed = MIN(speed, Vm); + + float Jm, t2, t4, t6; + calculate_path(jerk_time, jerk_max, speed, accel_max, Vm, track_length * 0.5f, Jm, t2, t4, t6); + + uint8_t seg = SEG_CONST; + add_segment_const_jerk(seg, 0.0f, 0.0f); + + add_segments_jerk(seg, jerk_time, -Jm, t6); + add_segment_const_jerk(seg, t4, 0.0f); + add_segments_jerk(seg, jerk_time, Jm, t2); + + // remove numerical errors + segment[SEG_DECEL_END].end_accel = 0.0f; + segment[SEG_DECEL_END].end_vel = MAX(0.0f, segment[SEG_DECEL_END].end_vel); + + // add to constant velocity segment to end at the correct position + const float dP = MAX(0.0f, track_length - segment[SEG_DECEL_END].end_pos); + const float t15 = dP / segment[SEG_CONST].end_vel; + for (uint8_t i = SEG_CONST; i <= SEG_DECEL_END; i++) { + segment[i].end_time += t15; + segment[i].end_pos += dP; + } + + // catch calculation errors + if (!valid()) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + ::printf("SCurve::set_destination_speed_max invalid path\n"); + debug(); +#endif + INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result); + init(); + } +} + +// move target location along path from origin to destination +// prev_leg and next_leg are the paths before and after this path +// wp_radius is max distance from the waypoint at the apex of the turn +// fast_waypoint should be true if vehicle will not stop at end of this leg +// dt is the time increment the vehicle will move along the path +// target_pos should be set to this segment's origin and it will be updated to the current position target +// target_vel and target_accel are updated with new targets +// returns true if vehicle has passed the apex of the corner +bool SCurve::advance_target_along_track(SCurve &prev_leg, SCurve &next_leg, float wp_radius, bool fast_waypoint, float dt, Vector3f &target_pos, Vector3f &target_vel, Vector3f &target_accel) +{ + prev_leg.move_to_pos_vel_accel(dt, target_pos, target_vel, target_accel); + move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel); + bool s_finished = finished(); + + // check for change of leg on fast waypoint + const float time_to_destination = get_time_remaining(); + if (fast_waypoint && braking() && is_zero(next_leg.get_time_elapsed()) && (time_to_destination <= next_leg.get_accel_finished_time())) { + Vector3f turn_pos = -get_track(); + Vector3f turn_vel, turn_accel; + move_from_time_pos_vel_accel(get_time_elapsed() + time_to_destination * 0.5f, turn_pos, turn_vel, turn_accel); + next_leg.move_from_time_pos_vel_accel(time_to_destination * 0.5f, turn_pos, turn_vel, turn_accel); + const float speed_min = MIN(get_speed_along_track(), next_leg.get_speed_along_track()); + const float accel_min = MIN(get_accel_along_track(), next_leg.get_accel_along_track()); + if ((get_time_remaining() < next_leg.time_end() * 0.5f) && (turn_pos.length() < wp_radius) && + (Vector2f(turn_vel.x, turn_vel.y).length() < speed_min) && + (Vector2f(turn_accel.x, turn_accel.y).length() < 2.0f*accel_min)) { + next_leg.move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel); + } + } else if (!is_zero(next_leg.get_time_elapsed())) { + next_leg.move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel); + if (next_leg.get_time_elapsed() >= get_time_remaining()) { + s_finished = true; + } + } + + return s_finished; +} + +// time has reached the end of the sequence +bool SCurve::finished() const +{ + return ((time >= time_end()) || (position_sq >= track.length_squared())); +} + +// increment time pointer and return the position, velocity and acceleration vectors relative to the origin +void SCurve::move_from_pos_vel_accel(float dt, Vector3f &pos, Vector3f &vel, Vector3f &accel) +{ + advance_time(dt); + float scurve_P1 = 0.0f; + float scurve_V1, scurve_A1, scurve_J1; + get_jerk_accel_vel_pos_at_time(time, scurve_J1, scurve_A1, scurve_V1, scurve_P1); + pos += delta_unit * scurve_P1; + vel += delta_unit * scurve_V1; + accel += delta_unit * scurve_A1; + position_sq = sq(scurve_P1); +} + +// increment time pointer and return the position, velocity and acceleration vectors relative to the destination +void SCurve::move_to_pos_vel_accel(float dt, Vector3f &pos, Vector3f &vel, Vector3f &accel) +{ + advance_time(dt); + float scurve_P1 = 0.0f; + float scurve_V1, scurve_A1, scurve_J1; + get_jerk_accel_vel_pos_at_time(time, scurve_J1, scurve_A1, scurve_V1, scurve_P1); + pos += delta_unit * scurve_P1; + vel += delta_unit * scurve_V1; + accel += delta_unit * scurve_A1; + position_sq = sq(scurve_P1); + pos -= track; +} + +// return the position, velocity and acceleration vectors relative to the origin at a specified time along the path +void SCurve::move_from_time_pos_vel_accel(float time_now, Vector3f &pos, Vector3f &vel, Vector3f &accel) +{ + float scurve_P1 = 0.0f; + float scurve_V1, scurve_A1, scurve_J1; + get_jerk_accel_vel_pos_at_time(time_now, scurve_J1, scurve_A1, scurve_V1, scurve_P1); + pos += delta_unit * scurve_P1; + vel += delta_unit * scurve_V1; + accel += delta_unit * scurve_A1; +} + +// time at the end of the sequence +float SCurve::time_end() const +{ + if (num_segs != segments_max) { + return 0.0f; + } + return segment[SEG_DECEL_END].end_time; +} + +// time left before sequence will complete +float SCurve::get_time_remaining() const +{ + if (num_segs != segments_max) { + return 0.0f; + } + return segment[SEG_DECEL_END].end_time - time; +} + +// time when acceleration section of the sequence will complete +float SCurve::get_accel_finished_time() const +{ + if (num_segs != segments_max) { + return 0.0f; + } + return segment[SEG_ACCEL_END].end_time; +} + +// return true if the sequence is braking to a stop +bool SCurve::braking() const +{ + if (num_segs != segments_max) { + return true; + } + return time >= segment[SEG_CONST].end_time; +} + +// increment the internal time +void SCurve::advance_time(float dt) +{ + time = MIN(time+dt, time_end()); +} + +// calculate the jerk, acceleration, velocity and position at the provided time +void SCurve::get_jerk_accel_vel_pos_at_time(float time_now, float &Jt_out, float &At_out, float &Vt_out, float &Pt_out) const +{ + if ((num_segs != segments_max) || !is_positive(jerk_time)) { + Jt_out = 0; + At_out = 0; + Vt_out = 0; + Pt_out = 0; + return; + } + + SegmentType Jtype; + uint8_t pnt = num_segs; + float Jm, T0, A0, V0, P0; + + // find active segment at time_now + for (uint8_t i = 0; i < num_segs; i++) { + if (time_now < segment[num_segs - 1 - i].end_time) { + pnt = num_segs - 1 - i; + } + } + if (pnt == 0) { + Jtype = SegmentType::CONSTANT_JERK; + Jm = 0.0f; + T0 = segment[pnt].end_time; + A0 = segment[pnt].end_accel; + V0 = segment[pnt].end_vel; + P0 = segment[pnt].end_pos; + } else if (pnt == num_segs) { + Jtype = SegmentType::CONSTANT_JERK; + Jm = 0.0f; + T0 = segment[pnt - 1].end_time; + A0 = segment[pnt - 1].end_accel; + V0 = segment[pnt - 1].end_vel; + P0 = segment[pnt - 1].end_pos; + } else { + Jtype = segment[pnt].seg_type; + Jm = segment[pnt].jerk_ref; + T0 = segment[pnt - 1].end_time; + A0 = segment[pnt - 1].end_accel; + V0 = segment[pnt - 1].end_vel; + P0 = segment[pnt - 1].end_pos; + } + + switch (Jtype) { + case SegmentType::CONSTANT_JERK: + calc_javp_for_segment_const_jerk(time_now - T0, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out); + break; + case SegmentType::POSITIVE_JERK: + calc_javp_for_segment_incr_jerk(time_now - T0, jerk_time, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out); + break; + case SegmentType::NEGATIVE_JERK: + calc_javp_for_segment_decr_jerk(time_now - T0, jerk_time, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out); + break; + } + Pt_out = MAX(0.0f, Pt_out); +} + +// calculate the jerk, acceleration, velocity and position at time time_now when running the constant jerk time segment +void SCurve::calc_javp_for_segment_const_jerk(float time_now, float J0, float A0, float V0, float P0, float &Jt, float &At, float &Vt, float &Pt) const +{ + Jt = J0; + At = A0 + J0 * time_now; + Vt = V0 + A0 * time_now + 0.5f * J0 * (time_now * time_now); + Pt = P0 + V0 * time_now + 0.5f * A0 * (time_now * time_now) + (1.0f / 6.0f) * J0 * (time_now * time_now * time_now); +} + +// Calculate the jerk, acceleration, velocity and position at time time_now when running the increasing jerk magnitude time segment based on a raised cosine profile +void SCurve::calc_javp_for_segment_incr_jerk(float time_now, float tj, float Jm, float A0, float V0, float P0, float &Jt, float &At, float &Vt, float &Pt) const +{ + const float Alpha = Jm * 0.5f; + const float Beta = M_PI / tj; + Jt = Alpha * (1.0f - cosf(Beta * time_now)); + At = A0 + Alpha * time_now - (Alpha / Beta) * sinf(Beta * time_now); + Vt = V0 + A0 * time_now + (Alpha * 0.5f) * (time_now * time_now) + (Alpha / (Beta * Beta)) * cosf(Beta * time_now) - Alpha / (Beta * Beta); + Pt = P0 + V0 * time_now + 0.5f * A0 * (time_now * time_now) + (-Alpha / (Beta * Beta)) * time_now + Alpha * (time_now * time_now * time_now) / 6.0f + (Alpha / (Beta * Beta * Beta)) * sinf(Beta * time_now); +} + +// Calculate the jerk, acceleration, velocity and position at time time_now when running the decreasing jerk magnitude time segment based on a raised cosine profile +void SCurve::calc_javp_for_segment_decr_jerk(float time_now, float tj, float Jm, float A0, float V0, float P0, float &Jt, float &At, float &Vt, float &Pt) const +{ + const float Alpha = Jm * 0.5f; + const float Beta = M_PI / tj; + const float AT = Alpha * tj; + const float VT = Alpha * ((tj * tj) * 0.5f - 2.0f / (Beta * Beta)); + const float PT = Alpha * ((-1.0f / (Beta * Beta)) * tj + (1.0f / 6.0f) * (tj * tj * tj)); + Jt = Alpha * (1.0f - cosf(Beta * (time_now + tj))); + At = (A0 - AT) + Alpha * (time_now + tj) - (Alpha / Beta) * sinf(Beta * (time_now + tj)); + Vt = (V0 - VT) + (A0 - AT) * time_now + 0.5f * Alpha * (time_now + tj) * (time_now + tj) + (Alpha / (Beta * Beta)) * cosf(Beta * (time_now + tj)) - Alpha / (Beta * Beta); + Pt = (P0 - PT) + (V0 - VT) * time_now + 0.5f * (A0 - AT) * (time_now * time_now) + (-Alpha / (Beta * Beta)) * (time_now + tj) + (Alpha / 6.0f) * (time_now + tj) * (time_now + tj) * (time_now + tj) + (Alpha / (Beta * Beta * Beta)) * sinf(Beta * (time_now + tj)); +} + +// generate the segments for a path of length L +// the path consists of 23 segments +// 1 initial segment +// 7 segments forming the acceleration S-Curve +// 7 segments forming the velocity change S-Curve +// 1 constant velocity S-Curve +// 7 segments forming the deceleration S-Curve +void SCurve::add_segments(float L) +{ + if (is_zero(L)) { + return; + } + + float Jm, t2, t4, t6; + calculate_path(jerk_time, jerk_max, 0.0f, accel_max, vel_max, L * 0.5f, Jm, t2, t4, t6); + + add_segments_jerk(num_segs, jerk_time, Jm, t2); + add_segment_const_jerk(num_segs, t4, 0.0f); + add_segments_jerk(num_segs, jerk_time, -Jm, t6); + + // remove numerical errors + segment[SEG_ACCEL_END].end_accel = 0.0f; + + // add empty speed adjust segments + add_segment_const_jerk(num_segs, 0.0f, 0.0f); + add_segment_const_jerk(num_segs, 0.0f, 0.0f); + add_segment_const_jerk(num_segs, 0.0f, 0.0f); + add_segment_const_jerk(num_segs, 0.0f, 0.0f); + add_segment_const_jerk(num_segs, 0.0f, 0.0f); + add_segment_const_jerk(num_segs, 0.0f, 0.0f); + add_segment_const_jerk(num_segs, 0.0f, 0.0f); + + const float t15 = MAX(0.0f, (L - 2.0f * segment[SEG_SPEED_CHANGE_END].end_pos) / segment[SEG_SPEED_CHANGE_END].end_vel); + add_segment_const_jerk(num_segs, t15, 0.0f); + + add_segments_jerk(num_segs, jerk_time, -Jm, t6); + add_segment_const_jerk(num_segs, t4, 0.0f); + add_segments_jerk(num_segs, jerk_time, Jm, t2); + + // remove numerical errors + segment[SEG_DECEL_END].end_accel = 0.0f; + segment[SEG_DECEL_END].end_vel = 0.0f; +} + +// calculate the segment times for the trigonometric S-Curve path defined by: +// tj - duration of the raised cosine jerk profile +// Jm - maximum value of the raised cosine jerk profile +// V0 - initial velocity magnitude +// Am - maximum constant acceleration +// Vm - maximum constant velocity +// L - Length of the path +// t2_out, t4_out, t6_out are the segment durations needed to achieve the kinimatic path specified by the input variables +void SCurve::calculate_path(float tj, float Jm, float V0, float Am, float Vm, float L, float &Jm_out, float &t2_out, float &t4_out, float &t6_out) const +{ + // init outputs + Jm_out = 0.0f; + t2_out = 0.0f; + t4_out = 0.0f; + t6_out = 0.0f; + + // check for invalid arguments + if (!is_positive(tj) || !is_positive(Jm) || !is_positive(Am) || !is_positive(Vm) || !is_positive(L)) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + ::printf("SCurve::calculate_path invalid inputs\n"); +#endif + INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result); + return; + } + + if (V0 >= Vm) { + // no velocity change so all segments as zero length + return; + } + + Am = MIN(MIN(Am, (Vm - V0) / (2.0f * tj)), (L + 4.0f * V0 * tj) / (4.0f * sq(tj))); + if (fabsf(Am) < Jm * tj) { + Jm = Am / tj; + if ((Vm <= V0 + 2.0f * Am * tj) || (L <= 4.0f * V0 * tj + 4.0f * Am * sq(tj))) { + // solution = 0 - t6 t4 t2 = 0 0 0 + t2_out = 0.0f; + t4_out = 0.0f; + t6_out = 0.0f; + } else { + // solution = 2 - t6 t4 t2 = 0 1 0 + t2_out = 0.0f; + t4_out = MIN(-(V0 - Vm + Am * tj + (Am * Am) / Jm) / Am, MAX(((Am * Am) * (-3.0f / 2.0f) + safe_sqrt((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm), ((Am * Am) * (-3.0f / 2.0f) - safe_sqrt((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm))); + t6_out = 0.0f; + } + } else { + if ((Vm < V0 + Am * tj + (Am * Am) / Jm) || (L < 1.0f / (Jm * Jm) * (Am * Am * Am + Am * Jm * (V0 * 2.0f + Am * tj * 2.0f)) + V0 * tj * 2.0f + Am * (tj * tj))) { + // solution = 5 - t6 t4 t2 = 1 0 1 + Am = MIN(MIN(Am, MAX(Jm * (tj + safe_sqrt((V0 * -4.0f + Vm * 4.0f + Jm * (tj * tj)) / Jm)) * (-1.0f / 2.0f), Jm * (tj - safe_sqrt((V0 * -4.0f + Vm * 4.0f + Jm * (tj * tj)) / Jm)) * (-1.0f / 2.0f))), Jm * tj * (-2.0f / 3.0f) + ((Jm * Jm) * (tj * tj) * (1.0f / 9.0f) - Jm * V0 * (2.0f / 3.0f)) * 1.0f / powf(safe_sqrt(powf(- (Jm * Jm) * L * (1.0f / 2.0f) + (Jm * Jm * Jm) * (tj * tj * tj) * (8.0f / 2.7E1f) - Jm * tj * ((Jm * Jm) * (tj * tj) + Jm * V0 * 2.0f) * (1.0f / 3.0f) + (Jm * Jm) * V0 * tj, 2.0f) - powf((Jm * Jm) * (tj * tj) * (1.0f / 9.0f) - Jm * V0 * (2.0f / 3.0f), 3.0f)) + (Jm * Jm) * L * (1.0f / 2.0f) - (Jm * Jm * Jm) * (tj * tj * tj) * (8.0f / 2.7E1f) + Jm * tj * ((Jm * Jm) * (tj * tj) + Jm * V0 * 2.0f) * (1.0f / 3.0f) - (Jm * Jm) * V0 * tj, 1.0f / 3.0f) + powf(safe_sqrt(powf(- (Jm * Jm) * L * (1.0f / 2.0f) + (Jm * Jm * Jm) * (tj * tj * tj) * (8.0f / 2.7E1f) - Jm * tj * ((Jm * Jm) * (tj * tj) + Jm * V0 * 2.0f) * (1.0f / 3.0f) + (Jm * Jm) * V0 * tj, 2.0f) - powf((Jm * Jm) * (tj * tj) * (1.0f / 9.0f) - Jm * V0 * (2.0f / 3.0f), 3.0f)) + (Jm * Jm) * L * (1.0f / 2.0f) - (Jm * Jm * Jm) * (tj * tj * tj) * (8.0f / 2.7E1f) + Jm * tj * ((Jm * Jm) * (tj * tj) + Jm * V0 * 2.0f) * (1.0f / 3.0f) - (Jm * Jm) * V0 * tj, 1.0f / 3.0f)); + t2_out = Am / Jm - tj; + t4_out = 0.0f; + t6_out = t2_out; + } else { + // solution = 7 - t6 t4 t2 = 1 1 1 + t2_out = Am / Jm - tj; + t4_out = MIN(-(V0 - Vm + Am * tj + (Am * Am) / Jm) / Am, MAX(((Am * Am) * (-3.0f / 2.0f) + safe_sqrt((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm), ((Am * Am) * (-3.0f / 2.0f) - safe_sqrt((Am * Am * Am * Am) * (1.0f / 4.0f) + (Jm * Jm) * (V0 * V0) + (Am * Am) * (Jm * Jm) * (tj * tj) * (1.0f / 4.0f) + Am * (Jm * Jm) * L * 2.0f - (Am * Am) * Jm * V0 + (Am * Am * Am) * Jm * tj * (1.0f / 2.0f) - Am * (Jm * Jm) * V0 * tj) - Jm * V0 - Am * Jm * tj * (3.0f / 2.0f)) / (Am * Jm))); + t6_out = t2_out; + } + } + Jm_out = Jm; + + // check outputs and reset back to zero if necessary + if (!isfinite(Jm_out) || is_negative(Jm_out) || + !isfinite(t2_out) || is_negative(t2_out) || + !isfinite(t4_out) || is_negative(t4_out) || + !isfinite(t6_out) || is_negative(t6_out)) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + ::printf("SCurve::calculate_path invalid outputs\n"); +#endif + INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result); + Jm_out = 0.0f; + t2_out = 0.0f; + t4_out = 0.0f; + t6_out = 0.0f; + } +} + +// generate three consecutive segments forming a jerk profile +// the index variable is the position within the path array that this jerk profile should be added +// the index is incremented to reference the next segment in the array after the jerk profile +void SCurve::add_segments_jerk(uint8_t &index, float tj, float Jm, float Tcj) +{ + add_segment_incr_jerk(index, tj, Jm); + add_segment_const_jerk(index, Tcj, Jm); + add_segment_decr_jerk(index, tj, Jm); +} + +// generate constant jerk time segment +// calculate the information needed to populate the constant jerk segment from the segment duration tj and jerk J0 +// the index variable is the position of this segment in the path array and is incremented to reference the next segment in the array +void SCurve::add_segment_const_jerk(uint8_t &index, float tj, float J0) +{ + // if no time increase copy previous segment + if (is_zero(tj)) { + add_segment(index, segment[index - 1].end_time, + SegmentType::CONSTANT_JERK, + J0, + segment[index - 1].end_accel, + segment[index - 1].end_vel, + segment[index - 1].end_pos); + return; + } + + const float J = J0; + const float T = segment[index - 1].end_time + tj; + const float A = segment[index - 1].end_accel + J0 * tj; + const float V = segment[index - 1].end_vel + segment[index - 1].end_accel * tj + 0.5f * J0 * sq(tj); + const float P = segment[index - 1].end_pos + segment[index - 1].end_vel * tj + 0.5f * segment[index - 1].end_accel * sq(tj) + (1.0f / 6.0f) * J0 * powf(tj, 3.0f); + add_segment(index, T, SegmentType::CONSTANT_JERK, J, A, V, P); +} + +// generate increasing jerk magnitude time segment based on a raised cosine profile +// calculate the information needed to populate the increasing jerk magnitude segment from the segment duration tj and jerk magnitude Jm +// the index variable is the position of this segment in the path array and is incremented to reference the next segment in the array +void SCurve::add_segment_incr_jerk(uint8_t &index, float tj, float Jm) +{ + const float Beta = M_PI / tj; + const float Alpha = Jm * 0.5f; + const float AT = Alpha * tj; + const float VT = Alpha * (sq(tj) * 0.5f - 2.0f / sq(Beta)); + const float PT = Alpha * ((-1.0f / sq(Beta)) * tj + (1.0f / 6.0f) * powf(tj, 3.0f)); + + const float J = Jm; + const float T = segment[index - 1].end_time + tj; + const float A = segment[index - 1].end_accel + AT; + const float V = segment[index - 1].end_vel + segment[index - 1].end_accel * tj + VT; + const float P = segment[index - 1].end_pos + segment[index - 1].end_vel * tj + 0.5f * segment[index - 1].end_accel * sq(tj) + PT; + add_segment(index, T, SegmentType::POSITIVE_JERK, J, A, V, P); +} + +// generate decreasing jerk magnitude time segment based on a raised cosine profile +// calculate the information needed to populate the decreasing jerk magnitude segment from the segment duration tj and jerk magnitude Jm +// the index variable is the position of this segment in the path and is incremented to reference the next segment in the array +void SCurve::add_segment_decr_jerk(uint8_t &index, float tj, float Jm) +{ + const float Beta = M_PI / tj; + const float Alpha = Jm * 0.5f; + const float AT = Alpha * tj; + const float VT = Alpha * (sq(tj) * 0.5f - 2.0f / sq(Beta)); + const float PT = Alpha * ((-1.0f / sq(Beta)) * tj + (1.0f / 6.0f) * powf(tj, 3.0f)); + const float A2T = Jm * tj; + const float V2T = Jm * sq(tj); + const float P2T = Alpha * ((-1.0f / sq(Beta)) * 2.0f * tj + (4.0f / 3.0f) * powf(tj, 3.0f)); + + const float J = Jm; + const float T = segment[index - 1].end_time + tj; + const float A = (segment[index - 1].end_accel - AT) + A2T; + const float V = (segment[index - 1].end_vel - VT) + (segment[index - 1].end_accel - AT) * tj + V2T; + const float P = (segment[index - 1].end_pos - PT) + (segment[index - 1].end_vel - VT) * tj + 0.5f * (segment[index - 1].end_accel - AT) * sq(tj) + P2T; + add_segment(index, T, SegmentType::NEGATIVE_JERK, J, A, V, P); +} + +// add single S-Curve segment +// populate the information for the segment specified in the path by the index variable. +// the index variable is incremented to reference the next segment in the array +void SCurve::add_segment(uint8_t &index, float end_time, SegmentType seg_type, float jerk_ref, float end_accel, float end_vel, float end_pos) +{ + segment[index].end_time = end_time; + segment[index].seg_type = seg_type; + segment[index].jerk_ref = jerk_ref; + segment[index].end_accel = end_accel; + segment[index].end_vel = end_vel; + segment[index].end_pos = end_pos; + index++; +} + +// set speed and acceleration limits for the path +// origin and destination are offsets from EKF origin +// speed and acceleration parameters are given in horizontal, up and down. +void SCurve::set_kinematic_limits(const Vector3f &origin, const Vector3f &destination, + float speed_xy, float speed_up, float speed_down, + float accel_xy, float accel_z) +{ + // ensure arguments are positive + speed_xy = fabsf(speed_xy); + speed_up = fabsf(speed_up); + speed_down = fabsf(speed_down); + accel_xy = fabsf(accel_xy); + accel_z = fabsf(accel_z); + + Vector3f direction = destination - origin; + const float track_speed_max = kinematic_limit(direction, speed_xy, speed_up, speed_down); + const float track_accel_max = kinematic_limit(direction, accel_xy, accel_z, accel_z); + + vel_max = track_speed_max; + accel_max = track_accel_max; +} + +// return true if the curve is valid. Used to identify and protect against code errors +bool SCurve::valid() const +{ + // check number of segments + if (num_segs != segments_max) { + return false; + } + + for (uint8_t i = 0; i < num_segs; i++) { + // jerk_ref should be finite (i.e. not NaN or infinity) + // time, accel, vel and pos should finite and not negative + if (!isfinite(segment[i].jerk_ref) || + !isfinite(segment[i].end_time) || + !isfinite(segment[i].end_accel) || + !isfinite(segment[i].end_vel) || is_negative(segment[i].end_vel) || + !isfinite(segment[i].end_pos)) { + return false; + } + + // time and pos should be increasing + if (i >= 1) { + if (is_negative(segment[i].end_time - segment[i-1].end_time) || + is_negative(segment[i].end_pos - segment[i-1].end_pos)) { + return false; + } + } + } + + // last segment should have zero acceleration + if (!is_zero(segment[num_segs-1].end_accel)) { + return false; + } + + // if we get this far then the curve must be valid + return true; +} + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +// debugging messages +void SCurve::debug() const +{ + ::printf("num_segs:%u, time:%4.2f, jerk_time:%4.2f, jerk_max:%4.2f, accel_max:%4.2f, vel_max:%4.2f\n", + (unsigned)num_segs, (double)time, (double)jerk_time, (double)jerk_max, (double)accel_max, (double)vel_max); + ::printf("T, Jt, J, A, V, P \n"); + for (uint8_t i = 0; i < num_segs; i++) { + ::printf("i:%u, T:%4.2f, Jtype:%4.2f, J:%4.2f, A:%4.2f, V: %4.2f, P: %4.2f\n", + (unsigned)i, (double)segment[i].end_time, (double)segment[i].seg_type, (double)segment[i].jerk_ref, + (double)segment[i].end_accel, (double)segment[i].end_vel, (double)segment[i].end_pos); + } + ::printf("track x:%4.2f, y:%4.2f, z:%4.2f\n", (double)track.x, (double)track.y, (double)track.z); + ::printf("delta_unit x:%4.2f, y:%4.2f, z:%4.2f\n", (double)delta_unit.x, (double)delta_unit.y, (double)delta_unit.z); +} +#endif diff --git a/libraries/AP_Math/SCurve.h b/libraries/AP_Math/SCurve.h new file mode 100644 index 0000000000..e499136083 --- /dev/null +++ b/libraries/AP_Math/SCurve.h @@ -0,0 +1,211 @@ +#pragma once + +#include + +/* + * SCurves calculate paths between waypoints (including the corners) using specified speed, acceleration and jerk limits + * + * How to use: + * 1. create three SCurve objects called something like "prev_leg", "this_leg" and "next_leg" + * 2. call this_leg.calculate_track() to calculate the path from the origin to the destination for the given speed, accel and jerk limits + * 3. if the vehicle will fly past the destination to another "next destination": + * a) call next_leg.calculate_track() with the appropriate arguments + * b) set a "fast_waypoint" boolean to true. this will be passed into "advance_target_along_track()" in the next step + * if there is no "next destination" + * a) call next_leg.init() + * b) set the "fast_waypoint" boolean to false + * 4. call this_leg.advance_target_along_track() with a small "dt" value and retrieve the resulting target position, velocity and acceleration + * Note: the target_pos should be set to the segments's earth frame origin before this function is called + * 5. pass the target position, velocity and acceleration into the position controller + * 6. repeat steps 4 and 5 until finished() returns true + * 7. promote the legs: + * a) set prev_leg = this_leg + * b) set this_leg = next_leg + * c) jump back to step 3 + * + * Other features: + * 1. set_speed_max() allows changing the max speeds mid path. The path will be recalculated + * 2. set_origin_speed_max() and set_destination_speed_max() allows setting the speed along the path at the beginning and end of the leg + * this is used to smoothly integrate with spline segments + * + * This library works with any units (meters, cm, etc) as long as they are used consistently. + * e.g. if origin and destination are meters, speeds should be in m/s, accel in m/s/s, etc. + * + * Terminology: + * position: a point in space + * velocity: rate of change of position. aka speed + * acceleration: rate of change of speed + * jerk: rate of change of acceleration + * jerk time: the time (in seconds) for jerk to increase from zero to its maximum value + * jounce: rate of change of jerk + * track: 3D path that the vehicle will follow + * path: position, velocity, accel and jerk kinematic profile that this library generates + */ + +class SCurve { + +public: + + // constructor + SCurve(); + + // initialise and clear the path + void init(); + + // generate a trigonometric track in 3D space that moves over a straight line + // between two points defined by the origin and destination + void calculate_track(const Vector3f &origin, const Vector3f &destination, + float speed_xy, float speed_up, float speed_down, + float accel_xy, float accel_z, + float jerk_time_sec, float jerk_maximum); + + // set maximum velocity and re-calculate the path using these limits + void set_speed_max(float speed_xy, float speed_up, float speed_down); + + // set the maximum vehicle speed at the origin + // returns the expected speed at the origin which will always be equal or lower than speed + float set_origin_speed_max(float speed); + + // set the maximum vehicle speed at the destination + void set_destination_speed_max(float speed); + + // move target location along path from origin to destination + // prev_leg and next_leg are the paths before and after this path + // wp_radius is max distance from the waypoint at the apex of the turn + // fast_waypoint should be true if vehicle will not stop at end of this leg + // dt is the time increment the vehicle will move along the path + // target_pos should be set to this segment's origin and it will be updated to the current position target + // target_vel and target_accel are updated with new targets + // returns true if vehicle has passed the apex of the corner + bool advance_target_along_track(SCurve &prev_leg, SCurve &next_leg, float wp_radius, bool fast_waypoint, float dt, Vector3f &target_pos, Vector3f &target_vel, Vector3f &target_accel) WARN_IF_UNUSED; + + // time has reached the end of the sequence + bool finished() const WARN_IF_UNUSED; + +private: + + // increment time and return the position, velocity and acceleration vectors relative to the origin + void move_from_pos_vel_accel(float dt, Vector3f &pos, Vector3f &vel, Vector3f &accel); + + // increment time and return the position, velocity and acceleration vectors relative to the destination + void move_to_pos_vel_accel(float dt, Vector3f &pos, Vector3f &vel, Vector3f &accel); + + // return the position, velocity and acceleration vectors relative to the origin at a specified time along the path + void move_from_time_pos_vel_accel(float t, Vector3f &pos, Vector3f &vel, Vector3f &accel); + + // get desired maximum speed along track + float get_speed_along_track() const WARN_IF_UNUSED { return vel_max; } + + // get desired maximum acceleration along track + float get_accel_along_track() const WARN_IF_UNUSED { return accel_max; } + + // return the change in position from origin to destination + const Vector3f& get_track() const WARN_IF_UNUSED { return track; }; + + // return the current time elapsed + float get_time_elapsed() const WARN_IF_UNUSED { return time; } + + // time at the end of the sequence + float time_end() const WARN_IF_UNUSED; + + // time left before sequence will complete + float get_time_remaining() const WARN_IF_UNUSED; + + // time when acceleration section of the sequence will complete + float get_accel_finished_time() const WARN_IF_UNUSED; + + // return true if the sequence is braking to a stop + bool braking() const WARN_IF_UNUSED; + + // increment the internal time + void advance_time(float dt); + + // calculate the jerk, acceleration, velocity and position at time t + void get_jerk_accel_vel_pos_at_time(float time_now, float &Jt_out, float &At_out, float &Vt_out, float &Pt_out) const; + + // calculate the jerk, acceleration, velocity and position at time t when running the constant jerk time segment + void calc_javp_for_segment_const_jerk(float time_now, float J0, float A0, float V0, float P0, float &Jt, float &At, float &Vt, float &Pt) const; + + // Calculate the jerk, acceleration, velocity and position at time t when running the increasing jerk magnitude time segment based on a raised cosine profile + void calc_javp_for_segment_incr_jerk(float time_now, float tj, float Jm, float A0, float V0, float P0, float &Jt, float &At, float &Vt, float &Pt) const; + + // Calculate the jerk, acceleration, velocity and position at time t when running the decreasing jerk magnitude time segment based on a raised cosine profile + void calc_javp_for_segment_decr_jerk(float time_now, float tj, float Jm, float A0, float V0, float P0, float &Jt, float &At, float &Vt, float &Pt) const; + + // generate time segments for straight segment + void add_segments(float L); + + // calculate the segment times for the trigonometric S-Curve path defined by: + // tj - duration of the raised cosine jerk profile (aka jerk time) + // Jm - maximum value of the raised cosine jerk profile (aka jerk max) + // V0 - initial velocity magnitude + // Am - maximum constant acceleration + // Vm - maximum constant velocity + // L - Length of the path + void calculate_path(float tj, float Jm, float V0, float Am, float Vm, float L, float &Jm_out, float &t2_out, float &t4_out, float &t6_out) const; + + // generate three time segments forming the jerk profile + void add_segments_jerk(uint8_t &seg_pnt, float tj, float Jm, float Tcj); + + // generate constant jerk time segment + void add_segment_const_jerk(uint8_t &seg_pnt, float tin, float J0); + + // generate increasing jerk magnitude time segment based on a raised cosine profile + void add_segment_incr_jerk(uint8_t &seg_pnt, float tj, float Jm); + + // generate decreasing jerk magnitude time segment based on a raised cosine profile + void add_segment_decr_jerk(uint8_t &seg_pnt, float tj, float Jm); + + // set speed and acceleration limits for the path + // origin and destination are offsets from EKF origin + // speed and acceleration parameters are given in horizontal, up and down. + void set_kinematic_limits(const Vector3f &origin, const Vector3f &destination, + float speed_xy, float speed_up, float speed_down, + float accel_xy, float accel_z); + + // return true if the curve is valid. Used to identify and protect against code errors + bool valid() const WARN_IF_UNUSED; + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + // debugging messages + void debug() const; +#endif + + // segment types + enum class SegmentType { + CONSTANT_JERK, + POSITIVE_JERK, + NEGATIVE_JERK + }; + + // add single segment + void add_segment(uint8_t &seg_pnt, float end_time, SegmentType seg_type, float jerk_ref, float end_accel, float end_vel, float end_pos); + + // members + float jerk_time; // duration of jerk raised cosine time segment + float jerk_max; // maximum jerk magnitude + float accel_max; // maximum acceleration magnitude + float vel_max; // maximum velocity magnitude + float time; // time that defines position on the path + float position_sq; // position (squared) on the path at the last time step (used to detect finish) + + // segment 0 is the initial segment and holds the vehicle's initial position and velocity + // segments 1 to 7 are the acceleration segments + // segments 8 to 14 are the speed change segments + // segment 15 is the constant velocity segment + // segment 16 to 22 is the deceleration segment + const static uint8_t segments_max = 23; // maximum number of time segments + + uint8_t num_segs; // number of time segments being used + struct { + float jerk_ref; // jerk reference value for time segment (the jerk at the beginning, middle or end depending upon the segment type) + SegmentType seg_type; // segment type (jerk is constant, increasing or decreasing) + float end_time; // final time value for segment + float end_accel; // final acceleration value for segment + float end_vel; // final velocity value for segment + float end_pos; // final position value for segment + } segment[segments_max]; + + Vector3f track; // total change in position from origin to destination + Vector3f delta_unit; // reference direction vector for path +};