zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

gps-1.3.1
Leonard Hall 3 years ago committed by Andrew Tridgell
parent
9c70fc1aa9
commit
594fc3f02d
3 changed files with 128 additions and 68 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 160
      libraries/AP_Math/SCurve.cpp
  2. 21
      libraries/AP_Math/SCurve.h
  3. 15
      libraries/AP_Math/tests/test_scurve.cpp

160
libraries/AP_Math/SCurve.cpp
Unescape View File

@ -40,7 +40,7 @@ SCurve::SCurve() @@ -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() @@ -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 @@ -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 @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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, @@ -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) @@ -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) @@ -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) @@ -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 @@ -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 @@ -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) @@ -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) @@ -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) @@ -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 @@ -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",

21
libraries/AP_Math/SCurve.h
Unescape View File

@ -36,8 +36,8 @@ @@ -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: @@ -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: @@ -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: @@ -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

15
libraries/AP_Math/tests/test_scurve.cpp
Unescape View File

@ -7,13 +7,14 @@ @@ -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);
}

Write Preview
Loading…
Cancel
Save