diff --git a/libraries/AP_Math/SCurve.cpp b/libraries/AP_Math/SCurve.cpp index a09a751428..115687f616 100644 --- a/libraries/AP_Math/SCurve.cpp +++ b/libraries/AP_Math/SCurve.cpp @@ -40,7 +40,7 @@ SCurve::SCurve() // initialise and clear the path void SCurve::init() { - jerk_time = 0.0f; + snap_max = 0.0f; jerk_max = 0.0f; accel_max = 0.0f; vel_max = 0.0f; @@ -57,7 +57,7 @@ void SCurve::init() 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) + float snap_maximum, float jerk_maximum) { init(); @@ -67,8 +67,8 @@ void SCurve::calculate_track(const Vector3f &origin, const Vector3f &destination return; } - // set jerk time and jerk max - jerk_time = jerk_time_sec; + // set snap_max and jerk max + snap_max = snap_maximum; jerk_max = jerk_maximum; // update speed and acceleration limits along path @@ -77,7 +77,7 @@ void SCurve::calculate_track(const Vector3f &origin, const Vector3f &destination 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 (!is_positive(snap_max) || !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 @@ -210,13 +210,13 @@ void SCurve::set_speed_max(float speed_xy, float speed_up, float speed_down) // 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); + float Jm, tj, t2, t4, t6; + calculate_path(snap_max, jerk_max, Vstart, accel_max, MAX(Vmin, vel_max), Pend * 0.5f, Jm, tj, t2, t4, t6); uint8_t seg = SEG_INIT+1; - add_segments_jerk(seg, jerk_time, Jm, t2); + add_segments_jerk(seg, tj, Jm, t2); add_segment_const_jerk(seg, t4, 0.0f); - add_segments_jerk(seg, jerk_time, -Jm, t6); + add_segments_jerk(seg, tj, -Jm, t6); // remove numerical errors segment[SEG_ACCEL_END].end_accel = 0.0f; @@ -231,12 +231,12 @@ void SCurve::set_speed_max(float speed_xy, float speed_up, float speed_down) 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); + calculate_path(snap_max, jerk_max, 0.0f, accel_max, MAX(Vmin, vel_max), Pend * 0.5f, Jm, tj, t2, t4, t6); seg = SEG_CONST + 1; - add_segments_jerk(seg, jerk_time, -Jm, t6); + add_segments_jerk(seg, tj, -Jm, t6); add_segment_const_jerk(seg, t4, 0.0f); - add_segments_jerk(seg, jerk_time, Jm, t2); + add_segments_jerk(seg, tj, Jm, t2); // remove numerical errors segment[SEG_DECEL_END].end_accel = 0.0f; @@ -264,27 +264,30 @@ void SCurve::set_speed_max(float speed_xy, float speed_up, float speed_down) 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 tj = 0; float t2 = 0; float t4 = 0; float t6 = 0; + float jerk_time = MIN(powf((fabsf(vel_max - segment[SEG_ACCEL_END].end_vel) * M_PI) / (4 * snap_max), 1/3), jerk_max * M_PI / (2 * snap_max)); 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); + calculate_path(snap_max, jerk_max, vel_max, accel_max, segment[SEG_ACCEL_END].end_vel, L * 0.5f, Jm, tj, 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); + calculate_path(snap_max, jerk_max, segment[SEG_ACCEL_END].end_vel, accel_max, Vm, L * 0.5f, Jm, tj, 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_segments_jerk(seg, tj, Jm, t2); add_segment_const_jerk(seg, t4, 0.0f); - add_segments_jerk(seg, jerk_time, -Jm, t6); + add_segments_jerk(seg, tj, -Jm, t6); // remove numerical errors segment[SEG_SPEED_CHANGE_END].end_accel = 0.0f; @@ -297,11 +300,11 @@ void SCurve::set_speed_max(float speed_xy, float speed_up, float speed_down) 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); + float Jm, tj, t2, t4, t6; + calculate_path(snap_max, jerk_max, Vend, accel_max, segment[SEG_CONST].end_vel, Pend - segment[SEG_CONST].end_pos, Jm, tj, t2, t4, t6); + add_segments_jerk(seg, tj, -Jm, t6); add_segment_const_jerk(seg, t4, 0.0f); - add_segments_jerk(seg, jerk_time, Jm, t2); + add_segments_jerk(seg, tj, Jm, t2); } else { // No deceleration is required for (uint8_t i = SEG_CONST+1; i <= SEG_DECEL_END; i++) { @@ -355,14 +358,14 @@ float SCurve::set_origin_speed_max(float speed) 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); + float Jm, tj, t2, t4, t6; + calculate_path(snap_max, jerk_max, speed, accel_max, Vm, track_length * 0.5f, Jm, tj, 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_segments_jerk(seg, tj, Jm, t2); add_segment_const_jerk(seg, t4, 0.0f); - add_segments_jerk(seg, jerk_time, -Jm, t6); + add_segments_jerk(seg, tj, -Jm, t6); // remove numerical errors segment[SEG_ACCEL_END].end_accel = 0.0f; @@ -388,11 +391,11 @@ float SCurve::set_origin_speed_max(float speed) 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); + calculate_path(snap_max, jerk_max, 0.0f, accel_max, segment[SEG_CONST].end_vel, track_length * 0.5f, Jm, tj, t2, t4, t6); - add_segments_jerk(seg, jerk_time, -Jm, t6); + add_segments_jerk(seg, tj, -Jm, t6); add_segment_const_jerk(seg, t4, 0.0f); - add_segments_jerk(seg, jerk_time, Jm, t2); + add_segments_jerk(seg, tj, Jm, t2); // remove numerical errors segment[SEG_DECEL_END].end_accel = 0.0f; @@ -437,15 +440,15 @@ void SCurve::set_destination_speed_max(float speed) 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); + float Jm, tj, t2, t4, t6; + calculate_path(snap_max, jerk_max, speed, accel_max, Vm, track_length * 0.5f, Jm, tj, 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_segments_jerk(seg, tj, -Jm, t6); add_segment_const_jerk(seg, t4, 0.0f); - add_segments_jerk(seg, jerk_time, Jm, t2); + add_segments_jerk(seg, tj, Jm, t2); // remove numerical errors segment[SEG_DECEL_END].end_accel = 0.0f; @@ -597,7 +600,7 @@ void SCurve::advance_time(float dt) // 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)) { + if (num_segs != segments_max) { Jt_out = 0; At_out = 0; Vt_out = 0; @@ -607,7 +610,7 @@ void SCurve::get_jerk_accel_vel_pos_at_time(float time_now, float &Jt_out, float SegmentType Jtype; uint8_t pnt = num_segs; - float Jm, T0, A0, V0, P0; + float Jm, tj, T0, A0, V0, P0; // find active segment at time_now for (uint8_t i = 0; i < num_segs; i++) { @@ -618,6 +621,7 @@ void SCurve::get_jerk_accel_vel_pos_at_time(float time_now, float &Jt_out, float if (pnt == 0) { Jtype = SegmentType::CONSTANT_JERK; Jm = 0.0f; + tj = 0.0f; T0 = segment[pnt].end_time; A0 = segment[pnt].end_accel; V0 = segment[pnt].end_vel; @@ -625,6 +629,7 @@ void SCurve::get_jerk_accel_vel_pos_at_time(float time_now, float &Jt_out, float } else if (pnt == num_segs) { Jtype = SegmentType::CONSTANT_JERK; Jm = 0.0f; + tj = 0.0f; T0 = segment[pnt - 1].end_time; A0 = segment[pnt - 1].end_accel; V0 = segment[pnt - 1].end_vel; @@ -632,6 +637,7 @@ void SCurve::get_jerk_accel_vel_pos_at_time(float time_now, float &Jt_out, float } else { Jtype = segment[pnt].seg_type; Jm = segment[pnt].jerk_ref; + tj = segment[pnt].end_time - segment[pnt - 1].end_time; T0 = segment[pnt - 1].end_time; A0 = segment[pnt - 1].end_accel; V0 = segment[pnt - 1].end_vel; @@ -643,10 +649,10 @@ void SCurve::get_jerk_accel_vel_pos_at_time(float time_now, float &Jt_out, float 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); + calc_javp_for_segment_incr_jerk(time_now - T0, tj, 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); + calc_javp_for_segment_decr_jerk(time_now - T0, tj, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out); break; } Pt_out = MAX(0.0f, Pt_out); @@ -664,6 +670,13 @@ void SCurve::calc_javp_for_segment_const_jerk(float time_now, float J0, float A0 // 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 { + if (!is_positive(tj)) { + Jt = 0.0; + At = A0; + Vt = V0; + Pt = P0; + return; + } const float Alpha = Jm * 0.5f; const float Beta = M_PI / tj; Jt = Alpha * (1.0f - cosf(Beta * time_now)); @@ -675,6 +688,13 @@ void SCurve::calc_javp_for_segment_incr_jerk(float time_now, float tj, float Jm, // 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 { + if (!is_positive(tj)) { + Jt = 0.0; + At = A0; + Vt = V0; + Pt = P0; + return; + } const float Alpha = Jm * 0.5f; const float Beta = M_PI / tj; const float AT = Alpha * tj; @@ -699,12 +719,12 @@ void SCurve::add_segments(float 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); + float Jm, tj, t2, t4, t6; + calculate_path(snap_max, jerk_max, 0.0f, accel_max, vel_max, L * 0.5f, Jm, tj, t2, t4, t6); - add_segments_jerk(num_segs, jerk_time, Jm, t2); + add_segments_jerk(num_segs, tj, Jm, t2); add_segment_const_jerk(num_segs, t4, 0.0f); - add_segments_jerk(num_segs, jerk_time, -Jm, t6); + add_segments_jerk(num_segs, tj, -Jm, t6); // remove numerical errors segment[SEG_ACCEL_END].end_accel = 0.0f; @@ -721,9 +741,9 @@ void SCurve::add_segments(float L) 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_segments_jerk(num_segs, tj, -Jm, t6); add_segment_const_jerk(num_segs, t4, 0.0f); - add_segments_jerk(num_segs, jerk_time, Jm, t2); + add_segments_jerk(num_segs, tj, Jm, t2); // remove numerical errors segment[SEG_DECEL_END].end_accel = 0.0f; @@ -731,23 +751,24 @@ void SCurve::add_segments(float L) } // calculate the segment times for the trigonometric S-Curve path defined by: -// tj - duration of the raised cosine jerk profile +// Sm - 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 kinematic 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) +// tj_out, t2_out, t4_out, t6_out are the segment durations needed to achieve the kinematic path specified by the input variables +void SCurve::calculate_path(float Sm, float Jm, float V0, float Am, float Vm, float L,float &Jm_out, float &tj_out, float &t2_out, float &t4_out, float &t6_out) { // init outputs Jm_out = 0.0f; + tj_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 (!is_positive(Sm) || !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 @@ -760,9 +781,24 @@ void SCurve::calculate_path(float tj, float Jm, float V0, float Am, float Vm, fl 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; + float tj = Jm * M_PI / (2 * Sm); + float At = MIN(MIN(Am, + (Vm - V0) / (2.0f * tj) ), + (L + 4.0f * V0 * tj) / (4.0f * sq(tj)) ); + if (fabsf(At) < Jm * tj) { + if (is_zero(V0)) { + // we do not have a solution for non-zero initial velocity + tj = MIN( MIN( MIN( tj, + powf((L * M_PI) / (8.0 * Sm), 1.0/4.0) ), + powf((Vm * M_PI) / (4.0 * Sm), 1.0/3.0) ), + safe_sqrt((Am * M_PI) / (2.0 * Sm)) ); + Jm = 2.0 * Sm * tj / M_PI; + Am = Jm * tj; + } else { + // When doing speed change we use fixed tj and adjust Jm for small changes + Am = At; + 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; @@ -790,10 +826,12 @@ void SCurve::calculate_path(float tj, float Jm, float V0, float Am, float Vm, fl t6_out = t2_out; } } + tj_out = tj; Jm_out = Jm; // check outputs and reset back to zero if necessary if (!isfinite(Jm_out) || is_negative(Jm_out) || + !isfinite(tj_out) || is_negative(tj_out) || !isfinite(t2_out) || is_negative(t2_out) || !isfinite(t4_out) || is_negative(t4_out) || !isfinite(t6_out) || is_negative(t6_out)) { @@ -824,7 +862,7 @@ void SCurve::add_segments_jerk(uint8_t &index, float tj, float Jm, float Tcj) void SCurve::add_segment_const_jerk(uint8_t &index, float tj, float J0) { // if no time increase copy previous segment - if (is_zero(tj)) { + if (!is_positive(tj)) { add_segment(index, segment[index - 1].end_time, SegmentType::CONSTANT_JERK, J0, @@ -847,6 +885,16 @@ void SCurve::add_segment_const_jerk(uint8_t &index, float tj, float 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_incr_jerk(uint8_t &index, float tj, float Jm) { + // if no time increase copy previous segment + if (!is_positive(tj)) { + add_segment(index, segment[index - 1].end_time, + SegmentType::CONSTANT_JERK, + 0.0, + segment[index - 1].end_accel, + segment[index - 1].end_vel, + segment[index - 1].end_pos); + return; + } const float Beta = M_PI / tj; const float Alpha = Jm * 0.5f; const float AT = Alpha * tj; @@ -866,6 +914,16 @@ void SCurve::add_segment_incr_jerk(uint8_t &index, float tj, float 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) { + // if no time increase copy previous segment + if (!is_positive(tj)) { + add_segment(index, segment[index - 1].end_time, + SegmentType::CONSTANT_JERK, + 0.0, + segment[index - 1].end_accel, + segment[index - 1].end_vel, + segment[index - 1].end_pos); + return; + } const float Beta = M_PI / tj; const float Alpha = Jm * 0.5f; const float AT = Alpha * tj; @@ -960,8 +1018,8 @@ bool SCurve::valid() const // 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("num_segs:%u, time:%4.2f, snap_max:%4.2f, jerk_max:%4.2f, accel_max:%4.2f, vel_max:%4.2f\n", + (unsigned)num_segs, (double)time, (double)snap_max, (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", diff --git a/libraries/AP_Math/SCurve.h b/libraries/AP_Math/SCurve.h index 7696c33450..701e08446b 100644 --- a/libraries/AP_Math/SCurve.h +++ b/libraries/AP_Math/SCurve.h @@ -36,8 +36,8 @@ * velocity: rate of change of position. aka speed * acceleration: rate of change of speed * jerk: rate of change of acceleration + * snap: rate of change of jerk * 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 */ @@ -53,21 +53,22 @@ public: void init(); // 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) + // Sm - maximum value of the snap 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 + // tj_out, t2_out, t4_out, t6_out are the segment durations needed to achieve the kinematic path specified by the input variables // this is an internal function, static for test suite - static 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); + static void calculate_path(float Sm, float Jm, float V0, float Am, float Vm, float L, float &Jm_out, float &tj_out, float &t2_out, float &t4_out, float &t6_out); // 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); + float snap_maximum, 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); @@ -146,16 +147,16 @@ private: void add_segments(float L); // generate three time segments forming the jerk profile - void add_segments_jerk(uint8_t &seg_pnt, float tj, float Jm, float Tcj); + void add_segments_jerk(uint8_t &seg_pnt, float Jm, float tj, float Tcj); // generate constant jerk time segment - void add_segment_const_jerk(uint8_t &seg_pnt, float tin, float J0); + void add_segment_const_jerk(uint8_t &seg_pnt, float J0, float tin); // 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); + void add_segment_incr_jerk(uint8_t &seg_pnt, float Jm, float tj); // 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); + void add_segment_decr_jerk(uint8_t &seg_pnt, float Jm, float tj); // set speed and acceleration limits for the path // origin and destination are offsets from EKF origin @@ -183,7 +184,7 @@ private: 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 snap_max; // maximum snap magnitude float jerk_max; // maximum jerk magnitude float accel_max; // maximum acceleration magnitude float vel_max; // maximum velocity magnitude diff --git a/libraries/AP_Math/tests/test_scurve.cpp b/libraries/AP_Math/tests/test_scurve.cpp index 6620d790a0..22b1a0beda 100644 --- a/libraries/AP_Math/tests/test_scurve.cpp +++ b/libraries/AP_Math/tests/test_scurve.cpp @@ -7,13 +7,14 @@ TEST(LinesScurve, test_calculate_path) { - float Jm_out, t2_out, t4_out, t6_out; - SCurve::calculate_path(0.300000012, 19.4233513, 0, 5.82700586, 188.354691, 2.09772229, - Jm_out, t2_out, t4_out, t6_out); - EXPECT_FLOAT_EQ(Jm_out, 19.423351); - EXPECT_FLOAT_EQ(t2_out, 0.0); - EXPECT_FLOAT_EQ(t4_out, 0.0); - EXPECT_FLOAT_EQ(t6_out, 0.0); + // this test doesn't do much... + float Jm_out, tj_out, t2_out, t4_out, t6_out; + SCurve::calculate_path(62.8319, 10, 0, 5, 10, 100, + Jm_out, tj_out, t2_out, t4_out, t6_out); + EXPECT_FLOAT_EQ(Jm_out, 10); + EXPECT_FLOAT_EQ(t2_out, 0.25000018); + EXPECT_FLOAT_EQ(t4_out, 1.2500002); + EXPECT_FLOAT_EQ(t6_out, 0.25000018); }