Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include <AP_Math/AP_Math.h>
17 : #include <AP_HAL/AP_HAL.h>
18 : #include <AP_InternalError/AP_InternalError.h>
19 : #include <AP_Vehicle/AP_Vehicle_Type.h>
20 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_Blimp)
21 : #include <AP_Logger/AP_Logger.h>
22 : #endif
23 : #include "SCurve.h"
24 :
25 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
26 : #include <stdio.h>
27 : #endif
28 :
29 : extern const AP_HAL::HAL &hal;
30 :
31 : #define SEG_INIT 0
32 : #define SEG_ACCEL_MAX 4
33 : #define SEG_ACCEL_END 7
34 : #define SEG_SPEED_CHANGE_END 14
35 : #define SEG_CONST 15
36 : #define SEG_DECEL_START 15
37 : #define SEG_DECEL_END 22
38 :
39 : // constructor
40 4774 : SCurve::SCurve()
41 : {
42 4774 : init();
43 4774 : }
44 :
45 : // initialise and clear the path
46 54461 : void SCurve::init()
47 : {
48 54461 : snap_max = 0.0f;
49 54461 : jerk_max = 0.0f;
50 54461 : accel_max = 0.0f;
51 54461 : accel_c_max = 0.0f;
52 54461 : vel_max = 0.0f;
53 54461 : time = 0.0f;
54 54461 : num_segs = SEG_INIT;
55 54461 : add_segment(num_segs, 0.0f, SegmentType::CONSTANT_JERK, 0.0f, 0.0f, 0.0f, 0.0f);
56 :
57 54461 : is_arc_segment = false;
58 54461 : seg_delta.zero();
59 54461 : seg_length = 0.0f;
60 54461 : arc = {};
61 54461 : }
62 :
63 : // generate a 3D trigonometric track defined by origin, destination, and arc angle in radians (0 = straight)
64 : // includes speed, acceleration, and jerk limits for horizontal and vertical motion
65 1265 : void SCurve::calculate_track(const Vector3p &origin, const Vector3p &destination, float arc_ang_rad,
66 : float speed_xy, float speed_up, float speed_down,
67 : float accel_xy, float accel_z, float accel_c,
68 : float snap_maximum, float jerk_maximum)
69 : {
70 1265 : init();
71 :
72 : // ensure arguments are positive
73 1265 : speed_xy = fabsf(speed_xy);
74 1265 : speed_up = fabsf(speed_up);
75 1265 : speed_down = fabsf(speed_down);
76 1265 : accel_xy = fabsf(accel_xy);
77 1265 : accel_z = fabsf(accel_z);
78 :
79 : // leave track as zero length if origin and destination are equal or if the new track length squared is zero
80 1265 : seg_delta = (destination - origin).tofloat();
81 1265 : if (seg_delta.is_zero() || is_zero(seg_delta.length_squared())) {
82 128 : seg_delta.zero();
83 128 : return;
84 : }
85 :
86 : // convert the origin, destination and arc angle into the canonical arc/straight geometry
87 1137 : const Vector2f chord = seg_delta.xy();
88 1137 : const float chord_length = chord.length();
89 1137 : if (!is_positive(chord_length) || fabsf(wrap_PI(arc_ang_rad)) < radians(1.0f)) {
90 1111 : set_straight_geometry();
91 : } else {
92 : // arc segment. The outer condition guarantees chord_length > 0 and
93 : // |sin(arc_ang_rad/2)| >= sin(0.5 deg), so radius is positive (>= chord_length/2)
94 : // and both divisions below are safe.
95 26 : const float radius = fabsf(chord_length / (2.0f * fabsf(sinf(arc_ang_rad * 0.5f))));
96 26 : const float center_offset = safe_sqrt(sq(radius) - sq(chord_length * 0.5f)); // perpendicular offset from chord to circle center
97 26 : const float turn_dir = is_negative(arc_ang_rad) ? -1.0f : 1.0f; // -1 for CCW, 1 for CW
98 26 : const float center_side = (is_positive(wrap_PI(fabsf(arc_ang_rad)))) ? 1.0f : -1.0f; // 1 for |angle| < PI, -1 for |angle| > PI
99 26 : const Vector2f center_ne = chord * 0.5f + Vector2f(-chord.y, chord.x) * (center_side * turn_dir * center_offset / chord_length);
100 26 : set_arc_geometry(center_ne, radius, arc_ang_rad);
101 : }
102 1137 : if (is_zero(seg_length)) {
103 0 : seg_delta.zero();
104 0 : return;
105 : }
106 :
107 : // build the jerk-limited profile from the configured geometry and limits
108 1137 : generate_path(speed_xy, speed_up, speed_down, accel_xy, accel_z, accel_c, snap_maximum, jerk_maximum);
109 : }
110 :
111 : // generate a 3D trigonometric track that follows a circular arc about center_ne.
112 : // total_angle_rad is the signed swept angle (may exceed 2*pi for multiple turns);
113 : // its sign sets direction. climb_d_m is the net D-axis change applied linearly
114 : // along the arc. The radius is the distance from origin to center so the origin
115 : // lies on the circle. Includes speed, acceleration and jerk limits.
116 18 : void SCurve::calculate_circle_track(const Vector3p &origin, const Vector2f ¢er_ne,
117 : float total_angle_rad, float climb_d_m,
118 : float speed_xy, float speed_up, float speed_down,
119 : float accel_xy, float accel_z, float accel_c,
120 : float snap_maximum, float jerk_maximum)
121 : {
122 18 : init();
123 :
124 : // ensure limit arguments are positive
125 18 : speed_xy = fabsf(speed_xy);
126 18 : speed_up = fabsf(speed_up);
127 18 : speed_down = fabsf(speed_down);
128 18 : accel_xy = fabsf(accel_xy);
129 18 : accel_z = fabsf(accel_z);
130 :
131 : // the arc radius is the distance from the origin to the circle center, so the origin lies on the circle
132 18 : const Vector2f origin_to_center_ne = center_ne - origin.xy().tofloat();
133 18 : const float radius = origin_to_center_ne.length();
134 :
135 : // leave track as zero length if the radius or swept angle is too small to form an arc
136 18 : if (!is_positive(radius) || fabsf(total_angle_rad) < radians(1.0f)) {
137 0 : seg_delta.zero();
138 0 : return;
139 : }
140 :
141 : // seg_delta must hold the true origin-to-destination displacement (not just the climb):
142 : // SCurve::move_to_pos_vel_accel() relies on it to exactly cancel a finished leg's
143 : // contribution when that leg is preserved as another leg's prev_leg for corner blending
144 : // (see AC_WPNav::set_wp_destination_NED_m), and AC_WPNav derives the leg destination
145 : // from it via get_origin_to_destination(). For a closed arc the endpoint is the origin
146 : // rotated about the center by the swept angle, so the NE displacement is the
147 : // origin-to-center vector minus that same vector rotated by the swept angle -- zero for
148 : // whole turns, but not in general.
149 18 : Vector2f end_offset_ne = origin_to_center_ne;
150 18 : end_offset_ne.rotate(total_angle_rad);
151 18 : seg_delta = Vector3f(origin_to_center_ne - end_offset_ne, climb_d_m);
152 18 : set_arc_geometry(origin_to_center_ne, radius, total_angle_rad);
153 18 : if (is_zero(seg_length)) {
154 0 : seg_delta.zero();
155 0 : return;
156 : }
157 :
158 : // build the jerk-limited profile from the configured geometry and limits
159 18 : generate_path(speed_xy, speed_up, speed_down, accel_xy, accel_z, accel_c, snap_maximum, jerk_maximum);
160 : }
161 :
162 : // populate the canonical straight-segment geometry from seg_delta (the net
163 : // start-to-end displacement). The arc fields are cleared and the path length is
164 : // the full 3D chord length.
165 1111 : void SCurve::set_straight_geometry()
166 : {
167 1111 : is_arc_segment = false;
168 1111 : arc.angle_rad = 0.0f;
169 1111 : arc.length_ne = seg_delta.xy().length();
170 1111 : arc.radius_ne = 0.0f;
171 1111 : arc.center_ne = Vector2f();
172 1111 : seg_length = seg_delta.length();
173 1111 : }
174 :
175 : // populate the canonical arc geometry (center relative to the origin, radius and
176 : // signed swept angle) and the resulting path length. seg_delta must already hold the
177 : // net start-to-end displacement; its vertical component sets the constant climb.
178 44 : void SCurve::set_arc_geometry(const Vector2f ¢er_ne_rel, float radius, float angle_rad)
179 : {
180 44 : is_arc_segment = true;
181 44 : arc.angle_rad = angle_rad;
182 44 : arc.radius_ne = radius;
183 44 : arc.center_ne = center_ne_rel;
184 44 : arc.length_ne = radius * fabsf(angle_rad);
185 44 : seg_length = safe_sqrt(sq(seg_delta.z) + sq(arc.length_ne));
186 44 : }
187 :
188 : // build the jerk-limited S-curve profile for the already-configured geometry
189 : // (is_arc_segment, arc.*, seg_delta, seg_length). Applies the corner-acceleration
190 : // speed limit for arcs, derives the speed and acceleration limits from the path
191 : // actually flown (arc length plus vertical component), then builds and verifies the
192 : // segment profile. On error the path is left zero length.
193 1155 : void SCurve::generate_path(float speed_xy, float speed_up, float speed_down,
194 : float accel_xy, float accel_z, float accel_c,
195 : float snap_maximum, float jerk_maximum)
196 : {
197 1155 : if (is_arc_segment) {
198 : // limit horizontal speed so centripetal acceleration stays within the corner acceleration limit
199 44 : accel_c_max = is_positive(accel_c) ? accel_c : accel_xy;
200 44 : speed_xy = MIN(speed_xy, safe_sqrt(accel_c_max * arc.radius_ne));
201 : }
202 :
203 : // set snap and jerk maxima
204 1155 : snap_max = snap_maximum;
205 1155 : jerk_max = jerk_maximum;
206 :
207 : // set speed and acceleration limits from the path actually flown: the horizontal
208 : // extent is the arc length (equal to the chord for a straight segment) and the
209 : // vertical extent is seg_delta.z
210 1155 : vel_max = kinematic_limit(arc.length_ne, seg_delta.z, speed_xy, speed_up, speed_down);
211 1155 : accel_max = kinematic_limit(arc.length_ne, seg_delta.z, accel_xy, accel_z, accel_z);
212 1155 : accel_z_max = accel_z;
213 :
214 : // avoid divide-by zeros. Path will be left as a zero length path
215 1155 : if (!is_positive(snap_max) || !is_positive(jerk_max) || !is_positive(accel_max) || !is_positive(vel_max)) {
216 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
217 0 : ::printf("SCurve::generate_path created zero length path\n");
218 : #endif
219 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
220 0 : init();
221 0 : return;
222 : }
223 :
224 1155 : add_segments(seg_length);
225 :
226 : // catch calculation errors
227 1155 : if (!valid()) {
228 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
229 0 : ::printf("SCurve::generate_path invalid path\n");
230 0 : debug();
231 : #endif
232 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
233 0 : init();
234 : }
235 : }
236 :
237 : // set maximum velocity and re-calculate the path using these limits
238 1404 : void SCurve::set_speed_max(float speed_xy, float speed_up, float speed_down)
239 : {
240 : // ensure arguments are positive
241 1404 : speed_xy = fabsf(speed_xy);
242 1404 : speed_up = fabsf(speed_up);
243 1404 : speed_down = fabsf(speed_down);
244 :
245 : // return immediately if zero length path
246 1404 : if (num_segs != segments_max) {
247 1363 : return;
248 : }
249 :
250 605 : if (is_arc_segment) {
251 : // limit horizontal speed so centripetal acceleration stays within the corner acceleration limit
252 0 : speed_xy = MIN(speed_xy, safe_sqrt(accel_c_max * arc.radius_ne));
253 : }
254 :
255 : // segment accelerations can not be changed after segment creation.
256 605 : const float track_speed_max = kinematic_limit(arc.length_ne, seg_delta.z, speed_xy, speed_up, speed_down);
257 :
258 605 : if (is_equal(vel_max, track_speed_max)) {
259 : // new speed is equal to current speed maximum so no need to change anything
260 551 : return;
261 : }
262 :
263 54 : if (is_zero(track_speed_max)) {
264 : // new speed is zero which is not supported
265 0 : return;
266 : }
267 54 : vel_max = track_speed_max;
268 :
269 54 : if (time >= segment[SEG_CONST].end_time) {
270 3 : return;
271 : }
272 :
273 : // re-calculate the s-curve path based on update speeds
274 :
275 51 : const float Pend = segment[SEG_DECEL_END].end_pos;
276 51 : float Vend = MIN(vel_max, segment[SEG_DECEL_END].end_vel);
277 :
278 51 : if (is_zero(time)) {
279 : // path has not started so we can recompute the path
280 10 : const float Vstart = MIN(vel_max, segment[SEG_INIT].end_vel);
281 10 : num_segs = SEG_INIT;
282 10 : add_segment(num_segs, 0.0f, SegmentType::CONSTANT_JERK, 0.0f, 0.0f, 0.0f, 0.0f);
283 10 : add_segments(Pend);
284 10 : set_origin_speed_max(Vstart);
285 10 : set_destination_speed_max(Vend);
286 10 : return;
287 : }
288 :
289 41 : if ((time >= segment[SEG_ACCEL_END].end_time) && (time <= segment[SEG_SPEED_CHANGE_END].end_time)) {
290 : // in the speed change phase
291 : // move speed change phase to acceleration phase to provide room for further speed adjustments
292 :
293 : // set initial segment to last acceleration segment
294 0 : segment[SEG_INIT].seg_type = SegmentType::CONSTANT_JERK;
295 0 : segment[SEG_INIT].jerk_ref = 0.0f;
296 0 : segment[SEG_INIT].end_time = segment[SEG_ACCEL_END].end_time;
297 0 : segment[SEG_INIT].end_accel = segment[SEG_ACCEL_END].end_accel;
298 0 : segment[SEG_INIT].end_vel = segment[SEG_ACCEL_END].end_vel;
299 0 : segment[SEG_INIT].end_pos = segment[SEG_ACCEL_END].end_pos;
300 :
301 : // move speed change segments to acceleration segments
302 0 : for (uint8_t i = SEG_INIT+1; i <= SEG_ACCEL_END; i++) {
303 0 : segment[i] = segment[i+7];
304 : }
305 :
306 : // set change segments to last acceleration speed
307 0 : fill_empty_segments(SEG_ACCEL_END+1, SEG_SPEED_CHANGE_END, SEG_ACCEL_END);
308 :
309 41 : } else if ((time > segment[SEG_SPEED_CHANGE_END].end_time) && (time <= segment[SEG_CONST].end_time)) {
310 : // in the constant speed phase
311 : // overwrite the acceleration and speed change phases with the current position and velocity
312 :
313 : // set initial segment to last acceleration segment
314 28 : segment[SEG_INIT].seg_type = SegmentType::CONSTANT_JERK;
315 28 : segment[SEG_INIT].jerk_ref = 0.0f;
316 28 : segment[SEG_INIT].end_time = segment[SEG_SPEED_CHANGE_END].end_time;
317 28 : segment[SEG_INIT].end_accel = 0.0f;
318 28 : segment[SEG_INIT].end_vel = segment[SEG_SPEED_CHANGE_END].end_vel;
319 28 : segment[SEG_INIT].end_pos = segment[SEG_SPEED_CHANGE_END].end_pos;
320 :
321 : // set acceleration and change segments to current constant speed
322 : float Jt_out, At_out, Vt_out, Pt_out;
323 28 : get_jerk_accel_vel_pos_at_time(time, Jt_out, At_out, Vt_out, Pt_out);
324 420 : for (uint8_t i = SEG_INIT+1; i <= SEG_SPEED_CHANGE_END; i++) {
325 392 : segment[i].seg_type = SegmentType::CONSTANT_JERK;
326 392 : segment[i].jerk_ref = 0.0f;
327 392 : segment[i].end_time = time;
328 392 : segment[i].end_accel = 0.0f;
329 392 : segment[i].end_vel = Vt_out;
330 392 : segment[i].end_pos = Pt_out;
331 : }
332 : }
333 :
334 : // adjust the INIT and ACCEL segments for new speed
335 41 : 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) ) {
336 : // path has not finished constant positive acceleration segment
337 : // reduce velocity as close to target velocity as possible
338 :
339 3 : const float Vstart = segment[SEG_INIT].end_vel;
340 :
341 : // minimum velocity that can be obtained by shortening SEG_ACCEL_MAX
342 3 : 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));
343 :
344 : float Jm, tj, t2, t4, t6;
345 3 : calculate_path(snap_max, jerk_max, Vstart, accel_max, MAX(Vmin, vel_max), Pend * 0.5f, Jm, tj, t2, t4, t6);
346 :
347 3 : uint8_t seg = SEG_INIT+1;
348 3 : add_segments_jerk(seg, tj, Jm, t2);
349 3 : add_segment_const_jerk(seg, t4, 0.0f);
350 3 : add_segments_jerk(seg, tj, -Jm, t6);
351 :
352 : // remove numerical errors
353 3 : segment[SEG_ACCEL_END].end_accel = 0.0f;
354 :
355 : // add empty speed adjust segments
356 3 : fill_empty_segments(SEG_ACCEL_END+1, SEG_CONST, SEG_ACCEL_END);
357 :
358 3 : calculate_path(snap_max, jerk_max, 0.0f, accel_max, MAX(Vmin, vel_max), Pend * 0.5f, Jm, tj, t2, t4, t6);
359 :
360 3 : seg = SEG_CONST + 1;
361 3 : add_segments_jerk(seg, tj, -Jm, t6);
362 3 : add_segment_const_jerk(seg, t4, 0.0f);
363 3 : add_segments_jerk(seg, tj, Jm, t2);
364 :
365 : // remove numerical errors
366 3 : segment[SEG_DECEL_END].end_accel = 0.0f;
367 3 : segment[SEG_DECEL_END].end_vel = MAX(0.0f, segment[SEG_DECEL_END].end_vel);
368 :
369 : // add to constant velocity segment to end at the correct position
370 3 : extend_const_vel_to(Pend);
371 : }
372 :
373 : // adjust the speed change segments (8 to 14) for new speed
374 : // start with empty speed adjust segments
375 41 : fill_empty_segments(SEG_ACCEL_END+1, SEG_SPEED_CHANGE_END, SEG_ACCEL_END);
376 41 : if (!is_equal(vel_max, segment[SEG_ACCEL_END].end_vel)) {
377 : // add velocity adjustment
378 : // check there is enough time to make velocity change
379 : // we use the approximation that the time will be distance/max_vel and 8 jerk segments
380 40 : const float L = segment[SEG_CONST].end_pos - segment[SEG_ACCEL_END].end_pos;
381 40 : float Jm = 0;
382 40 : float tj = 0;
383 40 : float t2 = 0;
384 40 : float t4 = 0;
385 40 : float t6 = 0;
386 40 : 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));
387 40 : if ((vel_max < segment[SEG_ACCEL_END].end_vel) && (jerk_time * 12.0f < L / segment[SEG_ACCEL_END].end_vel)) {
388 : // we have a problem here with small segments.
389 17 : calculate_path(snap_max, jerk_max, vel_max, accel_max, segment[SEG_ACCEL_END].end_vel, L * 0.5f, Jm, tj, t6, t4, t2);
390 17 : Jm = -Jm;
391 :
392 23 : } else if ((vel_max > segment[SEG_ACCEL_END].end_vel) && (L / (jerk_time * 12.0f) > segment[SEG_ACCEL_END].end_vel)) {
393 22 : float Vm = MIN(vel_max, L / (jerk_time * 12.0f));
394 22 : calculate_path(snap_max, jerk_max, segment[SEG_ACCEL_END].end_vel, accel_max, Vm, L * 0.5f, Jm, tj, t2, t4, t6);
395 : }
396 :
397 40 : uint8_t seg = SEG_ACCEL_END + 1;
398 40 : if (!is_zero(Jm) && !is_negative(t2) && !is_negative(t4) && !is_negative(t6)) {
399 39 : add_segments_jerk(seg, tj, Jm, t2);
400 39 : add_segment_const_jerk(seg, t4, 0.0f);
401 39 : add_segments_jerk(seg, tj, -Jm, t6);
402 :
403 : // remove numerical errors
404 39 : segment[SEG_SPEED_CHANGE_END].end_accel = 0.0f;
405 : }
406 : }
407 :
408 : // add deceleration segments
409 : // earlier check should ensure that we should always have sufficient time to stop
410 41 : uint8_t seg = SEG_CONST;
411 41 : Vend = MIN(Vend, segment[SEG_SPEED_CHANGE_END].end_vel);
412 41 : add_segment_const_jerk(seg, 0.0f, 0.0f);
413 41 : if (Vend < segment[SEG_SPEED_CHANGE_END].end_vel) {
414 : float Jm, tj, t2, t4, t6;
415 41 : 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);
416 41 : add_segments_jerk(seg, tj, -Jm, t6);
417 41 : add_segment_const_jerk(seg, t4, 0.0f);
418 41 : add_segments_jerk(seg, tj, Jm, t2);
419 : } else {
420 : // No deceleration is required
421 0 : fill_empty_segments(SEG_CONST+1, SEG_DECEL_END, SEG_CONST);
422 : }
423 :
424 : // remove numerical errors
425 41 : segment[SEG_DECEL_END].end_accel = 0.0f;
426 41 : segment[SEG_DECEL_END].end_vel = MAX(0.0f, segment[SEG_DECEL_END].end_vel);
427 :
428 : // add to constant velocity segment to end at the correct position
429 41 : extend_const_vel_to(Pend);
430 :
431 : // catch calculation errors
432 41 : if (!valid()) {
433 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
434 0 : ::printf("SCurve::set_speed_max invalid path\n");
435 0 : debug();
436 : #endif
437 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
438 0 : init();
439 : }
440 : }
441 :
442 : // set the maximum vehicle speed at the origin
443 : // returns the expected speed at the origin which will always be equal or lower than speed
444 20 : float SCurve::set_origin_speed_max(float speed)
445 : {
446 : // if path is zero length then start speed must be zero
447 20 : if (num_segs != segments_max) {
448 0 : return 0.0f;
449 : }
450 :
451 : // ensure speed is positive
452 20 : speed = fabsf(speed);
453 :
454 : // avoid re-calculating if unnecessary
455 20 : if (is_equal(segment[SEG_INIT].end_vel, speed)) {
456 10 : return speed;
457 : }
458 :
459 10 : const float Vm = segment[SEG_ACCEL_END].end_vel;
460 10 : speed = MIN(speed, Vm);
461 :
462 : float Jm, tj, t2, t4, t6;
463 10 : calculate_path(snap_max, jerk_max, speed, accel_max, Vm, seg_length * 0.5f, Jm, tj, t2, t4, t6);
464 :
465 10 : uint8_t seg = SEG_INIT;
466 10 : add_segment(seg, 0.0f, SegmentType::CONSTANT_JERK, 0.0f, 0.0f, speed, 0.0f);
467 10 : add_segments_jerk(seg, tj, Jm, t2);
468 10 : add_segment_const_jerk(seg, t4, 0.0f);
469 10 : add_segments_jerk(seg, tj, -Jm, t6);
470 :
471 : // remove numerical errors
472 10 : segment[SEG_ACCEL_END].end_accel = 0.0f;
473 :
474 : // offset acceleration segment if we can't fit it all into half the original length
475 10 : const float dPstart = MIN(0.0f, seg_length * 0.5f - segment[SEG_ACCEL_END].end_pos);
476 10 : const float dt = dPstart / segment[SEG_ACCEL_END].end_vel;
477 90 : for (uint8_t i = SEG_INIT; i <= SEG_ACCEL_END; i++) {
478 80 : segment[i].end_time += dt;
479 80 : segment[i].end_pos += dPstart;
480 : }
481 :
482 : // add empty speed change segments and constant speed segment
483 10 : fill_empty_segments(SEG_ACCEL_END+1, SEG_SPEED_CHANGE_END, SEG_ACCEL_END);
484 :
485 10 : seg = SEG_CONST;
486 10 : add_segment_const_jerk(seg, 0.0f, 0.0f);
487 :
488 10 : calculate_path(snap_max, jerk_max, 0.0f, accel_max, segment[SEG_CONST].end_vel, seg_length * 0.5f, Jm, tj, t2, t4, t6);
489 :
490 10 : add_segments_jerk(seg, tj, -Jm, t6);
491 10 : add_segment_const_jerk(seg, t4, 0.0f);
492 10 : add_segments_jerk(seg, tj, Jm, t2);
493 :
494 : // remove numerical errors
495 10 : segment[SEG_DECEL_END].end_accel = 0.0f;
496 10 : segment[SEG_DECEL_END].end_vel = MAX(0.0f, segment[SEG_DECEL_END].end_vel);
497 :
498 : // add to constant velocity segment to end at the correct position
499 10 : extend_const_vel_to(seg_length);
500 :
501 : // catch calculation errors
502 10 : if (!valid()) {
503 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
504 0 : ::printf("SCurve::set_origin_speed_max invalid path\n");
505 0 : debug();
506 : #endif
507 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
508 0 : init();
509 0 : return 0.0f;
510 : }
511 :
512 10 : return speed;
513 : }
514 :
515 : // set the maximum vehicle speed at the destination
516 12 : void SCurve::set_destination_speed_max(float speed)
517 : {
518 : // if path is zero length then all speeds must be zero
519 12 : if (num_segs != segments_max) {
520 10 : return;
521 : }
522 :
523 : // ensure speed is positive
524 12 : speed = fabsf(speed);
525 :
526 : // avoid re-calculating if unnecessary
527 12 : if (is_equal(segment[segments_max-1].end_vel, speed)) {
528 10 : return;
529 : }
530 :
531 2 : const float Vm = segment[SEG_CONST].end_vel;
532 2 : speed = MIN(speed, Vm);
533 :
534 : float Jm, tj, t2, t4, t6;
535 2 : calculate_path(snap_max, jerk_max, speed, accel_max, Vm, seg_length * 0.5f, Jm, tj, t2, t4, t6);
536 :
537 2 : uint8_t seg = SEG_CONST;
538 2 : add_segment_const_jerk(seg, 0.0f, 0.0f);
539 :
540 2 : add_segments_jerk(seg, tj, -Jm, t6);
541 2 : add_segment_const_jerk(seg, t4, 0.0f);
542 2 : add_segments_jerk(seg, tj, Jm, t2);
543 :
544 : // remove numerical errors
545 2 : segment[SEG_DECEL_END].end_accel = 0.0f;
546 2 : segment[SEG_DECEL_END].end_vel = MAX(0.0f, segment[SEG_DECEL_END].end_vel);
547 :
548 : // add to constant velocity segment to end at the correct position
549 2 : extend_const_vel_to(seg_length);
550 :
551 : // catch calculation errors
552 2 : if (!valid()) {
553 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
554 0 : ::printf("SCurve::set_destination_speed_max invalid path\n");
555 0 : debug();
556 : #endif
557 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
558 0 : init();
559 : }
560 : }
561 :
562 : // move target location along path from origin to destination
563 : // prev_leg and next_leg are the paths before and after this path
564 : // wp_radius is max distance from the waypoint at the apex of the turn
565 : // fast_waypoint should be true if vehicle will not stop at end of this leg
566 : // dt is the time increment the vehicle will move along the path
567 : // target_pos should be set to this segment's origin and it will be updated to the current position target
568 : // target_vel and target_accel are updated with new targets
569 : // returns true if vehicle has passed the apex of the corner
570 4200479 : bool SCurve::advance_target_along_track(SCurve &prev_leg, SCurve &next_leg, float wp_radius, float accel_corner, bool fast_waypoint, float dt, Vector3p &target_pos, Vector3f &target_vel, Vector3f &target_accel)
571 : {
572 4200479 : prev_leg.move_to_pos_vel_accel(dt, target_pos, target_vel, target_accel);
573 4200479 : move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel);
574 4200479 : bool s_finished = finished();
575 :
576 : // check for change of leg on fast waypoint
577 4200479 : const float time_to_destination = get_time_remaining();
578 4200479 : if (fast_waypoint
579 1056486 : && is_zero(next_leg.get_time_elapsed()) // The next leg has not started
580 888436 : && (get_time_elapsed() >= time_decel_start()) // The current leg has started the deceleration phase
581 5256965 : && (get_time_remaining() <= next_leg.time_accel_end()) // The current leg will finish before completion of the acceleration phase of the next leg
582 : ) {
583 :
584 : // Calculate the position, velocity and acceleration at the turn mid point
585 53157 : Vector3p turn_pos = -get_origin_to_destination().topostype();
586 53157 : Vector3f turn_vel, turn_accel;
587 53157 : move_from_time_pos_vel_accel(get_time_elapsed() + time_to_destination * 0.5f, turn_pos, turn_vel, turn_accel);
588 53157 : next_leg.move_from_time_pos_vel_accel(time_to_destination * 0.5f, turn_pos, turn_vel, turn_accel);
589 53157 : const float speed_min = MIN(get_speed_along_track(), next_leg.get_speed_along_track());
590 53157 : const float accel_z_lim = MIN(get_accel_z_max(), next_leg.get_accel_z_max());
591 53157 : if ((turn_pos.length() < wp_radius) // The turn mid point is within the waypoint radius
592 13667 : && (turn_vel.length() < speed_min) // The speed at the turn mid point is less than the minimum speed
593 13620 : && (Vector2f{turn_accel.x, turn_accel.y}.length() < accel_corner) // The acceleration at the turn mid point is less than the corner acceleration
594 66824 : && (fabsf(turn_accel.z) < accel_z_lim) // The vertical acceleration at the turn mid point is less than the maximum vertical acceleration
595 : ) {
596 486 : next_leg.move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel);
597 : }
598 4147322 : } else if (!is_zero(next_leg.get_time_elapsed())) {
599 168050 : next_leg.move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel);
600 168050 : if (next_leg.get_time_elapsed() >= get_time_remaining()) {
601 : // consider the current leg finished when we have passed half way through the turn between legs
602 485 : s_finished = true;
603 : }
604 : }
605 :
606 4200479 : return s_finished;
607 : }
608 :
609 : // time has reached the end of the sequence
610 4200995 : bool SCurve::finished() const
611 : {
612 4200995 : return time >= time_end();
613 : }
614 :
615 : // return the angle swept around the arc so far, in radians, unsigned and unwrapped so it grows
616 : // past 2*pi on a multi-turn arc. returns zero for a straight segment
617 46670 : float SCurve::get_arc_angle_covered_rad() const
618 : {
619 46670 : if (!is_arc_segment || !is_positive(seg_length)) {
620 0 : return 0.0;
621 : }
622 :
623 : // distance travelled along the track. This is the 3D path length, so the fraction of the
624 : // track covered is used rather than dividing by the arc radius: seg_length carries the
625 : // climb applied over the arc, which the horizontal arc length does not
626 46670 : float scurve_P1 = 0.0f;
627 : float scurve_V1, scurve_A1, scurve_J1;
628 46670 : get_jerk_accel_vel_pos_at_time(time, scurve_J1, scurve_A1, scurve_V1, scurve_P1);
629 :
630 : // this is the rotation project_scurve_onto_track() applies, so the angle reported is the
631 : // angle flown by construction
632 46670 : return fabsf(arc.angle_rad) * (scurve_P1 / seg_length);
633 : }
634 :
635 : // increment time pointer and return the position, velocity and acceleration vectors relative to the origin
636 4369015 : void SCurve::move_from_pos_vel_accel(float dt, Vector3p &pos, Vector3f &vel, Vector3f &accel)
637 : {
638 4369015 : advance_time(dt);
639 4369015 : float scurve_P1 = 0.0f;
640 : float scurve_V1, scurve_A1, scurve_J1;
641 4369015 : get_jerk_accel_vel_pos_at_time(time, scurve_J1, scurve_A1, scurve_V1, scurve_P1);
642 4369015 : project_scurve_onto_track(scurve_A1, scurve_V1, scurve_P1, pos, vel, accel);
643 4369015 : }
644 :
645 : // increment time pointer and return the position, velocity and acceleration vectors relative to the destination
646 4200479 : void SCurve::move_to_pos_vel_accel(float dt, Vector3p &pos, Vector3f &vel, Vector3f &accel)
647 : {
648 4200479 : advance_time(dt);
649 4200479 : float scurve_P1 = 0.0f;
650 : float scurve_V1, scurve_A1, scurve_J1;
651 4200479 : get_jerk_accel_vel_pos_at_time(time, scurve_J1, scurve_A1, scurve_V1, scurve_P1);
652 4200479 : project_scurve_onto_track(scurve_A1, scurve_V1, scurve_P1, pos, vel, accel);
653 4200479 : pos -= seg_delta.topostype();
654 4200479 : }
655 :
656 : // return the position, velocity and acceleration vectors relative to the origin at a specified time along the path
657 106314 : void SCurve::move_from_time_pos_vel_accel(float time_now, Vector3p &pos, Vector3f &vel, Vector3f &accel)
658 : {
659 106314 : float scurve_P1 = 0.0f;
660 106314 : float scurve_V1 = 0.0f, scurve_A1 = 0.0f, scurve_J1 = 0.0f;
661 106314 : get_jerk_accel_vel_pos_at_time(time_now, scurve_J1, scurve_A1, scurve_V1, scurve_P1);
662 106314 : project_scurve_onto_track(scurve_A1, scurve_V1, scurve_P1, pos, vel, accel);
663 106314 : }
664 :
665 : // project the straight-line S-curve motion profile onto the active track segment
666 : // converts scalar S-curve kinematics (A1, V1, P1) into 3D position, velocity and acceleration
667 : // along either a circular arc or straight segment
668 8675808 : void SCurve::project_scurve_onto_track(float scurve_A1, float scurve_V1, float scurve_P1, Vector3p &pos, Vector3f &vel, Vector3f &accel)
669 : {
670 8675808 : if (is_zero(seg_length)) {
671 2168245 : return;
672 : }
673 :
674 7042781 : if (is_arc_segment) {
675 : // Arc segment projection
676 :
677 : // protect against divide by zero
678 535218 : if (!is_positive(arc.radius_ne)) {
679 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
680 0 : ::printf("SCurve:: Arc radius is negative or zero\n");
681 : #endif
682 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
683 0 : init();
684 0 : return;
685 : }
686 :
687 535218 : Vector2f center_to_pos_ne = -arc.center_ne;
688 535218 : const float turn_dir = is_negative(arc.angle_rad) ? -1.0f : 1.0f; // -1 for CCW, 1 for CW
689 : // rotate by progress along the arc
690 535218 : center_to_pos_ne.rotate(turn_dir * fabsf(arc.angle_rad) * (scurve_P1 / seg_length));
691 :
692 : // position
693 535218 : const float dz_ds = seg_delta.z / seg_length;
694 535218 : Vector3f delta_pos(arc.center_ne + center_to_pos_ne, scurve_P1 * dz_ds);
695 535218 : pos += delta_pos.topostype();
696 :
697 : // direction unit
698 535218 : Vector2f arc_tangent_ne = Vector2f(-center_to_pos_ne.y, center_to_pos_ne.x) * turn_dir;
699 535218 : arc_tangent_ne /= arc.radius_ne;
700 535218 : const float horiz_ds = arc.length_ne / seg_length;
701 : // unit length by construction: |arc_tangent_ne| = 1 and horiz_ds^2 + dz_ds^2 = 1 from the seg_length definition
702 535218 : const Vector3f path_unit(arc_tangent_ne.x * horiz_ds, arc_tangent_ne.y * horiz_ds, dz_ds);
703 :
704 : // velocity & tangential accel
705 535218 : vel += path_unit * scurve_V1;
706 535218 : accel += path_unit * scurve_A1;
707 :
708 : // centripetal accel uses the horizontal speed component (scurve_V1 * horiz_ds)
709 535218 : accel.xy() -= center_to_pos_ne * sq(scurve_V1 * horiz_ds / arc.radius_ne);
710 :
711 535218 : return;
712 : }
713 :
714 : // Straight segment projection
715 6507563 : const Vector3f path_unit = seg_delta.normalized();
716 6507563 : pos += path_unit.topostype() * (postype_t)scurve_P1;
717 6507563 : vel += path_unit * scurve_V1;
718 6507563 : accel += path_unit * scurve_A1;
719 : }
720 :
721 : // time at the end of the sequence
722 12770489 : float SCurve::time_end() const
723 : {
724 12770489 : if (num_segs != segments_max) {
725 1709339 : return 0.0f;
726 : }
727 11061150 : return segment[SEG_DECEL_END].end_time;
728 : }
729 :
730 : // time left before sequence will complete
731 4506435 : float SCurve::get_time_remaining() const
732 : {
733 4506435 : if (num_segs != segments_max) {
734 76329 : return 0.0f;
735 : }
736 4430106 : return segment[SEG_DECEL_END].end_time - time;
737 : }
738 :
739 : // time when acceleration section of the sequence will complete
740 0 : float SCurve::get_accel_finished_time() const
741 : {
742 0 : if (num_segs != segments_max) {
743 0 : return 0.0f;
744 : }
745 0 : return segment[SEG_ACCEL_END].end_time;
746 : }
747 :
748 : // return true if the sequence is braking to a stop
749 0 : bool SCurve::braking() const
750 : {
751 0 : if (num_segs != segments_max) {
752 0 : return true;
753 : }
754 0 : return time >= segment[SEG_CONST].end_time;
755 : }
756 :
757 : // return time offset for the start of the constant speed section and end of the acceleration section
758 : // used to initiate the turn onto leg
759 137906 : float SCurve::time_accel_end() const
760 : {
761 137906 : if (num_segs != segments_max) {
762 37369 : return 0.0f;
763 : }
764 100537 : return segment[SEG_ACCEL_END].end_time;
765 : }
766 :
767 : // return time offset for the end of the constant speed section and start of the deceleration section
768 : // used to initiate the turn from leg
769 888436 : float SCurve::time_decel_start() const
770 : {
771 888436 : if (num_segs != segments_max) {
772 2 : return 0.0f;
773 : }
774 888434 : return segment[SEG_DECEL_START].end_time;
775 : }
776 :
777 : // increment the internal time
778 8569494 : void SCurve::advance_time(float dt)
779 : {
780 8569494 : time = MIN(time + dt, time_end());
781 8569494 : }
782 :
783 : // calculate the jerk, acceleration, velocity and position at the provided time
784 8722506 : 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
785 : {
786 : // start with zeros as function is void and we want to guarantee all outputs are initialised
787 8722506 : Jt_out = 0.0f;
788 8722506 : At_out = 0.0f;
789 8722506 : Vt_out = 0.0f;
790 8722506 : Pt_out = 0.0f;
791 8722506 : if (num_segs != segments_max) {
792 1633027 : return;
793 : }
794 :
795 : SegmentType Jtype;
796 7089479 : uint8_t pnt = num_segs;
797 : float Jm, tj, T0, A0, V0, P0;
798 :
799 : // find active segment at time_now
800 170147496 : for (uint8_t i = 0; i < num_segs; i++) {
801 163058017 : if (time_now < segment[num_segs - 1 - i].end_time) {
802 41728688 : pnt = num_segs - 1 - i;
803 : }
804 : }
805 7089479 : if (pnt == 0) {
806 0 : Jtype = SegmentType::CONSTANT_JERK;
807 0 : Jm = 0.0f;
808 0 : tj = 0.0f;
809 0 : T0 = segment[pnt].end_time;
810 0 : A0 = segment[pnt].end_accel;
811 0 : V0 = segment[pnt].end_vel;
812 0 : P0 = segment[pnt].end_pos;
813 7089479 : } else if (pnt == num_segs) {
814 3071042 : Jtype = SegmentType::CONSTANT_JERK;
815 3071042 : Jm = 0.0f;
816 3071042 : tj = 0.0f;
817 3071042 : T0 = segment[pnt - 1].end_time;
818 3071042 : A0 = segment[pnt - 1].end_accel;
819 3071042 : V0 = segment[pnt - 1].end_vel;
820 3071042 : P0 = segment[pnt - 1].end_pos;
821 : } else {
822 4018437 : Jtype = segment[pnt].seg_type;
823 4018437 : Jm = segment[pnt].jerk_ref;
824 4018437 : tj = segment[pnt].end_time - segment[pnt - 1].end_time;
825 4018437 : T0 = segment[pnt - 1].end_time;
826 4018437 : A0 = segment[pnt - 1].end_accel;
827 4018437 : V0 = segment[pnt - 1].end_vel;
828 4018437 : P0 = segment[pnt - 1].end_pos;
829 : }
830 :
831 7089479 : switch (Jtype) {
832 6493682 : case SegmentType::CONSTANT_JERK:
833 6493682 : calc_javp_for_segment_const_jerk(time_now - T0, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out);
834 6493682 : break;
835 295229 : case SegmentType::POSITIVE_JERK:
836 295229 : calc_javp_for_segment_incr_jerk(time_now - T0, tj, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out);
837 295229 : break;
838 300568 : case SegmentType::NEGATIVE_JERK:
839 300568 : calc_javp_for_segment_decr_jerk(time_now - T0, tj, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out);
840 300568 : break;
841 : }
842 7089479 : Pt_out = MAX(0.0f, Pt_out);
843 : }
844 :
845 : // calculate the jerk, acceleration, velocity and position at time time_now when running the constant jerk time segment
846 6493682 : 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
847 : {
848 6493682 : Jt = J0;
849 6493682 : At = A0 + J0 * time_now;
850 6493682 : Vt = V0 + A0 * time_now + 0.5f * J0 * (time_now * time_now);
851 6493682 : Pt = P0 + V0 * time_now + 0.5f * A0 * (time_now * time_now) + (1.0f / 6.0f) * J0 * (time_now * time_now * time_now);
852 6493682 : }
853 :
854 : // 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
855 295229 : 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
856 : {
857 295229 : if (!is_positive(tj)) {
858 0 : Jt = 0.0f;
859 0 : At = A0;
860 0 : Vt = V0;
861 0 : Pt = P0;
862 0 : return;
863 : }
864 295229 : const float Alpha = Jm * 0.5f;
865 295229 : const float Beta = M_PI / tj;
866 295229 : Jt = Alpha * (1.0f - cosf(Beta * time_now));
867 295229 : At = A0 + Alpha * time_now - (Alpha / Beta) * sinf(Beta * time_now);
868 295229 : Vt = V0 + A0 * time_now + (Alpha * 0.5f) * (time_now * time_now) + (Alpha / (Beta * Beta)) * cosf(Beta * time_now) - Alpha / (Beta * Beta);
869 295229 : 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);
870 : }
871 :
872 : // 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
873 300568 : 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
874 : {
875 300568 : if (!is_positive(tj)) {
876 0 : Jt = 0.0f;
877 0 : At = A0;
878 0 : Vt = V0;
879 0 : Pt = P0;
880 0 : return;
881 : }
882 300568 : const float Alpha = Jm * 0.5f;
883 300568 : const float Beta = M_PI / tj;
884 300568 : const float AT = Alpha * tj;
885 300568 : const float VT = Alpha * ((tj * tj) * 0.5f - 2.0f / (Beta * Beta));
886 300568 : const float PT = Alpha * ((-1.0f / (Beta * Beta)) * tj + (1.0f / 6.0f) * (tj * tj * tj));
887 300568 : Jt = Alpha * (1.0f - cosf(Beta * (time_now + tj)));
888 300568 : At = (A0 - AT) + Alpha * (time_now + tj) - (Alpha / Beta) * sinf(Beta * (time_now + tj));
889 300568 : 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);
890 300568 : 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));
891 : }
892 :
893 : // generate the segments for a path of length L
894 : // the path consists of 23 segments
895 : // 1 initial segment
896 : // 7 segments forming the acceleration S-Curve
897 : // 7 segments forming the velocity change S-Curve
898 : // 1 constant velocity S-Curve
899 : // 7 segments forming the deceleration S-Curve
900 1165 : void SCurve::add_segments(float L)
901 : {
902 1165 : if (is_zero(L)) {
903 0 : return;
904 : }
905 :
906 : float Jm, tj, t2, t4, t6;
907 1165 : calculate_path(snap_max, jerk_max, 0.0f, accel_max, vel_max, L * 0.5f, Jm, tj, t2, t4, t6);
908 :
909 1165 : add_segments_jerk(num_segs, tj, Jm, t2);
910 1165 : add_segment_const_jerk(num_segs, t4, 0.0f);
911 1165 : add_segments_jerk(num_segs, tj, -Jm, t6);
912 :
913 : // remove numerical errors
914 1165 : segment[SEG_ACCEL_END].end_accel = 0.0f;
915 :
916 : // add empty speed adjust segments
917 9320 : for (uint8_t i = SEG_ACCEL_END + 1; i <= SEG_SPEED_CHANGE_END; i++) {
918 8155 : add_segment_const_jerk(num_segs, 0.0f, 0.0f);
919 : }
920 :
921 1165 : const float t15 = MAX(0.0f, (L - 2.0f * segment[SEG_SPEED_CHANGE_END].end_pos) / segment[SEG_SPEED_CHANGE_END].end_vel);
922 1165 : add_segment_const_jerk(num_segs, t15, 0.0f);
923 :
924 1165 : add_segments_jerk(num_segs, tj, -Jm, t6);
925 1165 : add_segment_const_jerk(num_segs, t4, 0.0f);
926 1165 : add_segments_jerk(num_segs, tj, Jm, t2);
927 :
928 : // remove numerical errors
929 1165 : segment[SEG_DECEL_END].end_accel = 0.0f;
930 1165 : segment[SEG_DECEL_END].end_vel = 0.0f;
931 : }
932 :
933 : // calculate the segment times for the trigonometric S-Curve path defined by:
934 : // Sm - duration of the raised cosine jerk profile
935 : // Jm - maximum value of the raised cosine jerk profile
936 : // V0 - initial velocity magnitude
937 : // Am - maximum constant acceleration
938 : // Vm - maximum constant velocity
939 : // L - Length of the path
940 : // tj_out, t2_out, t4_out, t6_out are the segment durations needed to achieve the kinematic path specified by the input variables
941 1401 : 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)
942 : {
943 : // init outputs
944 1401 : Jm_out = 0.0f;
945 1401 : tj_out = 0.0f;
946 1401 : t2_out = 0.0f;
947 1401 : t4_out = 0.0f;
948 1401 : t6_out = 0.0f;
949 :
950 : // check for invalid arguments
951 1401 : if (!is_positive(Sm) || !is_positive(Jm) || !is_positive(Am) || !is_positive(Vm) || !is_positive(L)) {
952 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
953 0 : ::printf("SCurve::calculate_path invalid inputs\n");
954 : #endif
955 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
956 33 : return;
957 : }
958 :
959 1401 : if (V0 >= Vm) {
960 : // no velocity change so all segments as zero length
961 17 : return;
962 : }
963 :
964 1384 : float tj = Jm * M_PI / (2.0f * Sm);
965 1384 : float At = MIN(MIN(Am,
966 2768 : (Vm - V0) / (2.0f * tj) ),
967 1384 : (L - 4.0f * V0 * tj) / (4.0f * sq(tj)) );
968 1384 : if (!is_positive(At)) {
969 16 : return;
970 : }
971 1368 : if (fabsf(At) < Jm * tj) {
972 326 : if (is_zero(V0)) {
973 : // we do not have a solution for non-zero initial velocity
974 294 : tj = MIN( MIN( MIN( tj,
975 588 : powf((L * M_PI) / (8.0f * Sm), 1.0f / 4.0f) ),
976 588 : powf((Vm * M_PI) / (4.0f * Sm), 1.0f / 3.0f) ),
977 294 : safe_sqrt((Am * M_PI) / (2.0f * Sm)) );
978 294 : Jm = 2.0f * Sm * tj / M_PI;
979 294 : Am = Jm * tj;
980 : } else {
981 : // When doing speed change we use fixed tj and adjust Jm for small changes
982 32 : Am = At;
983 32 : Jm = Am / tj;
984 : }
985 326 : if ((Vm <= V0 + 2.0f * Am * tj) || (L <= 4.0f * V0 * tj + 4.0f * Am * sq(tj))) {
986 : // solution = 0 - t6 t4 t2 = 0 0 0
987 167 : t2_out = 0.0f;
988 167 : t4_out = 0.0f;
989 167 : t6_out = 0.0f;
990 : } else {
991 : // solution = 2 - t6 t4 t2 = 0 1 0
992 159 : t2_out = 0.0f;
993 159 : 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)));
994 159 : t4_out = MAX(t4_out, 0.0f);
995 159 : t6_out = 0.0f;
996 : }
997 : } else {
998 1042 : 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))) {
999 : // solution = 5 - t6 t4 t2 = 1 0 1
1000 501 : 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));
1001 501 : t2_out = Am / Jm - tj;
1002 501 : t4_out = 0.0f;
1003 501 : t6_out = t2_out;
1004 : } else {
1005 : // solution = 7 - t6 t4 t2 = 1 1 1
1006 541 : t2_out = Am / Jm - tj;
1007 541 : 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)));
1008 541 : t4_out = MAX(t4_out, 0.0f);
1009 541 : t6_out = t2_out;
1010 : }
1011 : }
1012 1368 : tj_out = tj;
1013 1368 : Jm_out = Jm;
1014 :
1015 : // check outputs and reset back to zero if necessary
1016 2736 : if (!isfinite(Jm_out) || is_negative(Jm_out) ||
1017 1368 : !isfinite(tj_out) || is_negative(tj_out) ||
1018 1368 : !isfinite(t2_out) || is_negative(t2_out) ||
1019 1368 : !isfinite(t4_out) || is_negative(t4_out) ||
1020 2736 : !isfinite(t6_out) || is_negative(t6_out)) {
1021 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1022 0 : ::printf("SCurve::calculate_path invalid outputs\n");
1023 : #endif
1024 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
1025 :
1026 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_Blimp)
1027 : // @LoggerMessage: SCVE
1028 : // @Description: Debug message for SCurve internal error
1029 : // @Field: TimeUS: Time since system startup
1030 : // @Field: Sm: duration of the raised cosine jerk profile
1031 : // @Field: Jm: maximum value of the raised cosine jerk profile
1032 : // @Field: V0: initial velocity magnitude
1033 : // @Field: Am: maximum constant acceleration
1034 : // @Field: Vm: maximum constant velocity
1035 : // @Field: L: Length of the path
1036 : // @Field: Jm_out: maximum value of the raised cosine jerk profile
1037 : // @Field: tj_out: segment duration
1038 : // @Field: t2_out: segment duration
1039 : // @Field: t4_out: segment duration
1040 : // @Field: t6_out: segment duration
1041 :
1042 : #if HAL_LOGGING_ENABLED
1043 : static bool logged_scve; // only log once
1044 0 : if (!logged_scve) {
1045 0 : logged_scve = true;
1046 0 : AP::logger().Write(
1047 : "SCVE",
1048 : "TimeUS,Sm,Jm,V0,Am,Vm,L,Jm_out,tj_out,t2_out,t4_out,t6_out",
1049 : "s-----------",
1050 : "F-----------",
1051 : "Qfffffffffff",
1052 : AP_HAL::micros64(),
1053 : (double)Sm,
1054 : (double)Jm,
1055 : (double)V0,
1056 : (double)Am,
1057 : (double)Vm,
1058 : (double)L,
1059 0 : (double)Jm_out,
1060 0 : (double)tj_out,
1061 0 : (double)t2_out,
1062 0 : (double)t4_out,
1063 0 : (double)t6_out
1064 : );
1065 : }
1066 : #endif // HAL_LOGGING_ENABLED
1067 :
1068 : #endif // APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_Blimp)
1069 :
1070 0 : Jm_out = 0.0f;
1071 0 : t2_out = 0.0f;
1072 0 : t4_out = 0.0f;
1073 0 : t6_out = 0.0f;
1074 : }
1075 : }
1076 :
1077 : // generate three consecutive segments forming a jerk profile
1078 : // the index variable is the position within the path array that this jerk profile should be added
1079 : // the index is incremented to reference the next segment in the array after the jerk profile
1080 4876 : void SCurve::add_segments_jerk(uint8_t &index, float tj, float Jm, float Tcj)
1081 : {
1082 4876 : add_segment_incr_jerk(index, tj, Jm);
1083 4876 : add_segment_const_jerk(index, Tcj, Jm);
1084 4876 : add_segment_decr_jerk(index, tj, Jm);
1085 4876 : }
1086 :
1087 : // generate constant jerk time segment
1088 : // calculate the information needed to populate the constant jerk segment from the segment duration tj and jerk J0
1089 : // the index variable is the position of this segment in the path array and is incremented to reference the next segment in the array
1090 16687 : void SCurve::add_segment_const_jerk(uint8_t &index, float tj, float J0)
1091 : {
1092 : // if no time increase copy previous segment
1093 16687 : if (!is_positive(tj)) {
1094 11186 : add_segment(index, segment[index - 1].end_time,
1095 : SegmentType::CONSTANT_JERK,
1096 : J0,
1097 11186 : segment[index - 1].end_accel,
1098 11186 : segment[index - 1].end_vel,
1099 11186 : segment[index - 1].end_pos);
1100 11186 : return;
1101 : }
1102 :
1103 5501 : const float J = J0;
1104 5501 : const float T = segment[index - 1].end_time + tj;
1105 5501 : const float A = segment[index - 1].end_accel + J0 * tj;
1106 5501 : const float V = segment[index - 1].end_vel + segment[index - 1].end_accel * tj + 0.5f * J0 * sq(tj);
1107 5501 : 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);
1108 5501 : add_segment(index, T, SegmentType::CONSTANT_JERK, J, A, V, P);
1109 : }
1110 :
1111 : // generate increasing jerk magnitude time segment based on a raised cosine profile
1112 : // calculate the information needed to populate the increasing jerk magnitude segment from the segment duration tj and jerk magnitude Jm
1113 : // the index variable is the position of this segment in the path array and is incremented to reference the next segment in the array
1114 4876 : void SCurve::add_segment_incr_jerk(uint8_t &index, float tj, float Jm)
1115 : {
1116 : // if no time increase copy previous segment
1117 4876 : if (!is_positive(tj)) {
1118 2 : add_segment(index, segment[index - 1].end_time,
1119 : SegmentType::CONSTANT_JERK,
1120 : 0.0f,
1121 2 : segment[index - 1].end_accel,
1122 2 : segment[index - 1].end_vel,
1123 2 : segment[index - 1].end_pos);
1124 2 : return;
1125 : }
1126 4874 : const float Beta = M_PI / tj;
1127 4874 : const float Alpha = Jm * 0.5f;
1128 4874 : const float AT = Alpha * tj;
1129 4874 : const float VT = Alpha * (sq(tj) * 0.5f - 2.0f / sq(Beta));
1130 4874 : const float PT = Alpha * ((-1.0f / sq(Beta)) * tj + (1.0f / 6.0f) * powf(tj, 3.0f));
1131 :
1132 4874 : const float J = Jm;
1133 4874 : const float T = segment[index - 1].end_time + tj;
1134 4874 : const float A = segment[index - 1].end_accel + AT;
1135 4874 : const float V = segment[index - 1].end_vel + segment[index - 1].end_accel * tj + VT;
1136 4874 : const float P = segment[index - 1].end_pos + segment[index - 1].end_vel * tj + 0.5f * segment[index - 1].end_accel * sq(tj) + PT;
1137 4874 : add_segment(index, T, SegmentType::POSITIVE_JERK, J, A, V, P);
1138 : }
1139 :
1140 : // generate decreasing jerk magnitude time segment based on a raised cosine profile
1141 : // calculate the information needed to populate the decreasing jerk magnitude segment from the segment duration tj and jerk magnitude Jm
1142 : // the index variable is the position of this segment in the path and is incremented to reference the next segment in the array
1143 4876 : void SCurve::add_segment_decr_jerk(uint8_t &index, float tj, float Jm)
1144 : {
1145 : // if no time increase copy previous segment
1146 4876 : if (!is_positive(tj)) {
1147 2 : add_segment(index, segment[index - 1].end_time,
1148 : SegmentType::CONSTANT_JERK,
1149 : 0.0f,
1150 2 : segment[index - 1].end_accel,
1151 2 : segment[index - 1].end_vel,
1152 2 : segment[index - 1].end_pos);
1153 2 : return;
1154 : }
1155 4874 : const float Beta = M_PI / tj;
1156 4874 : const float Alpha = Jm * 0.5f;
1157 4874 : const float AT = Alpha * tj;
1158 4874 : const float VT = Alpha * (sq(tj) * 0.5f - 2.0f / sq(Beta));
1159 4874 : const float PT = Alpha * ((-1.0f / sq(Beta)) * tj + (1.0f / 6.0f) * powf(tj, 3.0f));
1160 4874 : const float A2T = Jm * tj;
1161 4874 : const float V2T = Jm * sq(tj);
1162 4874 : const float P2T = Alpha * ((-1.0f / sq(Beta)) * 2.0f * tj + (4.0f / 3.0f) * powf(tj, 3.0f));
1163 :
1164 4874 : const float J = Jm;
1165 4874 : const float T = segment[index - 1].end_time + tj;
1166 4874 : const float A = (segment[index - 1].end_accel - AT) + A2T;
1167 4874 : const float V = (segment[index - 1].end_vel - VT) + (segment[index - 1].end_accel - AT) * tj + V2T;
1168 4874 : 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;
1169 4874 : add_segment(index, T, SegmentType::NEGATIVE_JERK, J, A, V, P);
1170 : }
1171 :
1172 : // extend the constant velocity segment so the deceleration finishes at target_pos
1173 56 : void SCurve::extend_const_vel_to(float target_pos)
1174 : {
1175 56 : const float dP = MAX(0.0f, target_pos - segment[SEG_DECEL_END].end_pos);
1176 56 : const float t15 = is_positive(segment[SEG_CONST].end_vel) ? dP / segment[SEG_CONST].end_vel : 0.0f;
1177 504 : for (uint8_t i = SEG_CONST; i <= SEG_DECEL_END; i++) {
1178 448 : segment[i].end_time += t15;
1179 448 : segment[i].end_pos += dP;
1180 : }
1181 56 : }
1182 :
1183 : // fill segment[first..last] with zero-delta constant-jerk segments anchored to segment[src]
1184 54 : void SCurve::fill_empty_segments(uint8_t first, uint8_t last, uint8_t src)
1185 : {
1186 435 : for (uint8_t i = first; i <= last; i++) {
1187 381 : segment[i].seg_type = SegmentType::CONSTANT_JERK;
1188 381 : segment[i].jerk_ref = 0.0f;
1189 381 : segment[i].end_time = segment[src].end_time;
1190 381 : segment[i].end_accel = 0.0f;
1191 381 : segment[i].end_vel = segment[src].end_vel;
1192 381 : segment[i].end_pos = segment[src].end_pos;
1193 : }
1194 54 : }
1195 :
1196 : // add single S-Curve segment
1197 : // populate the information for the segment specified in the path by the index variable.
1198 : // the index variable is incremented to reference the next segment in the array
1199 80920 : 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)
1200 : {
1201 80920 : segment[index].end_time = end_time;
1202 80920 : segment[index].seg_type = seg_type;
1203 80920 : segment[index].jerk_ref = jerk_ref;
1204 80920 : segment[index].end_accel = end_accel;
1205 80920 : segment[index].end_vel = end_vel;
1206 80920 : segment[index].end_pos = end_pos;
1207 80920 : index++;
1208 80920 : }
1209 :
1210 : // return true if the curve is valid. Used to identify and protect against code errors
1211 1208 : bool SCurve::valid() const
1212 : {
1213 : // check number of segments
1214 1208 : if (num_segs != segments_max) {
1215 0 : return false;
1216 : }
1217 :
1218 28992 : for (uint8_t i = 0; i < num_segs; i++) {
1219 : // jerk_ref should be finite (i.e. not NaN or infinity)
1220 : // time, accel, vel and pos should finite and not negative
1221 27784 : if (!isfinite(segment[i].jerk_ref) ||
1222 27784 : !isfinite(segment[i].end_time) ||
1223 27784 : !isfinite(segment[i].end_accel) ||
1224 83352 : !isfinite(segment[i].end_vel) || is_negative(segment[i].end_vel) ||
1225 27784 : !isfinite(segment[i].end_pos)) {
1226 0 : return false;
1227 : }
1228 :
1229 : // time and pos should be increasing
1230 27784 : if (i >= 1) {
1231 53152 : if (is_negative(segment[i].end_time - segment[i-1].end_time) ||
1232 26576 : is_negative(segment[i].end_pos - segment[i-1].end_pos)) {
1233 0 : return false;
1234 : }
1235 : }
1236 : }
1237 :
1238 : // last segment should have zero acceleration
1239 1208 : if (!is_zero(segment[num_segs-1].end_accel)) {
1240 0 : return false;
1241 : }
1242 :
1243 : // if we get this far then the curve must be valid
1244 1208 : return true;
1245 : }
1246 :
1247 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1248 : // debugging messages
1249 0 : void SCurve::debug() const
1250 : {
1251 0 : ::printf("num_segs:%u, time:%4.2f, snap_max:%4.2f, jerk_max:%4.2f, accel_max:%4.2f, vel_max:%4.2f\n",
1252 0 : (unsigned)num_segs, (double)time, (double)snap_max, (double)jerk_max, (double)accel_max, (double)vel_max);
1253 0 : ::printf("T, Jt, J, A, V, P \n");
1254 0 : for (uint8_t i = 0; i < num_segs; i++) {
1255 0 : ::printf("i:%u, T:%4.2f, Jtype:%4.2f, J:%4.2f, A:%4.2f, V: %4.2f, P: %4.2f\n",
1256 0 : (unsigned)i, (double)segment[i].end_time, (double)segment[i].seg_type, (double)segment[i].jerk_ref,
1257 0 : (double)segment[i].end_accel, (double)segment[i].end_vel, (double)segment[i].end_pos);
1258 : }
1259 0 : ::printf("track x:%4.2f, y:%4.2f, z:%4.2f\n", (double)seg_delta.x, (double)seg_delta.y, (double)seg_delta.z);
1260 0 : ::printf("arc_center_ne x:%4.2f, y:%4.2f\n", (double)arc.center_ne.x, (double)arc.center_ne.y);
1261 0 : }
1262 : #endif
|
