Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

This commit is contained in:
Leonard Hall 2022-03-05 09:31:45 +10:30 committed by Randy Mackay
parent d0213dd886
commit fa12b0ac53
3 changed files with 128 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

160
libraries/AP_Math/SCurve.cpp
View File

@ -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",

21
libraries/AP_Math/SCurve.h
View File

@ -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

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

@ -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);
}