Line data Source code
1 : /*
2 : * control.cpp
3 : * Copyright (C) Leonard Hall 2020
4 : *
5 : * This file is free software: you can redistribute it and/or modify it
6 : * under the terms of the GNU General Public License as published by the
7 : * Free Software Foundation, either version 3 of the License, or
8 : * (at your option) any later version.
9 : *
10 : * This file is distributed in the hope that it will be useful, but
11 : * WITHOUT ANY WARRANTY; without even the implied warranty of
12 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 : * See the GNU General Public License for more details.
14 : *
15 : * You should have received a copy of the GNU General Public License along
16 : * with this program. If not, see <http://www.gnu.org/licenses/>.
17 : */
18 :
19 : /*
20 : * this module provides common controller functions
21 : */
22 : #include "AP_Math.h"
23 : #include "vector2.h"
24 : #include "vector3.h"
25 : #include <AP_InternalError/AP_InternalError.h>
26 :
27 : // control default definitions
28 : #define CORNER_ACCELERATION_RATIO 1.0/safe_sqrt(2.0) // acceleration reduction to enable zero overshoot corners
29 :
30 : // Normalised cross-track reference magnitude below which limit_accel_xy() fades
31 : // out cross-track prioritisation. The reference is expected normalised by the
32 : // maximum speed, so this is a fraction of max speed (0.25 -> 25%). When the
33 : // reference is small its direction is ill-defined, so prioritising "cross-track"
34 : // acceleration re-projects a saturated command onto a rapidly rotating axis and
35 : // injects a lateral acceleration spike (e.g. the roll wobble seen at the
36 : // zero-crossing of a hard Loiter stick reversal). Below this magnitude we fade to
37 : // an isotropic magnitude limit that preserves the commanded direction.
38 : #define LIMIT_ACCEL_XY_MIN_REF 0.25f
39 :
40 : // Projects velocity forward in time using acceleration, constrained by directional limit.
41 : // - If `limit` is non-zero, it defines a direction in which acceleration is constrained.
42 : // - The `vel_error` value defines the direction of velocity error (its sign matters, not its magnitude).
43 : // - When `limit` is active, velocity is only updated if doing so would not increase the error in the limited direction.
44 : // - If velocity is currently opposing the limit direction, the update is clipped to avoid crossing zero.
45 : // This prevents unwanted acceleration in the direction of a constraint when it would worsen velocity error.
46 115304041 : void update_vel_accel(float& vel, float accel, float dt, float limit, float vel_error)
47 : {
48 115304041 : float delta_vel = accel * dt;
49 : // do not add delta_vel if it will increase the velocity error in the direction of limit
50 : // unless adding delta_vel will reduce vel towards zero
51 115304041 : if (is_positive(delta_vel * limit) && is_positive(vel_error * limit)) {
52 216017 : if (is_negative(vel * limit)) {
53 68238 : delta_vel = constrain_float(delta_vel, -fabsf(vel), fabsf(vel));
54 : } else {
55 147779 : delta_vel = 0.0;
56 : }
57 : }
58 115304041 : vel += delta_vel;
59 115304041 : }
60 :
61 : // Projects position and velocity forward in time using acceleration, constrained by directional limit.
62 : // - `limit` defines the constrained direction of motion.
63 : // - `pos_error` and `vel_error` define the sign of error in position and velocity respectively (magnitude is ignored).
64 : // - If the update would increase position error in the constrained direction, the position update is skipped.
65 : // - The velocity update then proceeds with directional limit handling via `update_vel_accel()`.
66 : // This prevents motion in a constrained direction if it would worsen the position or velocity error.
67 97628535 : void update_pos_vel_accel(postype_t& pos, float& vel, float accel, float dt, float limit, float pos_error, float vel_error)
68 : {
69 : // move position and velocity forward by dt if it does not increase error when limited.
70 97628535 : float delta_pos = vel * dt + accel * 0.5f * sq(dt);
71 : // do not add delta_pos if it will increase the velocity error in the direction of limit
72 97628535 : if (is_positive(delta_pos * limit) && is_positive(pos_error * limit)) {
73 164988 : delta_pos = 0.0;
74 : }
75 97628535 : pos += delta_pos;
76 :
77 97628535 : update_vel_accel(vel, accel, dt, limit, vel_error);
78 97628535 : }
79 :
80 : // Projects velocity forward in time using acceleration, constrained by directional limits.
81 : // - If the `limit` vector is non-zero, it defines a direction in which acceleration is constrained.
82 : // - The `vel_error` vector defines the direction of velocity error (its magnitude is unused).
83 : // - When `limit` is active, velocity is only updated if doing so would not increase the error in the limited direction.
84 : // This function prevents the system from increasing velocity along the limit direction
85 : // if doing so would worsen the velocity error.
86 57737513 : void update_vel_accel_xy(Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit, const Vector2f& vel_error)
87 : {
88 : // increase velocity by acceleration * dt if it does not increase error when limited.
89 : // unless adding delta_vel will reduce the magnitude of vel
90 57737513 : Vector2f delta_vel = accel * dt;
91 57737513 : if (!limit.is_zero() && !delta_vel.is_zero()) {
92 : // check if delta_vel will increase the velocity error in the direction of limit
93 53071 : if (is_positive(delta_vel * limit) && is_positive(vel_error * limit) && !is_negative(vel * limit)) {
94 11357 : delta_vel.zero();
95 : }
96 : }
97 57737513 : vel += delta_vel;
98 57737513 : }
99 :
100 : // Projects position and velocity forward in time using acceleration, constrained by directional limits.
101 : // - The `limit` vector defines a directional constraint: if non-zero, motion in that direction is restricted.
102 : // - The `pos_error` and `vel_error` vectors represent the direction of error (magnitude is not used).
103 : // - If a motion step would increase error along a limited axis, it is suppressed.
104 : // This function avoids changes to position or velocity in the direction of `limit`
105 : // if those changes would worsen the position or velocity error respectively.
106 49751952 : void update_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, float dt, const Vector2f& limit, const Vector2f& pos_error, const Vector2f& vel_error)
107 : {
108 : // move position and velocity forward by dt.
109 49751952 : Vector2f delta_pos = vel * dt + accel * 0.5f * sq(dt);
110 :
111 49751952 : if (!is_zero(limit.length_squared())) {
112 : // zero delta_pos if it will increase the velocity error in the direction of limit
113 717339 : if (is_positive(delta_pos * limit) && is_positive(pos_error * limit)) {
114 9281 : delta_pos.zero();
115 : }
116 : }
117 :
118 49751952 : pos += delta_pos.topostype();
119 :
120 49751952 : update_vel_accel_xy(vel, accel, dt, limit, vel_error);
121 49751952 : }
122 :
123 : // Applies jerk-limited shaping to the acceleration value to gradually approach a new target.
124 : // - Constrains the rate of change of acceleration to be within ±`jerk_max` over time `dt`.
125 : // - The current acceleration value is modified in-place.
126 : // Useful for ensuring smooth transitions in thrust or lean angle command profiles.
127 193204098 : void shape_accel(float accel_desired, float& accel,
128 : float jerk_max, float dt)
129 : {
130 : // sanity check jerk_max
131 193204098 : if (!is_positive(jerk_max)) {
132 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
133 0 : return;
134 : }
135 :
136 : // jerk limit acceleration change
137 193204098 : if (is_positive(dt)) {
138 193194575 : float accel_delta = accel_desired - accel;
139 193194575 : accel_delta = constrain_float(accel_delta, -jerk_max * dt, jerk_max * dt);
140 193194575 : accel += accel_delta;
141 : }
142 : }
143 :
144 : // Applies jerk-limited shaping to a 2D acceleration vector.
145 : // - Constrains the rate of change of acceleration to a maximum of `jerk_max` over time `dt`.
146 : // - The current acceleration vector is modified in-place to approach `accel_desired`.
147 : /// Ensures smooth acceleration transitions in both axes simultaneously.
148 29291808 : void shape_accel_xy(const Vector2f& accel_desired, Vector2f& accel,
149 : float jerk_max, float dt)
150 : {
151 : // sanity check jerk_max
152 29291808 : if (!is_positive(jerk_max)) {
153 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
154 0 : return;
155 : }
156 :
157 : // jerk limit acceleration change
158 29291808 : if (is_positive(dt)) {
159 29269102 : Vector2f accel_delta = accel_desired - accel;
160 29269102 : accel_delta.limit_length(jerk_max * dt);
161 29269102 : accel = accel + accel_delta;
162 : }
163 : }
164 :
165 0 : void shape_accel_xy(const Vector3f& accel_desired, Vector3f& accel,
166 : float jerk_max, float dt)
167 : {
168 0 : const Vector2f accel_desired_2f {accel_desired.x, accel_desired.y};
169 0 : Vector2f accel_2f {accel.x, accel.y};
170 :
171 0 : shape_accel_xy(accel_desired_2f, accel_2f, jerk_max, dt);
172 0 : accel.x = accel_2f.x;
173 0 : accel.y = accel_2f.y;
174 0 : }
175 :
176 : // Shapes velocity and acceleration using jerk-limited control.
177 : // - Computes correction acceleration needed to reach `vel_desired` from current `vel`.
178 : // - Uses a square-root controller with max acceleration and jerk constraints.
179 : // - Correction is combined with feedforward `accel_desired`.
180 : // - If `limit_total_accel` is true, total acceleration is constrained to `accel_min` / `accel_max`.
181 : // The result is applied via `shape_accel`.
182 17675506 : void shape_vel_accel(float vel_desired, float accel_desired,
183 : float vel, float& accel,
184 : float accel_min, float accel_max,
185 : float jerk_max, float dt, bool limit_total_accel)
186 : {
187 : // sanity check accel_min, accel_max and jerk_max.
188 17675506 : if (!is_negative(accel_min) || !is_positive(accel_max) || !is_positive(jerk_max)) {
189 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
190 0 : return;
191 : }
192 :
193 : // velocity error to be corrected
194 17675506 : float vel_error = vel_desired - vel;
195 :
196 : // Calculate time constants and limits to ensure stable operation
197 : // The direction of acceleration limit is the same as the velocity error.
198 : // This is because the velocity error is negative when slowing down while
199 : // closing a positive position error.
200 : float KPa;
201 17675506 : if (is_positive(vel_error)) {
202 3791100 : KPa = jerk_max / accel_max;
203 : } else {
204 13884406 : KPa = jerk_max / (-accel_min);
205 : }
206 :
207 : // acceleration to correct velocity
208 17675506 : float accel_target = sqrt_controller(vel_error, KPa, jerk_max, dt);
209 :
210 : // constrain correction acceleration from accel_min to accel_max
211 17675506 : accel_target = constrain_float(accel_target, accel_min, accel_max);
212 :
213 : // velocity correction with input velocity
214 17675506 : accel_target += accel_desired;
215 :
216 : // Constrain total acceleration if limiting is enabled
217 17675506 : if (limit_total_accel) {
218 16289096 : accel_target = constrain_float(accel_target, accel_min, accel_max);
219 : }
220 :
221 17675506 : shape_accel(accel_target, accel, jerk_max, dt);
222 : }
223 :
224 : // Computes a jerk-limited acceleration command in 2D to track a desired velocity input.
225 : // - Uses a square-root controller to calculate correction acceleration based on velocity error.
226 : // - Correction is constrained to stay within `accel_max` (total acceleration magnitude).
227 : // - Correction is added to `accel_desired` (feedforward).
228 : // - If `limit_total_accel` is true, total acceleration is constrained after summing.
229 : // Ensures velocity tracking with smooth, physically constrained motion.
230 7985561 : void shape_vel_accel_xy(const Vector2f& vel_desired, const Vector2f& accel_desired,
231 : const Vector2f& vel, Vector2f& accel,
232 : float accel_max, float jerk_max, float dt, bool limit_total_accel)
233 : {
234 : // sanity check accel_max and jerk_max.
235 7985561 : if (!is_positive(accel_max) || !is_positive(jerk_max)) {
236 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
237 0 : return;
238 : }
239 :
240 : // Calculate time constants and limits to ensure stable operation
241 7985561 : const float KPa = jerk_max / accel_max;
242 :
243 : // velocity error to be corrected
244 7985561 : const Vector2f vel_error = vel_desired - vel;
245 :
246 : // acceleration to correct velocity
247 7985561 : Vector2f accel_target = sqrt_controller(vel_error, KPa, jerk_max, dt);
248 :
249 7985561 : limit_accel_corner_xy(vel, accel_target, accel_max);
250 :
251 7985561 : accel_target += accel_desired;
252 :
253 : // Constrain total acceleration if limiting is enabled
254 7985561 : if (limit_total_accel) {
255 7722796 : accel_target.limit_length(accel_max);
256 : }
257 :
258 7985561 : shape_accel_xy(accel_target, accel, jerk_max, dt);
259 : }
260 :
261 : // Shapes position, velocity, and acceleration using a jerk-limited square-root command model.
262 : // - Computes a velocity correction from position error using a square-root controller.
263 : // - Uses sqrt_controller_accel() to bias the velocity correction based on the correction-frame closing rate.
264 : // - Forms a velocity target by adding the correction to the feedforward velocity.
265 : // - Computes an acceleration demand from velocity error using k_v and adds external acceleration feedforward.
266 : // - Optionally constrains velocity and acceleration magnitudes when limit_total is true.
267 : // - Applies jerk limiting via shape_accel() to ensure smooth acceleration transitions.
268 : // This is the single-axis (1D) form of shape_pos_vel_accel_xy().
269 175526117 : void shape_pos_vel_accel(postype_t pos_desired, float vel_desired, float accel_desired,
270 : postype_t pos, float vel, float& accel,
271 : float vel_min, float vel_max,
272 : float accel_min, float accel_max,
273 : float jerk_max, float dt, bool limit_total)
274 : {
275 : // Sanity check limits and jerk_max.
276 175526117 : if (is_positive(vel_min) || is_negative(vel_max) || !is_negative(accel_min) || !is_positive(accel_max) || !is_positive(jerk_max)) {
277 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
278 0 : return;
279 : }
280 :
281 : // Position error to be corrected.
282 175526117 : const float pos_error = pos_desired - pos;
283 :
284 : // Select sqrt_controller parameters based on error sign so the second-order limit
285 : // (acceleration allowance) matches the direction of motion.
286 : float accel_lim;
287 : float k_v;
288 175526117 : if (is_positive(pos_error)) {
289 26502667 : accel_lim = -accel_min; // acceleration limit magnitude (positive)
290 26502667 : k_v = jerk_max / accel_lim; // inner velocity-loop gain derived from jerk/accel limits (1/s)
291 : } else {
292 149023450 : accel_lim = accel_max; // acceleration limit magnitude (positive)
293 149023450 : k_v = jerk_max / accel_lim; // inner velocity-loop gain derived from jerk/accel limits (1/s)
294 : }
295 :
296 : // Work in the correction frame by removing the feedforward velocity.
297 : // vel_corr is the correction-frame velocity, so pos_error_dot = vel_desired - vel = -vel_corr.
298 175526117 : const float vel_corr = vel - vel_desired;
299 :
300 : // Velocity correction command derived from position error (second-order limited).
301 175526117 : float vel_corr_cmd = sqrt_controller(pos_error, k_v, accel_lim, dt);
302 :
303 : // Rate-of-change implied by the shaped velocity correction, using correction-frame closing rate.
304 175526117 : const float accel_corr_cmd = sqrt_controller_accel(pos_error, vel_corr_cmd, vel_corr, k_v, accel_lim);
305 :
306 : // Convert the implied rate-of-change term into an equivalent velocity correction bias.
307 175526117 : vel_corr_cmd += accel_corr_cmd / k_v;
308 :
309 : // Limit correction velocity magnitude if velocity limiting is enabled (non-zero limits).
310 175526117 : if (is_negative(vel_min)) {
311 79689793 : vel_corr_cmd = MAX(vel_corr_cmd, vel_min);
312 : }
313 175526117 : if (is_positive(vel_max)) {
314 79689793 : vel_corr_cmd = MIN(vel_corr_cmd, vel_max);
315 : }
316 :
317 : // Total velocity target = feedforward + correction.
318 175526117 : float vel_target = vel_desired + vel_corr_cmd;
319 :
320 : // Constrain total velocity if limiting is enabled and velocity limits are enabled (non-zero).
321 175526117 : if (limit_total) {
322 123492736 : if (is_negative(vel_min)) {
323 27965562 : vel_target = MAX(vel_target, vel_min);
324 : }
325 123492736 : if (is_positive(vel_max)) {
326 27965562 : vel_target = MIN(vel_target, vel_max);
327 : }
328 : }
329 :
330 : // Acceleration demand from velocity error.
331 175526117 : float accel_target = (vel_target - vel) * k_v;
332 :
333 : // Bound acceleration command (before external feedforward is added).
334 175526117 : accel_target = constrain_float(accel_target, accel_min, accel_max);
335 :
336 : // Add external acceleration feedforward.
337 175526117 : accel_target += accel_desired;
338 :
339 : // Constrain total acceleration if limiting is enabled.
340 175526117 : if (limit_total) {
341 123492736 : accel_target = constrain_float(accel_target, accel_min, accel_max);
342 : }
343 :
344 : // Jerk-limit acceleration toward accel_target.
345 175526117 : shape_accel(accel_target, accel, jerk_max, dt);
346 : }
347 :
348 : // Shapes lateral position, velocity, and acceleration using a jerk-limited square-root command model.
349 : // - Computes a velocity correction from position error using a square-root controller.
350 : // - Uses sqrt_controller_accel() to bias the velocity correction based on correction-frame closing rate.
351 : // - Forms a velocity target by adding the correction to the feedforward velocity.
352 : // - Computes an acceleration demand from velocity error using k_v and adds external acceleration feedforward.
353 : // - Limits acceleration magnitude with a braking-priority limiter based on the current velocity direction.
354 : // - Optionally constrains velocity and acceleration magnitudes when limit_total is true.
355 : // - Applies jerk limiting via shape_accel_xy() to ensure smooth acceleration transitions.
356 21303772 : void shape_pos_vel_accel_xy(const Vector2p& pos_desired, const Vector2f& vel_desired, const Vector2f& accel_desired,
357 : const Vector2p& pos, const Vector2f& vel, Vector2f& accel,
358 : float vel_max, float accel_max,
359 : float jerk_max, float dt, bool limit_total)
360 : {
361 : // sanity check vel_max, accel_max and jerk_max.
362 21303772 : if (is_negative(vel_max) || !is_positive(accel_max) || !is_positive(jerk_max)) {
363 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
364 0 : return;
365 : }
366 :
367 : // inner velocity-loop gain derived from jerk/accel limits (1/s)
368 21303772 : const float k_v = jerk_max / accel_max;
369 :
370 : // Velocity correction vector (added to vel_desired to form vel_target).
371 21303772 : Vector2f vel_corr_cmd;
372 :
373 : // Position error to be corrected (direction preserved; magnitude used for shaping).
374 21303772 : const Vector2f pos_error = (pos_desired - pos).tofloat();
375 21303772 : const float pos_error_length = pos_error.length();
376 21303772 : if (is_positive(pos_error_length)) {
377 : // Correction-frame velocity projected onto the position error direction.
378 : // For a moving setpoint, pos_error_dot = vel_desired - vel, so the closing rate is -(vel - vel_desired).
379 1832172 : float vel_corr_proj = (vel - vel_desired).dot(pos_error) / pos_error_length;
380 :
381 : // Velocity correction magnitude from square-root position controller.
382 1832172 : float vel_corr_cmd_length = sqrt_controller(pos_error_length, k_v, accel_max, dt);
383 :
384 : // Rate-of-change implied by the shaped velocity correction, using correction-frame closing rate.
385 1832172 : float accel_corr_cmd_length = sqrt_controller_accel(pos_error_length, vel_corr_cmd_length, vel_corr_proj, k_v, accel_max);
386 :
387 : // Convert the implied rate-of-change term into an equivalent velocity correction bias.
388 1832172 : vel_corr_cmd_length += accel_corr_cmd_length / k_v;
389 :
390 : // Limit correction velocity magnitude if velocity limiting is enabled (non-zero limits).
391 1832172 : if (is_positive(vel_max)) {
392 1739392 : vel_corr_cmd_length = constrain_float(vel_corr_cmd_length, -vel_max, vel_max);
393 : }
394 :
395 : // Map scalar correction back onto the position error direction.
396 1832172 : vel_corr_cmd = pos_error * (vel_corr_cmd_length / pos_error_length);
397 : }
398 :
399 : // Total velocity target is the sum of feedforward velocity and correction.
400 21303772 : Vector2f vel_target = vel_desired + vel_corr_cmd;
401 21303772 : if (limit_total && is_positive(vel_max)) {
402 : // Constrain total velocity magnitude if limiting is enabled.
403 1151241 : vel_target.limit_length(vel_max);
404 : }
405 :
406 : // Acceleration demand from velocity error.
407 21303772 : Vector2f accel_target = (vel_target - vel) * k_v;
408 :
409 : // Limit acceleration magnitude while prioritising braking along the current velocity direction.
410 21303772 : limit_accel_corner_xy(vel, accel_target, accel_max);
411 :
412 : // Add external acceleration feedforward.
413 21303772 : accel_target += accel_desired;
414 :
415 21303772 : if (limit_total) {
416 : // Constrain total acceleration magnitude if limiting is enabled.
417 1151241 : accel_target.limit_length(accel_max);
418 : }
419 :
420 : // Apply jerk limiting to smoothly approach the target acceleration.
421 21303772 : shape_accel_xy(accel_target, accel, jerk_max, dt);
422 : }
423 :
424 : // Shapes angular position, velocity, and acceleration using a jerk-limited square-root command model.
425 : // - Computes an angular velocity correction from angular position error using a square-root controller.
426 : // - Uses sqrt_controller_accel() to bias the angular velocity correction based on correction-frame closing rate.
427 : // - Forms an angular velocity target by adding the correction to the feedforward angular velocity.
428 : // - Computes an angular acceleration demand from angular velocity error using k_v and adds external angular
429 : // acceleration feedforward.
430 : // - Optionally constrains angular velocity and angular acceleration magnitudes when limit_total is true.
431 : // - Applies jerk limiting via shape_accel() to ensure smooth angular acceleration transitions.
432 : // This is the angular (wrapped) form of shape_pos_vel_accel().
433 106315986 : void shape_angle_vel_accel(float angle_desired, float angle_vel_desired, float angle_accel_desired,
434 : float angle, float angle_vel, float& angle_accel,
435 : float angle_vel_min, float angle_vel_max, float angle_accel_max,
436 : float angle_jerk_max, float dt, bool limit_total)
437 : {
438 : // Wrap desired angle to the nearest equivalent setpoint relative to the current angle.
439 106315986 : const float angle_desired_wrapped = angle + wrap_PI(angle_desired - angle);
440 106315986 : shape_pos_vel_accel( angle_desired_wrapped, angle_vel_desired, angle_accel_desired, angle, angle_vel, angle_accel, angle_vel_min, angle_vel_max, -angle_accel_max, angle_accel_max, angle_jerk_max, dt, limit_total);
441 106315986 : }
442 :
443 : // Limits a 2D acceleration vector to prioritize lateral (cross-track) acceleration over longitudinal (in-track) acceleration.
444 : // - `vel_norm` is the normalised velocity (velocity divided by maximum speed) that sets the reference direction used to split the acceleration: the in-track component is parallel to it and the cross-track component is perpendicular. Normalising keys the cross-track prioritisation fade to a fraction of max speed.
445 : // - `accel` is modified in-place to remain within `accel_max`.
446 : // - If the full acceleration vector exceeds `accel_max`, it is reshaped to prioritize the cross-track component.
447 : // - If `vel_norm` is zero, a simple magnitude limit is applied.
448 : // Returns true if the acceleration vector was modified.
449 19264871 : bool limit_accel_xy(const Vector2f& vel_norm, Vector2f& accel, float accel_max)
450 : {
451 : // check accel_max is defined
452 19264871 : if (!is_positive(accel_max)) {
453 0 : return false;
454 : }
455 : // nothing to do unless the acceleration vector exceeds the limit
456 19264871 : if (accel.length_squared() <= sq(accel_max)) {
457 18927028 : return false;
458 : }
459 :
460 : // isotropic (direction-preserving) magnitude limit. Used directly when there
461 : // is no meaningful reference direction, and blended in at low reference
462 : // magnitude below.
463 337843 : Vector2f accel_isotropic = accel;
464 337843 : accel_isotropic.limit_length(accel_max);
465 :
466 337843 : const float ref_mag = vel_norm.length();
467 337843 : if (!is_positive(ref_mag)) {
468 : // We do not have a reference direction so do a simple vector length limit
469 5534 : accel = accel_isotropic;
470 5534 : return true;
471 : }
472 :
473 : // limit acceleration to accel_max while prioritizing cross track acceleration
474 : // calculate acceleration along and perpendicular to the reference direction
475 332309 : const Vector2f ref_unit = vel_norm / ref_mag;
476 : // acceleration along the reference direction (in-track)
477 332309 : float accel_dir = ref_unit * accel;
478 : // cross track acceleration
479 332309 : Vector2f accel_cross = accel - (ref_unit * accel_dir);
480 332309 : if (accel_cross.limit_length(accel_max)) {
481 28744 : accel_dir = 0.0;
482 : } else {
483 : // limit_length can't absolutely guarantee this subtraction
484 : // won't be slightly negative, so safe_sqrt is used
485 303565 : float accel_max_dir = safe_sqrt(sq(accel_max) - accel_cross.length_squared());
486 303565 : accel_dir = constrain_float(accel_dir, -accel_max_dir, accel_max_dir);
487 : }
488 332309 : const Vector2f accel_prioritised = accel_cross + ref_unit * accel_dir;
489 :
490 : // Fade between the isotropic limit (weak reference) and the cross-track
491 : // prioritised limit (reference magnitude at or above LIMIT_ACCEL_XY_MIN_REF).
492 : // When the reference is small its direction is ill-defined and the prioritised
493 : // split would re-project the saturated braking command into a lateral
494 : // acceleration spike (e.g. a hard stick reversal in Loiter). Fading to the
495 : // direction-preserving limit removes that spike. Both blend inputs have
496 : // magnitude <= accel_max, so the result does too.
497 332309 : const float prioritise_ratio = constrain_float(ref_mag / LIMIT_ACCEL_XY_MIN_REF, 0.0f, 1.0f);
498 332309 : accel = accel_isotropic * (1.0f - prioritise_ratio) + accel_prioritised * prioritise_ratio;
499 :
500 332309 : return true;
501 : }
502 :
503 : // Limits a 2D acceleration vector with direction-dependent prioritisation.
504 : // - Acceleration is decomposed into along-track (parallel to velocity) and cross-track components.
505 : // - If braking is requested (negative along-track component), braking is prioritised and the
506 : // remaining acceleration budget is allocated to cross-track.
507 : // - If no braking is requested (along-track acceleration or zero), cross-track acceleration
508 : // is prioritised and the remaining budget is allocated to along-track.
509 : // - Ensures the final acceleration magnitude does not exceed accel_max.
510 : // - If velocity is zero (no defined direction), a simple magnitude limit is applied.
511 : // Returns true if the limiting logic was applied.
512 29289333 : bool limit_accel_corner_xy(const Vector2f& vel, Vector2f& accel, float accel_max)
513 : {
514 : // Check accel_max is defined.
515 29289333 : if (!is_positive(accel_max)) {
516 0 : return false;
517 : }
518 :
519 29289333 : if (vel.is_zero()) {
520 : // No along/cross decomposition possible; apply a simple magnitude limit.
521 26266862 : return accel.limit_length(accel_max);
522 : }
523 :
524 : // Pre-limit to keep the acceleration direction well-conditioned.
525 : // This allows cross-track components to appear earlier when the upstream
526 : // acceleration demand (often proportional to velocity error) is very large.
527 3022471 : accel.limit_length(2.0 * accel_max);
528 :
529 : // Unit velocity direction defines the along-track axis.
530 3022471 : const Vector2f vel_unit = vel.normalized();
531 :
532 : // Signed scalar projection of acceleration onto the velocity direction.
533 : // Negative values correspond to braking.
534 3022471 : float accel_dir_scalar = accel.dot(vel_unit);
535 :
536 : // Along-track and cross-track acceleration components.
537 3022471 : Vector2f accel_dir = vel_unit * accel_dir_scalar;
538 3022471 : Vector2f accel_cross = accel - accel_dir;
539 :
540 3022471 : if (is_positive(accel_dir_scalar)) {
541 : // Non-braking regime
542 : // Prioritise cross-track acceleration and allocate the remaining budget to along-track.
543 :
544 : // Limit cross-track magnitude first.
545 863673 : const float accel_cross_mag = MIN(accel_cross.length(), accel_max);
546 863673 : const float accel_along_max = safe_sqrt(sq(accel_max) - sq(accel_cross_mag));
547 :
548 863673 : accel_cross.limit_length(accel_max);
549 863673 : accel_dir.limit_length(accel_along_max);
550 :
551 863673 : accel = accel_cross + accel_dir;
552 863673 : return true;
553 : }
554 :
555 : // Braking regime
556 : // Prioritise along-track deceleration and allocate the remaining budget to cross-track.
557 :
558 : // Limit braking magnitude.
559 2158798 : accel_dir_scalar = MAX(accel_dir_scalar, -accel_max);
560 2158798 : accel_dir = vel_unit * accel_dir_scalar;
561 :
562 : // Allocate remaining acceleration budget to cross-track.
563 2158798 : const float accel_cross_max = safe_sqrt(sq(accel_max) - sq(accel_dir_scalar));
564 2158798 : accel_cross.limit_length(accel_cross_max);
565 :
566 2158798 : accel = accel_cross + accel_dir;
567 2158798 : return true;
568 : }
569 :
570 : // Piecewise square-root + linear controller that limits second-order response (acceleration).
571 : // - Behaves like a P controller near the setpoint.
572 : // - Switches to sqrt(2·a·Δx) shaping beyond a threshold to limit acceleration.
573 : // - `second_ord_lim` sets the max acceleration allowed.
574 : // - Returns the constrained correction rate for a given error and gain.
575 354628292 : float sqrt_controller(float error, float p, float second_ord_lim, float dt)
576 : {
577 : float correction_rate;
578 354628292 : if (is_negative(second_ord_lim) || is_zero(second_ord_lim)) {
579 : // No second-order limit: use pure linear controller
580 2 : correction_rate = error * p;
581 354628290 : } else if (is_zero(p)) {
582 : // No P gain, but with acceleration limit — use sqrt-shaped response only
583 64935 : if (is_positive(error)) {
584 17527 : correction_rate = safe_sqrt(2.0 * second_ord_lim * (error));
585 47408 : } else if (is_negative(error)) {
586 33641 : correction_rate = -safe_sqrt(2.0 * second_ord_lim * (-error));
587 : } else {
588 13767 : correction_rate = 0.0;
589 : }
590 : } else {
591 : // Both P and second-order limits defined — use hybrid model
592 354563355 : const float linear_dist = second_ord_lim / sq(p);
593 354563355 : if (error > linear_dist) {
594 : // Positive error beyond linear region — use sqrt branch
595 7075597 : correction_rate = safe_sqrt(2.0 * second_ord_lim * (error - (linear_dist / 2.0)));
596 347487758 : } else if (error < -linear_dist) {
597 : // Negative error beyond linear region — use sqrt branch
598 4467853 : correction_rate = -safe_sqrt(2.0 * second_ord_lim * (-error - (linear_dist / 2.0)));
599 : } else {
600 : // Inside linear region
601 343019905 : correction_rate = error * p;
602 : }
603 : }
604 354628292 : if (is_positive(dt)) {
605 : // Clamp to ensure we do not overshoot the error in the last time step
606 312380419 : return constrain_float(correction_rate, -fabsf(error) / dt, fabsf(error) / dt);
607 : } else {
608 42247873 : return correction_rate;
609 : }
610 : }
611 :
612 : // Vector form of `sqrt_controller()`, applied along the direction of the input error vector.
613 : // - Returns a correction vector with magnitude shaped using `sqrt_controller()`.
614 : // - Direction is preserved from the input error.
615 : // - Used in 2D position or velocity control with second-order constraints.
616 27428090 : Vector2f sqrt_controller(const Vector2f& error, float p, float second_ord_lim, float dt)
617 : {
618 27428090 : const float error_length = error.length();
619 27428090 : if (!is_positive(error_length)) {
620 9607780 : return Vector2f{};
621 : }
622 :
623 17820310 : const float correction_length = sqrt_controller(error_length, p, second_ord_lim, dt);
624 17820310 : return error * (correction_length / error_length);
625 : }
626 :
627 : // Inverts the output of `sqrt_controller()` to recover the input error that would produce a given output.
628 : // - Useful for calculating required error to produce a desired rate.
629 : // - Handles both linear and square-root regions of the controller response.
630 42214335 : float inv_sqrt_controller(float output, float p, float D_max)
631 : {
632 : // Degenerate case: second-order limit (D_max) is positive, but P gain is zero
633 42214335 : if (is_positive(D_max) && is_zero(p)) {
634 0 : return (output * output) / (2.0 * D_max);
635 : }
636 :
637 : // Degenerate case: no D_max, but P gain is non-zero → use linear model
638 42214335 : if ((is_negative(D_max) || is_zero(D_max)) && !is_zero(p)) {
639 0 : return output / p;
640 : }
641 :
642 : // Degenerate case: both gains are zero — no useful model
643 42214335 : if ((is_negative(D_max) || is_zero(D_max)) && is_zero(p)) {
644 2 : return 0.0;
645 : }
646 :
647 : // Compute transition threshold between linear and sqrt regions
648 42214333 : const float linear_velocity = D_max / p;
649 :
650 42214333 : if (fabsf(output) < linear_velocity) {
651 : // Linear region: below transition threshold
652 3739660 : return output / p;
653 : }
654 :
655 : // Square-root region: above transition threshold
656 38474673 : const float linear_dist = D_max / sq(p);
657 38474673 : const float stopping_dist = (linear_dist * 0.5f) + sq(output) / (2.0 * D_max);
658 38474673 : return is_positive(output) ? stopping_dist : -stopping_dist;
659 : }
660 :
661 : // Computes the rate-of-change implied by sqrt_controller() for the commanded correction rate.
662 : // - Uses the chain rule to estimate rate_cmd_dot = d(rate_cmd)/dt based on the actual closing rate.
663 : // - For a fixed target: error = target - state, and error_dot = -rate_state (since state_dot = rate_state).
664 : // - In the linear region of sqrt_controller(): d(rate_cmd)/d(error) = p
665 : // - In the sqrt region of sqrt_controller(): d(rate_cmd)/d(error) = second_ord_lim / |rate_cmd|
666 : // - Therefore:
667 : // linear region: rate_cmd_dot = -p * rate_state
668 : // sqrt region: rate_cmd_dot = -(second_ord_lim / |rate_cmd|) * rate_state
669 : // Notes:
670 : // - If second_ord_lim <= 0, the controller is linear everywhere.
671 : // - If p == 0, the controller is pure sqrt everywhere.
672 : // - rate_cmd must be the output of sqrt_controller() for the same error.
673 177358289 : float sqrt_controller_accel(float error, float rate_cmd, float rate_state, float p, float second_ord_lim)
674 : {
675 : // If we are moving away from the target return zero.
676 177358289 : if (!is_positive(rate_cmd * rate_state)) {
677 142518672 : return 0.0;
678 : }
679 :
680 : // If no second-order limit, controller is linear everywhere (rate_cmd ~ p*error).
681 34839617 : if (!is_positive(second_ord_lim)) {
682 0 : return -p * rate_state;
683 : }
684 :
685 : // If no P gain but second-order limit exists, controller is pure sqrt everywhere.
686 34839617 : if (!is_positive(p)) {
687 0 : if (is_zero(rate_cmd)) {
688 0 : return 0.0;
689 : }
690 0 : return -(second_ord_lim / fabsf(rate_cmd)) * rate_state;
691 : }
692 :
693 : // Both P and second-order limit defined — match sqrt_controller() region selection.
694 34839617 : const float linear_dist = second_ord_lim / sq(p);
695 :
696 34839617 : if (fabsf(error) <= linear_dist) {
697 : // Inside linear region.
698 31465015 : return -p * rate_state;
699 : }
700 :
701 : // Outside linear region (sqrt branch). Guard divide-by-zero on rate_cmd.
702 3374602 : if (is_zero(rate_cmd)) {
703 0 : return 0.0f;
704 : }
705 3374602 : return -(second_ord_lim / fabsf(rate_cmd)) * rate_state;
706 : }
707 :
708 : // Calculates stopping distance required to reduce a velocity to zero using a square-root controller.
709 : // - Uses the inverse of the `sqrt_controller()` response curve.
710 : // - Inputs: velocity, P gain, and max deceleration (`accel_max`)
711 : // - Output: stopping distance required to decelerate cleanly.
712 71501 : float stopping_distance(float velocity, float p, float accel_max)
713 : {
714 : // Use inverse of sqrt_controller to compute stopping distance from current velocity
715 71501 : return inv_sqrt_controller(velocity, p, accel_max);
716 : }
717 :
718 : // Return the largest M >= 0 that can scale a 3D direction without exceeding
719 : // independent axis limits:
720 : //
721 : // M * |unit.xy| <= max_xy
722 : // -max_z_neg <= M * unit.z <= max_z_pos
723 : //
724 : // where unit = normalize(direction). The magnitude of direction is ignored.
725 : //
726 : // max_z_pos limits travel in the +Z direction.
727 : // max_z_neg limits travel in the -Z direction.
728 : // All limits must be positive.
729 : //
730 : // Typical use: limit velocity or acceleration magnitude along a desired
731 : // direction without changing that direction.
732 : //
733 : // Returns 0 if the direction is zero or any limit is zero.
734 929073 : float kinematic_limit(Vector3f direction, float max_xy, float max_z_neg, float max_z_pos)
735 : {
736 : // Decompose into horizontal magnitude and vertical component
737 929073 : const float dir_xy = direction.xy().length();
738 :
739 929073 : return kinematic_limit(dir_xy, direction.z, max_xy, max_z_neg, max_z_pos);
740 : }
741 :
742 : // Return the largest M >= 0 along a direction defined by horizontal and
743 : // vertical components, constrained by:
744 : //
745 : // M * |unit.xy| <= max_xy
746 : // -max_z_neg <= M * unit.z <= max_z_pos
747 : //
748 : // dir_xy (>= 0) and dir_z define a direction; only their ratio matters
749 : // (normalized internally).
750 : //
751 : // max_z_pos limits travel in the +Z direction.
752 : // max_z_neg limits travel in the -Z direction.
753 : // All limits must be positive.
754 : //
755 : // Returns 0 if any limit is zero or the direction is zero.
756 932032 : float kinematic_limit(float dir_xy, float dir_z, float max_xy, float max_z_neg, float max_z_pos)
757 : {
758 : // Reject invalid limits
759 932032 : if (is_negative(dir_xy) || !is_positive(max_xy) || !is_positive(max_z_pos) || !is_positive(max_z_neg)) {
760 36 : return 0.0;
761 : }
762 :
763 : // Check for zero length direction vector
764 931996 : const float dir_length = safe_sqrt(sq(dir_xy) + sq(dir_z));
765 931996 : if (!is_positive(dir_length)) {
766 12293 : return 0.0;
767 : }
768 :
769 919703 : if (is_zero(dir_xy)) {
770 : // Pure vertical - constrained only by vertical limits
771 44878 : return is_positive(dir_z) ? max_z_pos : max_z_neg;
772 : }
773 :
774 874825 : if (is_zero(dir_z)) {
775 : // Pure horizontal - constrained only by horizontal limits
776 161797 : return max_xy;
777 : }
778 :
779 : // Normalize the direction vector (only ratio matters)
780 713028 : dir_xy /= dir_length;
781 713028 : dir_z /= dir_length;
782 :
783 : // Compare the direction slope (|dir_z/dir_xy|) to the limit slope
784 : // (max_z/max_xy) to determine which axis constraint is hit first.
785 713028 : const float slope = dir_z / dir_xy;
786 :
787 713028 : if (is_positive(slope)) {
788 : // Positive-Z: constrained by max_z_pos
789 322432 : if (slope < max_z_pos / max_xy) {
790 : // Shallow direction: horizontal limit reached first
791 189601 : return max_xy / dir_xy;
792 : }
793 : // Steep direction: vertical limit reached first
794 132831 : return max_z_pos / dir_z;
795 : }
796 :
797 : // Negative-Z: constrained by max_z_neg
798 390596 : if (-slope < max_z_neg / max_xy) {
799 : // Shallow direction: horizontal limit reached first
800 212344 : return max_xy / dir_xy;
801 : }
802 : // Steep direction: vertical limit reached first
803 178252 : return -max_z_neg / dir_z;
804 : }
805 :
806 : // Applies an exponential curve to a normalized input in the range [-1, 1].
807 : // - `expo` shapes the curve (0 = linear, closer to 1 = more curvature).
808 : // - Typically used for pilot stick input response shaping.
809 : // - Clipped to `expo < 0.95` to avoid divide-by-zero or extreme scaling.
810 29896429 : float input_expo(float input, float expo)
811 : {
812 : // Clamp input to normalized stick range
813 29896429 : input = constrain_float(input, -1.0, 1.0);
814 29896429 : if (expo < 0.95) {
815 : // Expo shaping: increases control around center stick
816 29896429 : return (1 - expo) * input / (1 - expo * fabsf(input));
817 : }
818 : // If expo is too close to 1, return input unchanged
819 0 : return input;
820 : }
821 :
822 : // Converts a lean angle (radians) to horizontal acceleration in m/s².
823 26019206 : float angle_rad_to_accel_mss(float angle_rad)
824 : {
825 : // Convert lean angle to horizontal acceleration
826 26019206 : return GRAVITY_MSS * tanf(angle_rad);
827 : }
828 :
829 : // Converts a lean angle (degrees) to horizontal acceleration in m/s².
830 0 : float angle_deg_to_accel_mss(float angle_deg)
831 : {
832 : // Convert degrees to radians, then to acceleration
833 0 : return angle_rad_to_accel_mss(radians(angle_deg));
834 : }
835 :
836 : // Converts a horizontal acceleration (m/s²) to lean angle in radians.
837 : // Assumes: angle = atan(a / g)
838 39904402 : float accel_mss_to_angle_rad(float accel_mss)
839 : {
840 : // Inverse of angle_rad_to_accel_mss
841 39904402 : return atanf(accel_mss/GRAVITY_MSS);
842 : }
843 :
844 : // Converts a horizontal acceleration (m/s²) to lean angle in degrees.
845 1347466 : float accel_mss_to_angle_deg(float accel_mss)
846 : {
847 : // Convert result of radian-based conversion to degrees
848 1347466 : return degrees(accel_mss_to_angle_rad(accel_mss));
849 : }
850 :
851 : // Converts pilot’s normalized roll/pitch input into target roll and pitch angles (radians).
852 : // - `roll_in_norm` and `pitch_in_norm`: stick inputs in range [-1, 1]
853 : // - `angle_max_rad`: maximum allowed lean angle
854 : // - `angle_limit_rad`: secondary limit to constrain output while preserving full stick range
855 : // Outputs are Euler angles in radians: `roll_out_rad`, `pitch_out_rad`
856 15531163 : void rc_input_to_roll_pitch_rad(float roll_in_norm, float pitch_in_norm, float angle_max_rad, float angle_limit_rad, float &roll_out_rad, float &pitch_out_rad)
857 : {
858 : // Constrain angle_max to 85 deg to avoid unstable behavior
859 15531163 : angle_max_rad = MIN(angle_max_rad, radians(85.0));
860 :
861 : // Convert normalized pitch and roll stick input into horizontal thrust components
862 15531163 : Vector2f thrust;
863 15531163 : thrust.x = - tanf(angle_max_rad * pitch_in_norm);
864 15531163 : thrust.y = tanf(angle_max_rad * roll_in_norm);
865 :
866 : // Calculate the horizontal thrust limit based on angle limit
867 15531163 : angle_limit_rad = constrain_float(angle_limit_rad, radians(10.0), angle_max_rad);
868 15531163 : float thrust_limit = tanf(angle_limit_rad);
869 :
870 : // Apply limit to the horizontal thrust vector (preserves stick direction)
871 15531163 : thrust.limit_length(thrust_limit);
872 :
873 : // Convert thrust vector back to pitch and roll Euler angles
874 15531163 : pitch_out_rad = - atanf(thrust.x);
875 15531163 : roll_out_rad = atanf(cosf(pitch_out_rad) * thrust.y);
876 15531163 : }
|
