Skip to content

SCurve: fix calculate_path bugs and add comprehensive tests #32730

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
rmackay9 merged 5 commits into from
Apr 14, 2026
Merged

SCurve: fix calculate_path bugs and add comprehensive tests #32730

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
c88b583
AP_Math: add comprehensive calculate_path tests covering all code paths
lthall Apr 10, 2026
10f1a3f
SCurve: fix path length sign, integer division, and divide-by-zero bugs
lthall Apr 10, 2026
b9b7396
SCurve: use consistent float literals throughout
lthall Apr 10, 2026
c9f4c10
SCurve: fix comment, whitespace and operator spacing
lthall Apr 10, 2026
ffc489b
AP_Math: add constraint validation to calculate_path tests
lthall Apr 10, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
75 changes: 39 additions & 36 deletions libraries/AP_Math/SCurve.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ void SCurve::init()

is_arc_segment = false;
seg_delta.zero();
seg_length = 0.0;
seg_length = 0.0f;
arc = {};
}

Expand Down Expand Up @@ -84,7 +84,7 @@ void SCurve::calculate_track(const Vector3p &origin, const Vector3p &destination

const Vector2f chord = seg_delta.xy();
const float chord_length = seg_delta.xy().length();
if (!is_positive(chord_length) || fabsf(wrap_PI(arc_ang_rad)) < radians(1.0)) {
if (!is_positive(chord_length) || fabsf(wrap_PI(arc_ang_rad)) < radians(1.0f)) {
// straight segment
is_arc_segment = false;
arc.angle_rad = 0.0f;
Expand All @@ -98,7 +98,7 @@ void SCurve::calculate_track(const Vector3p &origin, const Vector3p &destination
arc.radius_ne = fabsf(chord_length / (2.0f * fabsf(sinf(arc.angle_rad * 0.5f))));
const float center_offset = safe_sqrt(sq(arc.radius_ne) - sq(chord_length * 0.5f)); // perpendicular offset from chord to circle center
const float turn_dir = is_negative(arc.angle_rad) ? -1.0f : 1.0f; // -1 for CCW, 1 for CW
const float center_side = (is_positive(wrap_PI(fabsf(arc.angle_rad)))) ? 1.0f : -1.0f; // -1 for CCW, 1 for CW
const float center_side = (is_positive(wrap_PI(fabsf(arc.angle_rad)))) ? 1.0f : -1.0f; // 1 for |angle| < PI, -1 for |angle| > PI
if (!is_zero(arc.radius_ne) && !is_zero(chord_length)) {
arc.center_ne = chord * 0.5f + Vector2f(-chord.y, chord.x) * (center_side * turn_dir * center_offset / chord_length);
arc.length_ne = arc.radius_ne * fabsf(arc.angle_rad);
Expand Down Expand Up @@ -294,7 +294,7 @@ void SCurve::set_speed_max(float speed_xy, float speed_up, float speed_down)

// 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;
const float t15 = is_positive(segment[SEG_CONST].end_vel) ? dP / segment[SEG_CONST].end_vel : 0.0f;

@andyp1per andyp1per Apr 13, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's 124 bytes for doing the same check multiple times. You could probably avoid the impact of this by using a function

@lthall lthall Apr 15, 2026

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good suggestion. Addressed in a follow up PR:

#32787

for (uint8_t i = SEG_CONST; i <= SEG_DECEL_END; i++) {
segment[i].end_time += t15;
segment[i].end_pos += dP;
Expand All @@ -321,14 +321,14 @@ void SCurve::set_speed_max(float speed_xy, float speed_up, float speed_down)
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)) {
float jerk_time = MIN(powf((fabsf(vel_max - segment[SEG_ACCEL_END].end_vel) * M_PI) / (4.0f * snap_max), 1.0f / 3.0f), jerk_max * M_PI / (2.0f * 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(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));
} 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(snap_max, jerk_max, segment[SEG_ACCEL_END].end_vel, accel_max, Vm, L * 0.5f, Jm, tj, t2, t4, t6);
}

Expand Down Expand Up @@ -372,7 +372,7 @@ void SCurve::set_speed_max(float speed_xy, float speed_up, float speed_down)

// 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;
const float t15 = is_positive(segment[SEG_CONST].end_vel) ? dP / segment[SEG_CONST].end_vel : 0.0f;
for (uint8_t i = SEG_CONST; i <= SEG_DECEL_END; i++) {
segment[i].end_time += t15;
segment[i].end_pos += dP;
Expand Down Expand Up @@ -454,7 +454,7 @@ float SCurve::set_origin_speed_max(float speed)

// add to constant velocity segment to end at the correct position
const float dP = MAX(0.0f, seg_length - segment[SEG_DECEL_END].end_pos);
const float t15 = dP / segment[SEG_CONST].end_vel;
const float t15 = is_positive(segment[SEG_CONST].end_vel) ? dP / segment[SEG_CONST].end_vel : 0.0f;
for (uint8_t i = SEG_CONST; i <= SEG_DECEL_END; i++) {
segment[i].end_time += t15;
segment[i].end_pos += dP;
Expand Down Expand Up @@ -509,7 +509,7 @@ void SCurve::set_destination_speed_max(float speed)

// add to constant velocity segment to end at the correct position
const float dP = MAX(0.0f, seg_length - segment[SEG_DECEL_END].end_pos);
const float t15 = dP / segment[SEG_CONST].end_vel;
const float t15 = is_positive(segment[SEG_CONST].end_vel) ? dP / segment[SEG_CONST].end_vel : 0.0f;
for (uint8_t i = SEG_CONST; i <= SEG_DECEL_END; i++) {
segment[i].end_time += t15;
segment[i].end_pos += dP;
Expand Down Expand Up @@ -668,7 +668,7 @@ void SCurve::project_scurve_onto_track(float scurve_A1, float scurve_V1, float s
float SCurve::time_end() const
{
if (num_segs != segments_max) {
return 0.0;
return 0.0f;
}
return segment[SEG_DECEL_END].end_time;
}
Expand All @@ -677,7 +677,7 @@ float SCurve::time_end() const
float SCurve::get_time_remaining() const
{
if (num_segs != segments_max) {
return 0.0;
return 0.0f;
}
return segment[SEG_DECEL_END].end_time - time;
}
Expand All @@ -686,7 +686,7 @@ float SCurve::get_time_remaining() const
float SCurve::get_accel_finished_time() const
{
if (num_segs != segments_max) {
return 0.0;
return 0.0f;
}
return segment[SEG_ACCEL_END].end_time;
}
Expand All @@ -705,7 +705,7 @@ bool SCurve::braking() const
float SCurve::time_accel_end() const
{
if (num_segs != segments_max) {
return 0.0;
return 0.0f;
}
return segment[SEG_ACCEL_END].end_time;
}
Expand All @@ -715,25 +715,25 @@ float SCurve::time_accel_end() const
float SCurve::time_decel_start() const
{
if (num_segs != segments_max) {
return 0.0;
return 0.0f;
}
return segment[SEG_DECEL_START].end_time;
}

// increment the internal time
void SCurve::advance_time(float dt)
{
time = MIN(time+dt, time_end());
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
{
// start with zeros as function is void and we want to guarantee all outputs are initialised
Jt_out = 0;
At_out = 0;
Vt_out = 0;
Pt_out = 0;
Jt_out = 0.0f;
At_out = 0.0f;
Vt_out = 0.0f;
Pt_out = 0.0f;
if (num_segs != segments_max) {
return;
}
Expand Down Expand Up @@ -801,7 +801,7 @@ void SCurve::calc_javp_for_segment_const_jerk(float time_now, float J0, float A0
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;
Jt = 0.0f;
At = A0;
Vt = V0;
Pt = P0;
Expand All @@ -819,7 +819,7 @@ void SCurve::calc_javp_for_segment_incr_jerk(float time_now, float tj, float Jm,
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;
Jt = 0.0f;
At = A0;
Vt = V0;
Pt = P0;
Expand Down Expand Up @@ -888,7 +888,7 @@ void SCurve::add_segments(float L)
// 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
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)
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;
Expand All @@ -911,18 +911,21 @@ void SCurve::calculate_path(float Sm, float Jm, float V0, float Am, float Vm, fl
return;
}

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)) );
float tj = Jm * M_PI / (2.0f * Sm);
float At = MIN(MIN(Am,
(Vm - V0) / (2.0f * tj) ),
(L - 4.0f * V0 * tj) / (4.0f * sq(tj)) );
if (!is_positive(At)) {
return;
}
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;
powf((L * M_PI) / (8.0f * Sm), 1.0f / 4.0f) ),
powf((Vm * M_PI) / (4.0f * Sm), 1.0f / 3.0f) ),
safe_sqrt((Am * M_PI) / (2.0f * Sm)) );
Jm = 2.0f * Sm * tj / M_PI;
Am = Jm * tj;
} else {
// When doing speed change we use fixed tj and adjust Jm for small changes
Expand All @@ -938,7 +941,7 @@ void SCurve::calculate_path(float Sm, float Jm, float V0, float Am, float Vm, fl
// 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)));
t4_out = MAX(t4_out, 0.0);
t4_out = MAX(t4_out, 0.0f);
t6_out = 0.0f;
}
} else {
Expand All @@ -952,7 +955,7 @@ void SCurve::calculate_path(float Sm, float Jm, float V0, float Am, float Vm, fl
// 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)));
t4_out = MAX(t4_out, 0.0);
t4_out = MAX(t4_out, 0.0f);
t6_out = t2_out;
}
}
Expand Down Expand Up @@ -1064,7 +1067,7 @@ void SCurve::add_segment_incr_jerk(uint8_t &index, float tj, float Jm)
if (!is_positive(tj)) {
add_segment(index, segment[index - 1].end_time,
SegmentType::CONSTANT_JERK,
0.0,
0.0f,
segment[index - 1].end_accel,
segment[index - 1].end_vel,
segment[index - 1].end_pos);
Expand Down Expand Up @@ -1093,7 +1096,7 @@ void SCurve::add_segment_decr_jerk(uint8_t &index, float tj, float Jm)
if (!is_positive(tj)) {
add_segment(index, segment[index - 1].end_time,
SegmentType::CONSTANT_JERK,
0.0,
0.0f,
segment[index - 1].end_accel,
segment[index - 1].end_vel,
segment[index - 1].end_pos);
Expand Down
Loading
Loading