Line data Source code
1 : #pragma once
2 :
3 : #include <AP_Common/AP_Common.h>
4 :
5 : /*
6 : * SCurves calculate paths between waypoints (including the corners) using specified speed, acceleration and jerk limits
7 : *
8 : * How to use:
9 : * 1. create three SCurve objects called something like "prev_leg", "this_leg" and "next_leg"
10 : * 2. call this_leg.calculate_track() to calculate the path from the origin to the destination for the given speed, accel and jerk limits
11 : * 3. if the vehicle will fly past the destination to another "next destination":
12 : * a) call next_leg.calculate_track() with the appropriate arguments
13 : * b) set a "fast_waypoint" boolean to true. this will be passed into "advance_target_along_track()" in the next step
14 : * if there is no "next destination"
15 : * a) call next_leg.init()
16 : * b) set the "fast_waypoint" boolean to false
17 : * 4. call this_leg.advance_target_along_track() with a small "dt" value and retrieve the resulting target position, velocity and acceleration
18 : * Note: the target_pos should be set to the segments's earth frame origin before this function is called
19 : * 5. pass the target position, velocity and acceleration into the position controller
20 : * 6. repeat steps 4 and 5 until finished() returns true
21 : * 7. promote the legs:
22 : * a) set prev_leg = this_leg
23 : * b) set this_leg = next_leg
24 : * c) jump back to step 3
25 : *
26 : * Other features:
27 : * 1. set_speed_max() allows changing the max speeds mid path. The path will be recalculated
28 : * 2. set_origin_speed_max() and set_destination_speed_max() allows setting the speed along the path at the beginning and end of the leg
29 : * this is used to smoothly integrate with spline segments
30 : *
31 : * This library works with any units (meters, cm, etc) as long as they are used consistently.
32 : * e.g. if origin and destination are meters, speeds should be in m/s, accel in m/s/s, etc.
33 : *
34 : * Terminology:
35 : * position: a point in space
36 : * velocity: rate of change of position. aka speed
37 : * acceleration: rate of change of speed
38 : * jerk: rate of change of acceleration
39 : * snap: rate of change of jerk
40 : * jerk time: the time (in seconds) for jerk to increase from zero to its maximum value
41 : * track: 3D path that the vehicle will follow
42 : * path: position, velocity, accel and jerk kinematic profile that this library generates
43 : */
44 :
45 : class SCurve {
46 :
47 : public:
48 :
49 : // constructor
50 : SCurve();
51 :
52 : // initialise and clear the path
53 : void init();
54 :
55 : // generate a trigonometric track in 3D space that moves over a straight line
56 : // between two points defined by the origin and destination
57 : void calculate_track(const Vector3p &origin, const Vector3p &destination, float arc_rad,
58 : float speed_xy, float speed_up, float speed_down,
59 : float accel_xy, float accel_z, float accel_c,
60 : float snap_maximum, float jerk_maximum);
61 :
62 : // generate a trigonometric track that follows a circular arc about center_ne
63 : // total_angle_rad is the signed swept angle and may exceed 2*pi for multiple turns
64 : // climb_d_m is the net change along the down (D) axis, applied linearly over the arc
65 : // the arc radius is taken as the distance from origin to center, so the origin lies on the circle
66 : void calculate_circle_track(const Vector3p &origin, const Vector2f ¢er_ne,
67 : float total_angle_rad, float climb_d_m,
68 : float speed_xy, float speed_up, float speed_down,
69 : float accel_xy, float accel_z, float accel_c,
70 : float snap_maximum, float jerk_maximum);
71 :
72 : // set maximum velocity and re-calculate the path using these limits
73 : void set_speed_max(float speed_xy, float speed_up, float speed_down);
74 :
75 : // set the maximum vehicle speed at the origin
76 : // returns the expected speed at the origin which will always be equal or lower than speed
77 : float set_origin_speed_max(float speed);
78 :
79 : // set the maximum vehicle speed at the destination
80 : void set_destination_speed_max(float speed);
81 :
82 : // move target location along path from origin to destination
83 : // prev_leg and next_leg - the paths before and after this path
84 : // wp_radius - max distance from the waypoint at the apex of the turn
85 : // accel_corner - max acceleration that the aircraft may use during the corner
86 : // fast_waypoint - true if vehicle will not stop at end of this leg
87 : // dt - the time increment the vehicle will move along the path
88 : // target_pos - set to this segment's origin and it will be updated to the current position target
89 : // target_vel and target_accel - updated with new targets
90 : // advance_target_along_track returns true if vehicle has passed the apex of the corner
91 : bool 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) WARN_IF_UNUSED;
92 :
93 : // time has reached the end of the sequence
94 : bool finished() const WARN_IF_UNUSED;
95 :
96 : // return the vector from the origin to the destination
97 : // (for an arc segment this is the chord between the endpoints, not the path flown)
98 53161 : const Vector3f& get_origin_to_destination() const WARN_IF_UNUSED { return seg_delta; };
99 :
100 : // return the angle swept around the arc so far, in radians, unsigned and unwrapped so it
101 : // grows past 2*pi on a multi-turn arc. returns zero for a straight segment
102 : float get_arc_angle_covered_rad() const WARN_IF_UNUSED;
103 :
104 : // calculate the segment times for the trigonometric S-Curve path defined by:
105 : // Sm - maximum value of the snap profile
106 : // Jm - maximum value of the raised cosine jerk profile
107 : // V0 - initial velocity magnitude
108 : // Am - maximum constant acceleration
109 : // Vm - maximum constant velocity
110 : // L - Length of the path
111 : // tj_out, t2_out, t4_out, t6_out are the segment durations needed to achieve the kinematic path specified by the input variables
112 : // this is an internal function, static for test suite
113 : 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);
114 :
115 : private:
116 :
117 : // increment time and return the position, velocity and acceleration vectors relative to the origin
118 : void move_from_pos_vel_accel(float dt, Vector3p &pos, Vector3f &vel, Vector3f &accel);
119 :
120 : // increment time and return the position, velocity and acceleration vectors relative to the destination
121 : void move_to_pos_vel_accel(float dt, Vector3p &pos, Vector3f &vel, Vector3f &accel);
122 :
123 : // return the position, velocity and acceleration vectors relative to the origin at a specified time along the path
124 : void move_from_time_pos_vel_accel(float t, Vector3p &pos, Vector3f &vel, Vector3f &accel);
125 :
126 : // project the straight-line S-curve motion profile onto the active track segment
127 : // converts scalar S-curve kinematics (A1, V1, P1) into 3D position, velocity and acceleration
128 : // along either a circular arc or straight segment
129 : void project_scurve_onto_track(float scurve_A1, float scurve_V1, float scurve_P1, Vector3p &pos, Vector3f &vel, Vector3f &accel);
130 :
131 : // get desired maximum speed along track
132 106314 : float get_speed_along_track() const WARN_IF_UNUSED { return vel_max; }
133 :
134 : // get desired maximum acceleration along track
135 : float get_accel_along_track() const WARN_IF_UNUSED { return accel_max; }
136 :
137 : // get desired maximum acceleration along track
138 106314 : float get_accel_z_max() const WARN_IF_UNUSED { return accel_z_max; }
139 :
140 : // return the current time elapsed
141 6313451 : float get_time_elapsed() const WARN_IF_UNUSED { return time; }
142 :
143 : // time at the end of the sequence
144 : float time_end() const WARN_IF_UNUSED;
145 :
146 : // time left before sequence will complete
147 : float get_time_remaining() const WARN_IF_UNUSED;
148 :
149 : // time when acceleration section of the sequence will complete
150 : float get_accel_finished_time() const WARN_IF_UNUSED;
151 :
152 : // return true if the sequence is braking to a stop
153 : bool braking() const WARN_IF_UNUSED;
154 :
155 : // return time offset used to initiate the turn onto leg
156 : float time_accel_end() const WARN_IF_UNUSED;
157 :
158 : // return time offset used to initiate the turn from leg
159 : float time_decel_start() const WARN_IF_UNUSED;
160 :
161 : // increment the internal time
162 : void advance_time(float dt);
163 :
164 : // calculate the jerk, acceleration, velocity and position at time t
165 : void get_jerk_accel_vel_pos_at_time(float time_now, float &Jt_out, float &At_out, float &Vt_out, float &Pt_out) const;
166 :
167 : // calculate the jerk, acceleration, velocity and position at time t when running the constant jerk time segment
168 : void 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;
169 :
170 : // Calculate the jerk, acceleration, velocity and position at time t when running the increasing jerk magnitude time segment based on a raised cosine profile
171 : void 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;
172 :
173 : // Calculate the jerk, acceleration, velocity and position at time t when running the decreasing jerk magnitude time segment based on a raised cosine profile
174 : void 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;
175 :
176 : // generate time segments for straight segment
177 : void add_segments(float L);
178 :
179 : // generate three time segments forming the jerk profile
180 : void add_segments_jerk(uint8_t &seg_pnt, float Jm, float tj, float Tcj);
181 :
182 : // generate constant jerk time segment
183 : void add_segment_const_jerk(uint8_t &seg_pnt, float J0, float tin);
184 :
185 : // generate increasing jerk magnitude time segment based on a raised cosine profile
186 : void add_segment_incr_jerk(uint8_t &seg_pnt, float Jm, float tj);
187 :
188 : // generate decreasing jerk magnitude time segment based on a raised cosine profile
189 : void add_segment_decr_jerk(uint8_t &seg_pnt, float Jm, float tj);
190 :
191 : // extend the constant velocity segment so the deceleration finishes at target_pos
192 : void extend_const_vel_to(float target_pos);
193 :
194 : // fill segment[first..last] with zero-delta constant-jerk segments anchored to segment[src]
195 : void fill_empty_segments(uint8_t first, uint8_t last, uint8_t src);
196 :
197 : // populate the canonical straight-segment geometry from seg_delta
198 : void set_straight_geometry();
199 :
200 : // populate the canonical arc geometry (center relative to origin, radius and signed swept angle)
201 : // seg_delta (its vertical component) must already be set
202 : void set_arc_geometry(const Vector2f ¢er_ne_rel, float radius, float angle_rad);
203 :
204 : // build the jerk-limited profile from the already-configured geometry and the given limits
205 : void generate_path(float speed_xy, float speed_up, float speed_down,
206 : float accel_xy, float accel_z, float accel_c,
207 : float snap_maximum, float jerk_maximum);
208 :
209 : // return true if the curve is valid. Used to identify and protect against code errors
210 : bool valid() const WARN_IF_UNUSED;
211 :
212 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
213 : // debugging messages
214 : void debug() const;
215 : #endif
216 :
217 : // segment types
218 : enum class SegmentType {
219 : CONSTANT_JERK,
220 : POSITIVE_JERK,
221 : NEGATIVE_JERK
222 : };
223 :
224 : // add single segment
225 : 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);
226 :
227 : // members
228 : float snap_max; // maximum snap magnitude
229 : float jerk_max; // maximum jerk magnitude
230 : float accel_max; // maximum acceleration magnitude
231 : float accel_c_max; // maximum cornering acceleration magnitude
232 : float accel_z_max; // maximum acceleration magnitude
233 : float vel_max; // maximum velocity magnitude
234 : float time; // time that defines position on the path
235 :
236 : // segment 0 is the initial segment and holds the vehicle's initial position and velocity
237 : // segments 1 to 7 are the acceleration segments
238 : // segments 8 to 14 are the speed change segments
239 : // segment 15 is the constant velocity segment
240 : // segment 16 to 22 is the deceleration segment
241 : const static uint8_t segments_max = 23; // maximum number of time segments
242 :
243 : uint8_t num_segs; // number of time segments being used
244 : struct {
245 : float jerk_ref; // jerk reference value for time segment (the jerk at the beginning, middle or end depending upon the segment type)
246 : SegmentType seg_type; // segment type (jerk is constant, increasing or decreasing)
247 : float end_time; // final time value for segment
248 : float end_accel; // final acceleration value for segment
249 : float end_vel; // final velocity value for segment
250 : float end_pos; // final position value for segment
251 : } segment[segments_max];
252 :
253 : bool is_arc_segment; // true if this segment is a circular arc, false if straight line
254 : Vector3f seg_delta; // total displacement vector from start to end point (NED frame)
255 : float seg_length; // 3D scalar length of the path (arc length + vertical component)
256 :
257 : struct {
258 : float angle_rad; // signed central angle of the arc [rad] (+CCW, -CW), 0 = straight
259 : float length_ne; // horizontal arc length along the circle (R * |theta|)
260 : float radius_ne; // arc radius
261 : Vector2f center_ne; // center of the circle in local NE plane, relative to start point
262 : } arc;
263 : };
|
