Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
stabilization_andi.c
Go to the documentation of this file.
1/*
2 *
3 * Copyright (C) 2025 Justin Dubois <j.p.g.dubois@student.tudelft.nl>
4 *
5 * This file is part of paparazzi
6 *
7 * paparazzi is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2, or (at your option)
10 * any later version.
11 *
12 * paparazzi is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with paparazzi; see the file COPYING. If not, see
19 * <http://www.gnu.org/licenses/>.
20 *
21 */
22
40
41
46#include "state.h"
47#include "generated/airframe.h"
50#include "modules/core/abi.h"
51#include "math/wls/wls_alloc.h"
56#include "autopilot.h"
65
66#include <stdio.h>
67#if INS_EXT_POSE
69#endif
70
71#ifdef STABILIZATION_ANDI_SCHEDULE_EFF
72 const bool SCHEDULE_EFF = STABILIZATION_ANDI_SCHEDULE_EFF;
73#else
74 const bool SCHEDULE_EFF = false;
75#endif
76
77#ifdef STABILIZATION_ANDI_USE_STATE_DYNAMICS
78 const bool USE_STATE_DYNAMICS = STABILIZATION_ANDI_USE_STATE_DYNAMICS;
79#else
80 const bool USE_STATE_DYNAMICS = false;
81#endif
82
83#ifdef STABILIZATION_ANDI_RELAX_OBM
84 const float ANDI_RELAX_OBM = STABILIZATION_ANDI_RELAX_OBM;
85#else
86 const float ANDI_RELAX_OBM = 1.0f;
87#endif
88
89#ifdef STABILIZATION_ANDI_ACT_IS_SERVO
90const bool ACTUATOR_IS_SERVO[ANDI_NUM_ACT] = STABILIZATION_ANDI_ACT_IS_SERVO;
91#else
93#endif
94
95#ifdef STABILIZATION_ANDI_ACT_DYNAMICS
96 const float ACTUATOR_DYNAMICS[ANDI_NUM_ACT] = STABILIZATION_ANDI_ACT_DYNAMICS; // rad/s
97#else
98 #error "You must specify the actuator dynamics"
99#endif
100
101#ifdef STABILIZATION_ANDI_ACT_DELAY
102const uint8_t ACTUATOR_DELAY[ANDI_NUM_ACT] = STABILIZATION_ANDI_ACT_DELAY;
103#else
105#endif
106
107#if defined(STABILIZATION_ANDI_ACT_MAX) && defined(STABILIZATION_ANDI_ACT_MIN)
108 const float ACTUATOR_MAX[ANDI_NUM_ACT] = STABILIZATION_ANDI_ACT_MAX;
109 const float ACTUATOR_MIN[ANDI_NUM_ACT] = STABILIZATION_ANDI_ACT_MIN;
110#else
111 #error "You must specify the actuator limits: STABILIZATION_ANDI_ACT_MAX and STABILIZATION_ANDI_ACT_MIN"
112#endif
113
114#if defined(STABILIZATION_ANDI_ACT_RATE_MAX) && defined(STABILIZATION_ANDI_ACT_RATE_MIN)
115 const float ACTUATOR_D_MAX[ANDI_NUM_ACT] = STABILIZATION_ANDI_ACT_RATE_MAX;
116 const float ACTUATOR_D_MIN[ANDI_NUM_ACT] = STABILIZATION_ANDI_ACT_RATE_MIN;
117#else
118 #error "You must specify the actuator limits: STABILIZATION_ANDI_ACT_RATE_MAX and STABILIZATION_ANDI_ACT_RATE_MIN"
119#endif
120
121#if defined STABILIZATION_ANDI_ACT_PREF
122const float ACTUATOR_PREF[ANDI_NUM_ACT] = STABILIZATION_ANDI_ACT_PREF;
123#else
124const float ACTUATOR_PREF[ANDI_NUM_ACT] = {0.0f};
125#endif
126
127#if defined STABILIZATION_ANDI_RC_RATE_MAX
128const float RC_RATE_MAX[ANDI_OUTPUTS] = STABILIZATION_ANDI_RC_RATE_MAX;
129#else
130const float RC_RATE_MAX[ANDI_OUTPUTS] = {[0 ... ANDI_OUTPUTS - 1] = 5.0f};
131#endif
132
133#ifdef STABILIZATION_ANDI_THRUST_MIN
134 const float THRUST_MIN = STABILIZATION_ANDI_THRUST_MIN;
135#else
136 const float THRUST_MIN = 0.0f;
137#endif
138
139#ifdef STABILIZATION_ANDI_THRUST_MAX
140 const float THRUST_MAX = STABILIZATION_ANDI_THRUST_MAX;
141#else
142 #error "You must specify maximum specific thrust: STABILIZATION_ANDI_THRUST_MAX"
143#endif
144
145#ifdef PERIODIC_FREQUENCY
146 const float SAMPLE_TIME = 1.0f / PERIODIC_FREQUENCY;
147#else
148 #error "Periodic frequency is not defined."
149#endif
150
151#if ANDI_NUM_ACT > WLS_N_U_MAX
152 #error Matrix-WLS_N_U_MAX too small or not defined: define WLS_N_U_MAX >= ANDI_NUM_ACT in airframe file
153#endif
154#if ANDI_OUTPUTS > WLS_N_V_MAX
155 #error Matrix-WLS_N_V_MAX too small or not defined: define WLS_N_V_MAX >= ANDI_OUTPUTS in airframe file
156#endif
157
158#ifdef STABILIZATION_ANDI_WLS_WV
159const float WLS_WV[ANDI_OUTPUTS] = STABILIZATION_ANDI_WLS_WV;
160#else
161const float WLS_WV[ANDI_OUTPUTS] = {[0 ... ANDI_OUTPUTS - 1] = 1.0f};
162#endif
163
170#ifdef STABILIZATION_ANDI_WLS_WU
171float WLS_WU[ANDI_NUM_ACT] = STABILIZATION_ANDI_WLS_WU;
172#else
173float WLS_WU[ANDI_NUM_ACT] = {[0 ... ANDI_NUM_ACT - 1] = 1.0f};
174#endif
175
176#ifdef STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA
177const struct FloatVect3 CUTOFF_FREQ_OMEGA = {
178 .x = STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA,
179 .y = STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA,
180 .z = STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA
181}; // rad/s
182#else
183#error "You must specify the cutoff frequency for omega: STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA"
184#endif
185
186#ifdef STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA_DOT
187const struct FloatVect3 CUTOFF_FREQ_OMEGA_DOT = {
188 .x = STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA_DOT,
189 .y = STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA_DOT,
190 .z = STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA_DOT
191}; // rad/s
192const float CUTOFF_FREQ_ACTUATOR[ANDI_NUM_ACT] = {[0 ... ANDI_NUM_ACT - 1] = STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA_DOT}; // rad/s
193const float CUTOFF_FREQ_THRUST = STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA_DOT; // rad/s
194#else
195#error "You must specify the cutoff frequency for omega_dot: STABILIZATION_ANDI_CUTOFF_FREQ_OMEGA_DOT"
196#endif
197
198#ifdef STABILIZATION_ANDI_CUTOFF_FREQ_ACCEL
199const struct FloatVect3 CUTOFF_FREQ_ACCEL = {
200 .x = STABILIZATION_ANDI_CUTOFF_FREQ_ACCEL,
201 .y = STABILIZATION_ANDI_CUTOFF_FREQ_ACCEL,
202 .z = STABILIZATION_ANDI_CUTOFF_FREQ_ACCEL
203};
204#else
205#error "You must specify the cutoff frequency for accel: STABILIZATION_ANDI_CUTOFF_FREQ_ACCEL"
206#endif
207
208#ifdef STABILIZATION_ANDI_CUTOFF_FREQ_VEL
209const struct FloatVect3 CUTOFF_FREQ_VEL = {
210 .x = STABILIZATION_ANDI_CUTOFF_FREQ_VEL,
211 .y = STABILIZATION_ANDI_CUTOFF_FREQ_VEL,
212 .z = STABILIZATION_ANDI_CUTOFF_FREQ_VEL
213}; // rad/s
214#else
215#error "You must specify the cutoff frequency for vel: STABILIZATION_ANDI_CUTOFF_FREQ_VEL"
216#endif
217
218
219// Function prototypes
220static void actuators_t4_in_callback(uint8_t sender_id, struct ActuatorsT4In *actuators_t4_in_ptr,
221 float *actuators_t4_extra_data_in_ptr);
222static void fetch_actuators_t4(float actuator_meas[ANDI_NUM_ACT], const struct ActuatorsT4In *actuators_t4_in_ptr);
227static void generate_reference_rate(float dt, const struct FloatRates *rate_des,
228 const struct GainsOrder2Vect3 *k_rate_rm, const struct AttQuat *bounds,
229 struct AttQuat *att_ref) __attribute__((unused));
230static void generate_reference_attitude(float dt, const struct FloatQuat *att_des,
231 const struct GainsOrder3Vect3 *k_att_rm, const struct AttQuat *bounds, struct AttQuat *att_ref);
232static void generate_reference_thrust(float dt, float thrust_des, const float k_thrust_rm,
233 const struct ThrustRef *bounds_min, const struct ThrustRef *bounds_max, struct ThrustRef *thrust_ref);
234static struct FloatVect3 control_error_rate(const struct AttQuat *att_ref, const struct AttStateQuat *att_state,
235 const struct GainsOrder2Vect3 *k_rate_ec) __attribute__((unused));
236static struct FloatVect3 control_error_attitude(const struct AttQuat *att_ref, const struct AttStateQuat *att_state,
237 const struct GainsOrder3Vect3 *k_att_ec);
238static float control_error_thrust(const struct ThrustRef *thrust_ref, const float thrust_state,
239 const float k_thrust_ec);
240static void compute_wls_upper_bounds(float u_d_max[ANDI_NUM_ACT], const float act_state[ANDI_NUM_ACT],
241 const float act_max[ANDI_NUM_ACT], const float act_rate_max[ANDI_NUM_ACT],
242 const float actuator_bandwidth[ANDI_NUM_ACT]);
243static void compute_wls_lower_bounds(float u_d_min[ANDI_NUM_ACT], const float act_state[ANDI_NUM_ACT],
244 const float act_min[ANDI_NUM_ACT], const float act_rate_min[ANDI_NUM_ACT],
245 const float actuator_bandwidth[ANDI_NUM_ACT]);
246static void compute_wls_u_scaler(float u_scaler[ANDI_NUM_ACT], const float act_min[ANDI_NUM_ACT],
247 const float act_max[ANDI_NUM_ACT]);
248static void compute_wls_v_scaler(float v_scaler[ANDI_NUM_ACT], const float v[ANDI_NUM_ACT]) __attribute__((unused));
249
250static inline float ec_k1_order3_f(const float omega_n, const float zeta, const float omega_a) { return (omega_n * omega_n * (omega_a - 2 * zeta * omega_n)); }
251static inline float ec_k2_order3_f(const float omega_n, const float zeta, const float omega_a) { return (omega_n * omega_n + 2.0f * zeta * omega_n * (omega_a - 2 * zeta * omega_n)); }
252static inline float ec_k3_order3_f(const float omega_n __attribute__((unused)),
253 const float zeta __attribute__((unused)), const float omega_a) { return omega_a; }
254static inline float rm_k1_order3_f(const float omega_n, const float zeta, const float omega_a) { return (omega_n * omega_n * omega_a) / (omega_n * omega_n + 2 * zeta * omega_n * omega_a); }
255static inline float rm_k2_order3_f(const float omega_n, const float zeta, const float omega_a) { return (omega_n * omega_n + 2 * zeta * omega_n * omega_a) / (2 * zeta * omega_n + omega_a); }
256static inline float rm_k3_order3_f(const float omega_n, const float zeta, const float omega_a) { return 2 * zeta * omega_n + omega_a; }
257
258static inline float ec_k1_order2_f(const float omega_a, const float zeta) { return omega_a * omega_a / (4 * zeta * zeta); }
259static inline float ec_k2_order2_f(const float omega_a, const float zeta __attribute__((unused))) { return omega_a; }
260static inline float rm_k1_order2_f(const float omega_a, const float zeta) { return ec_k2_order2_f(omega_a, zeta) / ec_k2_order2_f(omega_a, zeta); }
261static inline float rm_k2_order2_f(const float omega_a, const float zeta) { return ec_k2_order2_f(omega_a, zeta); }
262
264 .omega_a = {
265 .x = STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_X,
266 .y = STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_Y,
267 .z = STABILIZATION_ANDI_POLE_RATE_EC_OMEGA_A_Z
268 },
269 .zeta = {
270 .x = STABILIZATION_ANDI_POLE_RATE_EC_ZETA_X,
271 .y = STABILIZATION_ANDI_POLE_RATE_EC_ZETA_Y,
272 .z = STABILIZATION_ANDI_POLE_RATE_EC_ZETA_Z
273 }
274};
276 .omega_a = {
277 .x = STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_X,
278 .y = STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_Y,
279 .z = STABILIZATION_ANDI_POLE_RATE_RM_OMEGA_A_Z
280 },
281 .zeta = {
282 .x = STABILIZATION_ANDI_POLE_RATE_RM_ZETA_X,
283 .y = STABILIZATION_ANDI_POLE_RATE_RM_ZETA_Y,
284 .z = STABILIZATION_ANDI_POLE_RATE_RM_ZETA_Z
285 }
286};
288 .omega_n = {
289 .x = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_X,
290 .y = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_Y,
291 .z = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_N_Z
292 },
293 .zeta = {
294 .x = STABILIZATION_ANDI_POLE_ATT_EC_ZETA_X,
295 .y = STABILIZATION_ANDI_POLE_ATT_EC_ZETA_Y,
296 .z = STABILIZATION_ANDI_POLE_ATT_EC_ZETA_Z
297 },
298 .omega_a = {
299 .x = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_X,
300 .y = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_Y,
301 .z = STABILIZATION_ANDI_POLE_ATT_EC_OMEGA_A_Z
302 }
303};
305 .omega_n = {
306 .x = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_X,
307 .y = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_Y,
308 .z = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_N_Z
309 },
310 .zeta = {
311 .x = STABILIZATION_ANDI_POLE_ATT_RM_ZETA_X,
312 .y = STABILIZATION_ANDI_POLE_ATT_RM_ZETA_Y,
313 .z = STABILIZATION_ANDI_POLE_ATT_RM_ZETA_Z
314 },
315 .omega_a = {
316 .x = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_X,
317 .y = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_Y,
318 .z = STABILIZATION_ANDI_POLE_ATT_RM_OMEGA_A_Z
319 }
320};
321float andi_p_thrust_ec = STABILIZATION_ANDI_POLE_THRUST_EC;
322float andi_p_thrust_rm = STABILIZATION_ANDI_POLE_THRUST_RM;
323
324
325// WLS allocation variables
327 .nu = ANDI_NUM_ACT,
328 .nv = ANDI_OUTPUTS,
329 .gamma_sq = 100000.0,
330 .u_pref = {0.0f},
331 .u_min = {0.0f},
332 .u_max = {0.0f},
333 .PC = 0.0f,
334 .SC = 0.0f,
335 .iter = 0
336};
337
338// Controller gains
345
346// Complementary filter instances for state dependent contribution
350
354
355// Actuator and delay
356struct FirstOrderZOHLowPass actuator_obm_zohlpf[ANDI_NUM_ACT]; // ZOH low pass filter for actuator model
357struct TransportDelay actuator_obm_delay[ANDI_NUM_ACT]; // transport delay for actuator model
358float actuator_state[ANDI_NUM_ACT]; // from feedback or model
359float actuator_state_lpf[ANDI_NUM_ACT]; // delayed actuator state
360
361// T4 Actuator feedback handling
364float actuator_meas[ANDI_NUM_ACT]; // measured actuator feedback
365
366// Raw state measurement variables
368
369// State variables
370struct AttStateQuat attitude_state_cf; // undelayed attitude state for feedforward
371struct LinState linear_state_cf; // undelayed linear state for feedforward
372float thrust_state; // delayed thrust state for feedback
373
374// Reference model variables
377
378// Setpoints
382
383// Bounds
387
388// Controller variables
394
395// Pseudo command variables
396float nu_obj[ANDI_OUTPUTS]; // Total pseudo command allocated to the actuators
397float nu_ec[ANDI_OUTPUTS]; // Pseudo command from the error controller
398float nu_obm[ANDI_OUTPUTS]; // Pseudo command from the on board model state dependent term
399// float nu_reconstructed[ANDI_OUTPUTS]; // Reconstructed angular acceleration from the actuator commands (for model verification)
400
401#if PERIODIC_TELEMETRY
403static void send_wls_v_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
404{
405 char *name = "andi";
406 pprz_msg_send_WLS_V(trans, dev, AC_ID,
407 strlen(name), name,
408 &wls_stab_p.gamma_sq, // Does this need scaling as a function of scaling factor?
409 (uint8_t *)&wls_stab_p.iter,
411 ANDI_OUTPUTS, (float *)WLS_WV);
412}
413
414static void send_wls_u_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
415{
416 char *name = "andi";
417 float zero_array[ANDI_NUM_ACT] = {0.0f};
418 pprz_msg_send_WLS_U(trans, dev, AC_ID,
419 strlen(name), name,
420 ANDI_NUM_ACT, (float *)WLS_WU,
421 ANDI_NUM_ACT, zero_array,
425}
426
427static void send_eff_mat_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
428{
429 pprz_msg_send_EFF_MAT_STAB(trans, dev, AC_ID,
434}
435
436static void send_stab_attitude_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
437{
438 pprz_msg_send_STAB_ATTITUDE(trans, dev, AC_ID,
439 4, (float *)&attitude_des,
440 4, (float *)&attitude_state_cf.att,
441 4, (float *)&attitude_ref.att,
442 3, (float *)&attitude_state_cf.att_d,
443 3, (float *)&attitude_ref.att_d,
444 3, (float *)&attitude_state_cf.att_2d,
445 3, (float *)&attitude_ref.att_2d,
446 3, (float *)&attitude_ref.att_3d,
448}
449// For debug only
450// static void send_stab_thrust_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
451// {
452// pprz_msg_send_STAB_THRUST(trans, dev, AC_ID,
453// &thrust_des,
454// &thrust_ref.thrust,
455// &thrust_state,
456// &thrust_ref.thrust_d);
457// }
458
459// static void send_on_board_model_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
460// {
461// pprz_msg_send_ON_BOARD_MODEL(trans, dev, AC_ID,
462// 3, (float *)&linear_state_cf.vel,
463// 3, (float *)&linear_state_cf.acc,
464// ANDI_NUM_ACT, (float *)&actuator_meas,
465// ANDI_OUTPUTS, (float *)&nu_obm);
466// }
467
468// static void send_stab_pseudo_command_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
469// {
470// /**
471// * nu_obj: Total pseudo command sent to the actuators
472// * nu_ec: Pseudo command from the error controller
473// * nu_obm: Pseudo command from the on board model state dependent term
474// * nu_reconstructed: Reconstructed angular acceleration from the actuator commands.
475// * This is used to verify the on board model. If the OBM is perfect, then nu_reconstructed should be
476// * equal to the measured state plus any external disturbances.
477// *
478// * nu_obj = nu_ec + nu_obm
479// * nu_reconstructed = Ce * u_meas - nu_obm NOTE: This is wrong equation, fix it.
480// *
481// * FIXME: nu_reconstructed has been repurposed and no longer is the reconstructed value. Update the telemetry message accordingly.
482// */
483// pprz_msg_send_STAB_PSEUDO_COMMAND(trans, dev, AC_ID,
484// ANDI_OUTPUTS, nu_obj, // Total pseudo command
485// ANDI_OUTPUTS, nu_ec, // From error controller
486// ANDI_OUTPUTS, nu_obm, // From on board model state dependent term
487// ANDI_OUTPUTS, nu_reconstructed); // Reconstructed from actuator commands
488// }
489
490#endif // PERIODIC_TELEMETRY
491
492static void actuators_t4_in_callback(uint8_t sender_id __attribute__((unused)),
493 struct ActuatorsT4In *actuators_t4_in_ptr, float *actuators_t4_extra_data_in_ptr __attribute__((unused)));
494
506static void actuators_t4_in_callback(uint8_t sender_id __attribute__((unused)),
507 struct ActuatorsT4In *actuators_t4_in_ptr, float *actuators_t4_extra_data_in_ptr __attribute__((unused)))
508{
509 // Copy the entire ActuatorsT4In struct from the pointer to actuator_obs
510 memcpy(&actuators_t4_obs, actuators_t4_in_ptr, sizeof(struct ActuatorsT4In));
511}
512
531static void fetch_actuators_t4(float actuator_meas[ANDI_NUM_ACT], const struct ActuatorsT4In *actuators_t4_in_ptr)
532{
533 actuator_meas[0] = RadOfCentiDeg((float)actuators_t4_in_ptr->servo_1_angle);
534 actuator_meas[1] = -RadOfCentiDeg((float)actuators_t4_in_ptr->servo_6_angle);
535 actuator_meas[2] = (float)actuators_t4_in_ptr->esc_1_rpm * 2 * M_PI / 60; // Convert rpm to rad/s
536 actuator_meas[2] *= actuator_meas[2]; // square motor rpm
537 actuator_meas[3] = (float)actuators_t4_in_ptr->esc_2_rpm * 2 * M_PI / 60;
538 actuator_meas[3] *= actuator_meas[3]; // square motor rpm
539}
540
548{
549 struct GainsOrder3Vect3 gains;
550 gains.k1.x = rm_k1_order3_f(poles->omega_n.x, poles->zeta.x, poles->omega_a.x);
551 gains.k1.y = rm_k1_order3_f(poles->omega_n.y, poles->zeta.y, poles->omega_a.y);
552 gains.k1.z = rm_k1_order3_f(poles->omega_n.z, poles->zeta.z, poles->omega_a.z);
553
554 gains.k2.x = rm_k2_order3_f(poles->omega_n.x, poles->zeta.x, poles->omega_a.x);
555 gains.k2.y = rm_k2_order3_f(poles->omega_n.y, poles->zeta.y, poles->omega_a.y);
556 gains.k2.z = rm_k2_order3_f(poles->omega_n.z, poles->zeta.z, poles->omega_a.z);
557
558 gains.k3.x = rm_k3_order3_f(poles->omega_n.x, poles->zeta.x, poles->omega_a.x);
559 gains.k3.y = rm_k3_order3_f(poles->omega_n.y, poles->zeta.y, poles->omega_a.y);
560 gains.k3.z = rm_k3_order3_f(poles->omega_n.z, poles->zeta.z, poles->omega_a.z);
561 return gains;
562}
563
571{
572 struct GainsOrder2Vect3 gains;
573 gains.k1.x = rm_k1_order2_f(poles->omega_a.x, poles->zeta.x);
574 gains.k1.y = rm_k1_order2_f(poles->omega_a.y, poles->zeta.y);
575 gains.k1.z = rm_k1_order2_f(poles->omega_a.z, poles->zeta.z);
576
577 gains.k2.x = rm_k2_order2_f(poles->omega_a.x, poles->zeta.x);
578 gains.k2.y = rm_k2_order2_f(poles->omega_a.y, poles->zeta.y);
579 gains.k2.z = rm_k2_order2_f(poles->omega_a.z, poles->zeta.z);
580 return gains;
581}
582
590{
591 struct GainsOrder3Vect3 gains;
592 gains.k1.x = ec_k1_order3_f(poles->omega_n.x, poles->zeta.x, poles->omega_a.x);
593 gains.k1.y = ec_k1_order3_f(poles->omega_n.y, poles->zeta.y, poles->omega_a.y);
594 gains.k1.z = ec_k1_order3_f(poles->omega_n.z, poles->zeta.z, poles->omega_a.z);
595
596 gains.k2.x = ec_k2_order3_f(poles->omega_n.x, poles->zeta.x, poles->omega_a.x);
597 gains.k2.y = ec_k2_order3_f(poles->omega_n.y, poles->zeta.y, poles->omega_a.y);
598 gains.k2.z = ec_k2_order3_f(poles->omega_n.z, poles->zeta.z, poles->omega_a.z);
599
600 gains.k3.x = ec_k3_order3_f(poles->omega_n.x, poles->zeta.x, poles->omega_a.x);
601 gains.k3.y = ec_k3_order3_f(poles->omega_n.y, poles->zeta.y, poles->omega_a.y);
602 gains.k3.z = ec_k3_order3_f(poles->omega_n.z, poles->zeta.z, poles->omega_a.z);
603 return gains;
604}
605
613{
614 struct GainsOrder2Vect3 gains;
615 gains.k1.x = ec_k1_order2_f(poles->omega_a.x, poles->zeta.x);
616 gains.k1.y = ec_k1_order2_f(poles->omega_a.y, poles->zeta.y);
617 gains.k1.z = ec_k1_order2_f(poles->omega_a.z, poles->zeta.z);
618
619 gains.k2.x = ec_k2_order2_f(poles->omega_a.x, poles->zeta.x);
620 gains.k2.y = ec_k2_order2_f(poles->omega_a.y, poles->zeta.y);
621 gains.k2.z = ec_k2_order2_f(poles->omega_a.z, poles->zeta.z);
622 return gains;
623}
624
640 float dt,
641 const struct FloatRates *rate_des,
642 const struct GainsOrder2Vect3 *k_rate_rm,
643 const struct AttQuat *bounds,
644 struct AttQuat *att_ref)
645{
646 float p_des = rate_des->p;
647 float q_des = rate_des->q;
648 float r_des = rate_des->r;
649
650 BoundAbs(p_des, bounds->att_d.p);
651 BoundAbs(q_des, bounds->att_d.q);
652 BoundAbs(r_des, bounds->att_d.r);
653
654 float p_d_des = k_rate_rm->k1.x * (p_des - att_ref->att_d.p);
655 float q_d_des = k_rate_rm->k1.y * (q_des - att_ref->att_d.q);
656 float r_d_des = k_rate_rm->k1.z * (r_des - att_ref->att_d.r);
657
658 BoundAbs(p_d_des, bounds->att_2d.x);
659 BoundAbs(q_d_des, bounds->att_2d.y);
660 BoundAbs(r_d_des, bounds->att_2d.z);
661
662 float p_2d_des = k_rate_rm->k2.x * (p_d_des - att_ref->att_2d.x);
663 float q_2d_des = k_rate_rm->k2.y * (q_d_des - att_ref->att_2d.y);
664 float r_2d_des = k_rate_rm->k2.z * (r_d_des - att_ref->att_2d.z);
665
666 BoundAbs(p_2d_des, bounds->att_3d.x);
667 BoundAbs(q_2d_des, bounds->att_3d.y);
668 BoundAbs(r_2d_des, bounds->att_3d.z);
669
670 att_ref->att_3d.x = p_2d_des;
671 att_ref->att_3d.y = q_2d_des;
672 att_ref->att_3d.z = r_2d_des;
673
674 att_ref->att_2d.x += p_2d_des * dt;
675 att_ref->att_2d.y += q_2d_des * dt;
676 att_ref->att_2d.z += r_2d_des * dt;
677
678 att_ref->att_d.p += att_ref->att_2d.x * dt;
679 att_ref->att_d.q += att_ref->att_2d.y * dt;
680 att_ref->att_d.r += att_ref->att_2d.z * dt;
681
682 float_quat_identity(&att_ref->att);
683}
684
704 float dt,
705 const struct FloatQuat *att_des,
706 const struct GainsOrder3Vect3 *k_att_rm,
707 const struct AttQuat *bounds,
708 struct AttQuat *att_ref)
709{
710 struct FloatQuat att_err; // rotation from ref to des
711 float_quat_inv_comp_norm_shortest(&att_err, &att_ref->att, att_des);
712 float p_des = k_att_rm->k1.x * att_err.qx * 2;
713 float q_des = k_att_rm->k1.y * att_err.qy * 2;
714 float r_des = k_att_rm->k1.z * att_err.qz * 2;
715
716 BoundAbs(p_des, bounds->att_d.p);
717 BoundAbs(q_des, bounds->att_d.q);
718 BoundAbs(r_des, bounds->att_d.r);
719
720 float p_d_des = k_att_rm->k2.x * (p_des - att_ref->att_d.p);
721 float q_d_des = k_att_rm->k2.y * (q_des - att_ref->att_d.q);
722 float r_d_des = k_att_rm->k2.z * (r_des - att_ref->att_d.r);
723
724 BoundAbs(p_d_des, bounds->att_2d.x);
725 BoundAbs(q_d_des, bounds->att_2d.y);
726 BoundAbs(r_d_des, bounds->att_2d.z);
727
728 float p_2d_des = k_att_rm->k3.x * (p_d_des - att_ref->att_2d.x);
729 float q_2d_des = k_att_rm->k3.y * (q_d_des - att_ref->att_2d.y);
730 float r_2d_des = k_att_rm->k3.z * (r_d_des - att_ref->att_2d.z);
731
732 BoundAbs(p_2d_des, bounds->att_3d.x);
733 BoundAbs(q_2d_des, bounds->att_3d.y);
734 BoundAbs(r_2d_des, bounds->att_3d.z);
735
736 att_ref->att_3d.x = p_2d_des;
737 att_ref->att_3d.y = q_2d_des;
738 att_ref->att_3d.z = r_2d_des;
739
740 att_ref->att_2d.x += p_2d_des * dt;
741 att_ref->att_2d.y += q_2d_des * dt;
742 att_ref->att_2d.z += r_2d_des * dt;
743
744 att_ref->att_d.p += att_ref->att_2d.x * dt;
745 att_ref->att_d.q += att_ref->att_2d.y * dt;
746 att_ref->att_d.r += att_ref->att_2d.z * dt;
747
748 float_quat_integrate(&att_ref->att, &att_ref->att_d, dt);
749 float_quat_normalize(&att_ref->att);
750}
751
752
767 float dt,
768 float thrust_des,
769 const float k_thrust_rm,
770 const struct ThrustRef *bounds_min,
771 const struct ThrustRef *bounds_max,
772 struct ThrustRef *thrust_ref)
773{
774 Bound(thrust_des, bounds_min->thrust, bounds_max->thrust);
775 thrust_ref->thrust_d = k_thrust_rm * (thrust_des - thrust_ref->thrust);
776
777 Bound(thrust_ref->thrust_d, bounds_min->thrust_d, bounds_max->thrust_d);
778 thrust_ref->thrust += thrust_ref->thrust_d * dt;
779}
780
795 const struct AttQuat *att_ref,
796 const struct AttStateQuat *att_state,
797 const struct GainsOrder2Vect3 *k_rate_ec)
798{
799 struct FloatVect3 nu = att_ref->att_3d;
800
801 nu.x += k_rate_ec->k2.x * (att_ref->att_2d.x - att_state->att_2d.x);
802 nu.y += k_rate_ec->k2.y * (att_ref->att_2d.y - att_state->att_2d.y);
803 nu.z += k_rate_ec->k2.z * (att_ref->att_2d.z - att_state->att_2d.z);
804
805 nu.x += k_rate_ec->k1.x * (att_ref->att_d.p - att_state->att_d.p);
806 nu.y += k_rate_ec->k1.y * (att_ref->att_d.q - att_state->att_d.q);
807 nu.z += k_rate_ec->k1.z * (att_ref->att_d.r - att_state->att_d.r);
808
809 return nu;
810}
811
826 const struct AttQuat *att_ref,
827 const struct AttStateQuat *att_state,
828 const struct GainsOrder3Vect3 *k_att_ec)
829{
830 struct FloatVect3 nu = att_ref->att_3d;
831
832 nu.x += k_att_ec->k3.x * (att_ref->att_2d.x - att_state->att_2d.x);
833 nu.y += k_att_ec->k3.y * (att_ref->att_2d.y - att_state->att_2d.y);
834 nu.z += k_att_ec->k3.z * (att_ref->att_2d.z - att_state->att_2d.z);
835
836 nu.x += k_att_ec->k2.x * (att_ref->att_d.p - att_state->att_d.p);
837 nu.y += k_att_ec->k2.y * (att_ref->att_d.q - att_state->att_d.q);
838 nu.z += k_att_ec->k2.z * (att_ref->att_d.r - att_state->att_d.r);
839
840 struct FloatQuat att_err; // rotation from state to reference
841 float_quat_inv_comp_norm_shortest(&att_err, &att_state->att, &att_ref->att);
842 // Multiplication by 2 needed to convert quaternion difference to angular rate.
843 nu.x += k_att_ec->k1.x * att_err.qx * 2;
844 nu.y += k_att_ec->k1.y * att_err.qy * 2;
845 nu.z += k_att_ec->k1.z * att_err.qz * 2;
846
847 return nu;
848}
849
866 const struct ThrustRef *thrust_ref,
867 const float thrust_state,
868 const float k_thrust_ec)
869{
870 float nu = thrust_ref->thrust_d;
871 nu += k_thrust_ec * (thrust_ref->thrust - thrust_state);
872 return nu;
873}
874
889static void compute_wls_upper_bounds(float u_d_max[ANDI_NUM_ACT], const float act_state[ANDI_NUM_ACT],
890 const float act_max[ANDI_NUM_ACT], const float act_rate_max[ANDI_NUM_ACT], const float actuator_bandwidth[ANDI_NUM_ACT])
891{
892 for (uint8_t i = 0; i < ANDI_NUM_ACT; i++) {
893 // Calculate max rate allowed to avoid exceeding actuator max position in one timestep
894 float rate_limit_pos = (act_max[i] - act_state[i]) * actuator_bandwidth[i];
895 u_d_max[i] = (rate_limit_pos < act_rate_max[i]) ? rate_limit_pos : act_rate_max[i];
896 Bound(u_d_max[i], 0.0f, INFINITY);
897 }
898}
899
915static void compute_wls_lower_bounds(float u_d_min[ANDI_NUM_ACT], const float act_state[ANDI_NUM_ACT],
916 const float act_min[ANDI_NUM_ACT], const float act_rate_min[ANDI_NUM_ACT], const float actuator_bandwidth[ANDI_NUM_ACT])
917{
918 for (uint8_t i = 0; i < ANDI_NUM_ACT; i++) {
919 // Calculate min rate allowed to avoid going below actuator min position in one timestep
920 float rate_limit_pos = (act_min[i] - act_state[i]) * actuator_bandwidth[i];
921 u_d_min[i] = (rate_limit_pos > act_rate_min[i]) ? rate_limit_pos : act_rate_min[i];
922 Bound(u_d_min[i], -INFINITY, 0.0f);
923 }
924}
925
940static void compute_wls_u_scaler(float u_scaler[ANDI_NUM_ACT], const float act_min[ANDI_NUM_ACT],
941 const float act_max[ANDI_NUM_ACT])
942{
943 for (uint8_t i = 0; i < ANDI_NUM_ACT; i++) {
944 float range = fabs(act_max[i] - act_min[i]);
945 if (range <= FLT_EPSILON) {
946 u_scaler[i] = 1.0f;
947 } else {
948 u_scaler[i] = 1.0f / range;
949 }
950 }
951}
952
964static void compute_wls_v_scaler(float v_scaler[ANDI_NUM_ACT], const float v[ANDI_NUM_ACT])
965{
966 for (uint8_t i = 0; i < ANDI_NUM_ACT; i++) {
967 if (fabs(v[i]) <= FLT_EPSILON) {
968 v_scaler[i] = 1.0f;
969 } else {
970 v_scaler[i] = 1.0f / fabs(v[i]);
971 }
972 }
973}
974
976{
977 // Compute gains
984
985 // Initialize state variables
986 rates_prev.p = 0.0f;
987 rates_prev.q = 0.0f;
988 rates_prev.r = 0.0f;
989
991 attitude_state_cf.att_d.p = 0.0f;
992 attitude_state_cf.att_d.q = 0.0f;
993 attitude_state_cf.att_d.r = 0.0f;
994 attitude_state_cf.att_2d.x = 0.0f;
995 attitude_state_cf.att_2d.y = 0.0f;
996 attitude_state_cf.att_2d.z = 0.0f;
997
998 linear_state_cf.vel.x = 0.0f;
999 linear_state_cf.vel.y = 0.0f;
1000 linear_state_cf.vel.z = 0.0f;
1001 linear_state_cf.acc.x = 0.0f;
1002 linear_state_cf.acc.y = 0.0f;
1003 linear_state_cf.acc.z = 0.0f;
1004
1005 thrust_state = 0.0f;
1012 // float_vect_zero(nu_reconstructed, ANDI_NUM_ACT);
1013
1014 // Initialize reference variables
1016 attitude_ref.att_d.p = 0.0f;
1017 attitude_ref.att_d.q = 0.0f;
1018 attitude_ref.att_d.r = 0.0f;
1019 attitude_ref.att_2d.x = 0.0f;
1020 attitude_ref.att_2d.y = 0.0f;
1021 attitude_ref.att_2d.z = 0.0f;
1022 attitude_ref.att_3d.x = 0.0f;
1023 attitude_ref.att_3d.y = 0.0f;
1024 attitude_ref.att_3d.z = 0.0f;
1025
1026 thrust_ref.thrust = 0.0f;
1027 thrust_ref.thrust_d = 0.0f;
1028
1029 // FIXME: These bounds should be set via parameters
1030 // Initialize attitude bounds (symmetric bounds on abs values)
1032 attitude_bounds.att_d.p = 100.0f;
1033 attitude_bounds.att_d.q = 100.0f;
1034 attitude_bounds.att_d.r = 25.0f;
1035 attitude_bounds.att_2d.x = 100.0f;
1036 attitude_bounds.att_2d.y = 100.0f;
1037 attitude_bounds.att_2d.z = 20.0f;
1038 attitude_bounds.att_3d.x = 100.0f;
1039 attitude_bounds.att_3d.y = 100.0f;
1040 attitude_bounds.att_3d.z = 100.0f;
1041
1042 // Limit thrust bounds (asymmetric bounds)
1044 thrust_bounds_max.thrust = THRUST_MAX;
1045 thrust_bounds_min.thrust_d = -1000.0f;
1046 thrust_bounds_max.thrust_d = 1000.0f;
1047
1048 // Initial control effectiveness matrix
1050
1051 // Initialize filters
1052 init_first_order_complementary_vect3(&attitude_rates_cf, &CUTOFF_FREQ_OMEGA, SAMPLE_TIME);
1053 init_butterworth_2_complementary_vect3(&attitude_accel_cf, &CUTOFF_FREQ_OMEGA_DOT, SAMPLE_TIME);
1054 angular_rates_obm.p = 0.0f;
1055 angular_rates_obm.q = 0.0f;
1056 angular_rates_obm.r = 0.0f;
1057
1058 init_first_order_complementary_vect3(&linear_vel_cf, &CUTOFF_FREQ_VEL, SAMPLE_TIME);
1059 init_butterworth_2_complementary_vect3(&linear_accel_cf, &CUTOFF_FREQ_ACCEL, SAMPLE_TIME);
1060 linear_velocity_obm.x = 0.0f;
1061 linear_velocity_obm.y = 0.0f;
1062 linear_velocity_obm.z = 0.0f;
1063
1064 // Bind T4 actuator feedback abi message
1065 AbiBindMsgACTUATORS_T4_IN(ABI_BROADCAST, &actuators_t4_in_event, actuators_t4_in_callback);
1066
1067 // Precompute discrete-time actuator dynamics coefficients
1070
1071// Start telemetry
1072#if PERIODIC_TELEMETRY
1074 // register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_THRUST, send_stab_thrust_stabilization_andi); // For debug only, dont forget to add your message.
1078 // register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_PSEUDO_COMMAND, send_stab_pseudo_command_stabilization_andi);
1079 // register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ON_BOARD_MODEL, send_on_board_model_stabilization_andi);
1080#endif
1081}
1082
1094{
1095 // Reset actuator states to current measurements
1099
1100 // Clear previous rates
1101 rates_prev.p = 0.0f;
1102 rates_prev.q = 0.0f;
1103 rates_prev.r = 0.0f;
1104
1105 // Reset state to current measurements, higher order derivatives are set to zero
1107 linear_state_cf.acc.x = 0.0f;
1108 linear_state_cf.acc.y = 0.0f;
1109 linear_state_cf.acc.z = 0.0f;
1113
1116 attitude_state_cf.att_2d.x = 0.0f;
1117 attitude_state_cf.att_2d.y = 0.0f;
1118 attitude_state_cf.att_2d.z = 0.0f;
1122
1124 attitude_ref.att_d = attitude_state_cf.att_d;
1125 attitude_ref.att_2d = attitude_state_cf.att_2d;
1126 attitude_ref.att_3d.x = 0.0f;
1127 attitude_ref.att_3d.y = 0.0f;
1128 attitude_ref.att_3d.z = 0.0f;
1129
1131
1132 thrust_ref.thrust = thrust_state;
1133 thrust_ref.thrust_d = 0.0f;
1134
1135 // Reset controller states
1138
1139 // Initial control effectiveness matrix
1141
1142}
1143
1184void stabilization_andi_run(bool use_rate_control, bool in_flight, struct StabilizationSetpoint *stab_setpoint,
1185 struct ThrustSetpoint *thrust_setpoint, int32_t *cmd)
1186{
1187 // Recompute gains
1188 // FIXME: Only recompute when parameters change, use parameter update handler callback.
1195
1196 // Fetch linear measurements
1197 struct LinState lin_meas;
1199 (struct FloatVect3 *)stateGetSpeedNed_f()); // From Kalman filter
1200 lin_meas.acc = *stateGetAccelBody_f(); // From accelerometer
1201
1202 // Fetch attitude measurements
1203 struct AttStateQuat attitude_meas;
1204 attitude_meas.att = *stateGetNedToBodyQuat_f(); // From Kalman filter
1205 attitude_meas.att_d = *stateGetBodyRates_f(); // From gyroscope
1206 attitude_meas.att_2d.x = (attitude_meas.att_d.p - rates_prev.p) * PERIODIC_FREQUENCY;
1207 attitude_meas.att_2d.y = (attitude_meas.att_d.q - rates_prev.q) * PERIODIC_FREQUENCY;
1208 attitude_meas.att_2d.z = (attitude_meas.att_d.r - rates_prev.r) * PERIODIC_FREQUENCY;
1209 rates_prev = attitude_meas.att_d; // Store previous rates for next acceleration calculation
1210
1211 // Propagate and get on board model actuator state estimates
1212 float actuator_obm[ANDI_NUM_ACT];
1217
1218 // Get actuator measurements from T4 feedback
1220
1221 // FIXME: Make configurable which source to use for each actuator.
1222 // Choose actuator feedback for elevon and on board model for esc (because lack of rpm control).
1225 actuator_state[2] = actuator_obm[2];
1226 actuator_state[3] = actuator_obm[3];
1227
1228 // Evaluate On Board Model with previous filtered state estimates
1229 struct FloatVect3 angular_accel_obm = evaluate_obm_moments(&attitude_state_cf.att_d, &linear_state_cf.vel,
1231 struct FloatVect3 linear_accel_obm = evaluate_obm_forces(&attitude_state_cf.att_d, &linear_state_cf.vel,
1233
1234 // FIXME: Integral before filtering can cause numerical issues. Update complementary filter to handle this case.
1235 // FIXME: Numerical differentiation to get accelerations can be noisy. Consider using a filter before differentiation.
1236 // Cascaded complementary filter for linear velocity and accelerations measurements
1237 update_butterworth_2_complementary_vect3(&linear_accel_cf, &linear_accel_obm, &lin_meas.acc);
1242
1243 // Cascaded complementary filter for angular rates and accelerations measurements
1244 update_butterworth_2_complementary_vect3(&attitude_accel_cf, &angular_accel_obm, &attitude_meas.att_2d);
1249 attitude_state_cf.att = attitude_meas.att;
1250
1251 // Get thrust measurement from on board model
1253
1254 // Reconstructed nu is repurposed to hold modeled moments and specific thrust from the on board model (for obm validation).
1255 // This can be used to compare the on board model prediction to the actual measured acceleration to validate model accuracy.
1256 // Do not compare this to the complementary filter values since these are influenced by the model, use the raw measurements instead.
1257 // nu_reconstructed[0] = angular_accel_obm.x;
1258 // nu_reconstructed[1] = angular_accel_obm.y;
1259 // nu_reconstructed[2] = angular_accel_obm.z;
1260 // nu_reconstructed[3] = thrust_state;
1261
1262 // Get setpoints
1263 if (use_rate_control) {
1264 rates_des = stab_sp_to_rates_f(stab_setpoint);
1265 if (in_flight)
1267 } else {
1268 attitude_des = stab_sp_to_quat_f(stab_setpoint);
1269 if (in_flight)
1271 }
1272
1273 // FIXME: Thrust setpoint cant be of type THRUST_INCR_SP, this is not enforced but will silently fail
1274 thrust_des = th_sp_to_thrust_f(thrust_setpoint, 0, THRUST_AXIS_Z) * THRUST_MAX;
1276 &thrust_ref);
1277
1278 // Construct pseudo control
1279 struct FloatVect3 nu_attitude;
1280 if (use_rate_control) {
1282 } else {
1284 }
1286
1287 nu_ec[0] = nu_attitude.x;
1288 nu_ec[1] = nu_attitude.y;
1289 nu_ec[2] = nu_attitude.z;
1290 nu_ec[3] = nu_thrust;
1291
1292 // State feedback from on board model
1296
1297 if (in_flight) {
1298 nu_obj[0] = nu_ec[0] - (nu_obm[0] * ANDI_RELAX_OBM);
1299 nu_obj[1] = nu_ec[1] - (nu_obm[1] * ANDI_RELAX_OBM);
1300 nu_obj[2] = nu_ec[2] - (nu_obm[2] * ANDI_RELAX_OBM);
1301 } else {
1302 nu_obj[0] = 0.0f;
1303 nu_obj[1] = 0.0f;
1304 nu_obj[2] = 0.0f;
1305 }
1306 nu_obj[3] = nu_ec[3] - (nu_obm[3] * ANDI_RELAX_OBM);
1307
1308 // Compute control effectiveness matrix based on current states
1310
1311 // Solve control allocation using weighted least squares
1312 compute_wls_lower_bounds(du_min, actuator_state, ACTUATOR_MIN, ACTUATOR_D_MIN, ACTUATOR_DYNAMICS);
1313 compute_wls_upper_bounds(du_max, actuator_state, ACTUATOR_MAX, ACTUATOR_D_MAX, ACTUATOR_DYNAMICS);
1314 float wls_u_scaler[ANDI_NUM_ACT];
1315 float wls_v_scaler[ANDI_OUTPUTS] = {[0 ... ANDI_OUTPUTS - 1] = 1.0f}; // Disable v scaling
1316 compute_wls_u_scaler(wls_u_scaler, du_min,
1317 du_max); // FIXME: Compute only once in advance for fixed bounds (important for well defined WLS Wu weights)
1318 // compute_wls_v_scaler(wls_v_scaler, nu_obj);
1319
1320 float ce_mat_scaled[ANDI_OUTPUTS][ANDI_NUM_ACT];
1321 float *bwls[ANDI_OUTPUTS];
1322 // Scale control effectiveness matrix
1323 for (uint8_t i = 0; i < ANDI_OUTPUTS; i++) {
1324 for (uint8_t j = 0; j < ANDI_NUM_ACT; j++) {
1325 ce_mat_scaled[i][j] = ce_mat[i * ANDI_NUM_ACT + j] * wls_v_scaler[i] / wls_u_scaler[j];
1326 }
1327 bwls[i] = ce_mat_scaled[i];
1328 }
1329
1330 // Scale actuator bounds and weights
1331 for (uint8_t i = 0; i < ANDI_NUM_ACT; i++) {
1332 wls_stab_p.u_min[i] = du_min[i] * wls_u_scaler[i];
1333 wls_stab_p.u_max[i] = du_max[i] * wls_u_scaler[i];
1334 wls_stab_p.u_pref[i] = (wls_stab_p.u_min[i] + wls_stab_p.u_max[i]) / 2.0f; // Use mid-point as preferred command
1335 wls_stab_p.Wu[i] = WLS_WU[i];
1336 }
1337
1338 // Scale pseudo control and weights
1339 for (uint8_t i = 0; i < ANDI_OUTPUTS; i++) {
1340 wls_stab_p.v[i] = nu_obj[i] * wls_v_scaler[i];
1341 wls_stab_p.Wv[i] = WLS_WV[i];
1342 }
1343
1344 wls_alloc(&wls_stab_p, bwls, 0, 0, 10);
1345
1346 // Scale back actuator commands
1347 for (uint8_t i = 0; i < ANDI_NUM_ACT; i++) {
1348 du_cmd[i] = (wls_stab_p.u[i] / wls_u_scaler[i]);
1349 u_cmd[i] = du_cmd[i] / ACTUATOR_DYNAMICS[i] + actuator_state[i];
1350 Bound(u_cmd[i], ACTUATOR_MIN[i], ACTUATOR_MAX[i]);
1351 }
1352
1353 // Commit actuator commands
1354 // Resulting commands are in rad for servo and rad/s for motor
1355 // Paparazzi expects the commands in pprz units (-MAX_PPRZ to MAX_PPRZ for servo, 0 to MAX_PPRZ for motor).
1356 // FIXME: Do not hardcode actuator layout
1357 commands[0] = (pprz_t)(u_cmd[0] / ACTUATOR_MAX[0] * MAX_PPRZ);
1358 commands[1] = (pprz_t)(u_cmd[1] / ACTUATOR_MAX[1] * MAX_PPRZ);
1359 commands[2] = (pprz_t)(sqrt(u_cmd[2] / ACTUATOR_MAX[2]) * MAX_PPRZ);
1360 commands[3] = (pprz_t)(sqrt(u_cmd[3] / ACTUATOR_MAX[3]) * MAX_PPRZ);
1361
1362 // Set Thrust command for compatibility with other modules
1363 // FIXME: Do not hardcode actuator layout
1364 cmd[COMMAND_THRUST] = 0;
1365 cmd[COMMAND_THRUST] += (pprz_t)(sqrt(u_cmd[2] / ACTUATOR_MAX[2]) * MAX_PPRZ);
1366 cmd[COMMAND_THRUST] += (pprz_t)(sqrt(u_cmd[3] / ACTUATOR_MAX[3]) * MAX_PPRZ);
1367 cmd[COMMAND_THRUST] /= 2;
1368}
1369
1370// FIXME: The following functions are to integrate the controller in the existing stabilization framework, find a better way to do this or move.
1372{
1373 ;
1374}
1375
1377{
1378 ;
1379}
1380
1381void stabilization_rate_run(bool in_flight __attribute__((unused)),
1382 struct StabilizationSetpoint *rate_sp __attribute__((unused)), struct ThrustSetpoint *thrust __attribute__((unused)),
1383 int32_t *cmd __attribute__((unused)))
1384{
1385 ;
1386}
1387
1388void stabilization_attitude_run(bool in_flight __attribute__((unused)),
1389 struct StabilizationSetpoint *sp __attribute__((unused)), struct ThrustSetpoint *thrust __attribute__((unused)),
1390 int32_t *cmd __attribute__((unused)))
1391{
1392 ;
1393}
1394
1395// FIXME: This function is duplicated in stabilization_rate.c, find a way to share it
1397{
1398 struct FloatRates rate_sp;
1399 FLOAT_RATES_ZERO(rate_sp);
1401 rate_sp.p = rc->values[RC_RATE_P] * RC_RATE_MAX[0] / MAX_PPRZ;
1402 }
1404 rate_sp.q = rc->values[RC_RATE_Q] * RC_RATE_MAX[1] / MAX_PPRZ;
1405 }
1407 rate_sp.r = rc->values[RC_RATE_R] * RC_RATE_MAX[2] / MAX_PPRZ;
1408 }
1409 return stab_sp_from_rates_f(&rate_sp);
1410}
1411
1412// FIXME: Autopilot end flight detection is not applicable to tailsitters.
1413bool autopilot_in_flight_end_detection(bool motors_on __attribute__((unused)))
1414{
1415 return false;
1416}
1417
1418// DEBUG FUNCTION
1419// static int counter = 0;
1420// /**
1421// * @brief Debug function for actuators.
1422// *
1423// * Send step inputs to actuators and observe responses.
1424// * useful to estimate actuator dynamics.
1425// */
1426// void actuator_debug(bool do_servo_step, bool do_motor_step, float step_rate)
1427// {
1428// if (counter++ > PERIODIC_FREQUENCY * step_rate) {
1429// counter = 0;
1430// }
1431
1432// if (counter <= (PERIODIC_FREQUENCY * step_rate) / 2) {
1433// commands[0] = (pprz_t)(do_servo_step ? MAX_PPRZ : 0);
1434// commands[1] = (pprz_t)(do_servo_step ? MAX_PPRZ : 0);
1435// commands[2] = (pprz_t)(do_motor_step ? MAX_PPRZ : 0);
1436// commands[3] = (pprz_t)(do_motor_step ? MAX_PPRZ : 0);
1437// } else {
1438// commands[0] = (pprz_t)(do_servo_step ? MIN_PPRZ : 0);
1439// commands[1] = (pprz_t)(do_servo_step ? MIN_PPRZ : 0);
1440// commands[2] = (pprz_t)(do_motor_step ? 0 : 0);
1441// commands[3] = (pprz_t)(do_motor_step ? 0 : 0);
1442// }
1443// }
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
#define ABI_BROADCAST
Broadcast address.
Definition abi_common.h:80
Core autopilot interface common to all firmwares.
Hardware independent code for commands handling.
Implementation of complementary filters (first order, second order, and Butterworth variants).
Definitions and inline functions for 1st order complementary filter vector types.
static struct FloatVect3 update_butterworth_2_complementary_vect3(struct Butterworth2ComplementaryVect3 *filter, const struct FloatVect3 *value_x, const struct FloatVect3 *value_y)
Initialize 3D vector of 2nd order Butterworth complementary filters.
static struct FloatRates update_first_order_complementary_rates(struct FirstOrderComplementaryVect3 *filter, const struct FloatRates *value_x, const struct FloatRates *value_y)
Update 3D vector of 1st order complementary filters for rates.
static void reset_first_order_complementary_rates(struct FirstOrderComplementaryVect3 *filter, const struct FloatRates *value)
Reset 3D vector of 1st order complementary filters for rates to a specific value.
static void reset_first_order_complementary_vect3(struct FirstOrderComplementaryVect3 *filter, const struct FloatVect3 *value)
Reset 3D vector of 1st order complementary filters to a specific value.
static void init_butterworth_2_complementary_vect3(struct Butterworth2ComplementaryVect3 *filter, const struct FloatVect3 *cut_off, float sample_time)
Initialize 3D vector of 2nd order Butterworth complementary filters.
static struct FloatVect3 update_first_order_complementary_vect3(struct FirstOrderComplementaryVect3 *filter, const struct FloatVect3 *value_x, const struct FloatVect3 *value_y)
Update 3D vector of 1st order complementary filters.
static struct FloatRates get_first_order_complementary_rates(const struct FirstOrderComplementaryVect3 *filter)
Get current output rates from 1st order complementary filters.
static void reset_butterworth_2_complementary_vect3(struct Butterworth2ComplementaryVect3 *filter, const struct FloatVect3 *value)
Reset 3D vector of 2nd order Butterworth complementary filters to a specific value.
static struct FloatVect3 get_butterworth_2_complementary_vect3(const struct Butterworth2ComplementaryVect3 *filter)
Get current output 3D vector from 2nd order Butterworth complementary filters.
static void init_first_order_complementary_vect3(struct FirstOrderComplementaryVect3 *filter, const struct FloatVect3 *cut_off, float sample_time)
Initialize 3D vector of 1st order complementary filters.
static struct FloatVect3 get_first_order_complementary_vect3(const struct FirstOrderComplementaryVect3 *filter)
Get current output 3D vector from 1st order complementary filters.
3D vector of 2nd order Butterworth complementary filters.
3D vector of 1st order complementary filters.
static struct uart_periph * dev
float q
in rad/s
float p
in rad/s
float r
in rad/s
static void float_quat_normalize(struct FloatQuat *q)
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
static void float_vect_zero(float *a, const int n)
a = 0
void float_vect3_integrate_fi(struct FloatVect3 *vec, const struct FloatVect3 *dv, const float dt)
in place first order integration of a 3D-vector
void float_rates_vect3_integrate_fi(struct FloatRates *r, const struct FloatVect3 *dr, const float dt)
in place first order integration of angular rates
void float_quat_integrate(struct FloatQuat *q, const struct FloatRates *omega, const float dt)
in place quaternion integration with constant rotational velocity
#define FLOAT_RATES_ZERO(_r)
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, const struct FloatQuat *a2b, const struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
void float_quat_vmult(struct FloatVect3 *v_out, const struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
Roation quaternion.
angular rates
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition state.h:1313
static struct FloatVect3 * stateGetAccelBody_f(void)
Definition state.h:1111
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition state.h:1392
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition state.h:1060
Integrated Navigation System interface.
Simple first order low pass filter with bilinear transform.
Definitions and inline functions for 1st order low-pass filter vector types.
A first order low pass filter using Zero Order Hold (ZOH) discretization.
Zero Order Hold (ZOH) discrete first order low pass filter structure.
static void update_first_order_zoh_low_pass_array(const uint8_t n, struct FirstOrderZOHLowPass filter_array[restrict n], const float input_array[restrict n])
Update an array of first order ZOH low-pass filters.
static void init_first_order_zoh_low_pass_array(const uint8_t n, struct FirstOrderZOHLowPass filter_array[restrict n], const float omega[restrict n], const float sample_time)
Initialize a set of first order ZOH low-pass filters to zero for 3D vector data.
static void get_first_order_zoh_low_pass_array(const uint8_t n, const struct FirstOrderZOHLowPass filter_array[restrict n], float output_array[restrict n])
Retrieve the filtered outputs from an array of first order ZOH low-pass filters.
static void reset_first_order_zoh_low_pass_array(const uint8_t n, struct FirstOrderZOHLowPass filter_array[restrict n], const float value_array[restrict n])
Reset an array of first order ZOH low-pass filters to specific values.
Hardware independent API for actuators (servos, motor controllers).
Specific navigation functions for hybrid aircraft.
float evaluate_obm_thrust_z(const float actuator_state[ANDI_NUM_ACT])
Compute total thrust produced by the current actuator state.
struct FloatVect3 evaluate_obm_forces(const struct FloatRates *rates, const struct FloatVect3 *vel_body, const float actuator_state[ANDI_NUM_ACT])
Evaluate total force acting on the vehicle from the OBM.
struct FloatVect3 evaluate_obm_moments(const struct FloatRates *rates, const struct FloatVect3 *vel_body, const float actuator_state[ANDI_NUM_ACT])
Evaluate total moments acting on the vehicle from the OBM.
void evaluate_obm_f_stb_u(float fu_mat[ANDI_NUM_ACT *ANDI_OUTPUTS], const struct FloatRates *rates, const struct FloatVect3 *vel_body, const float actuator_state[ANDI_NUM_ACT])
Evaluate the state-dependent control effectiveness matrix F_u for stabilization.
void evaluate_obm_f_stb_x(float nu_obm[ANDI_OUTPUTS], const struct FloatRates *rates, const struct FloatVect3 *vel_body, const struct FloatVect3 *ang_accel, const struct FloatVect3 *accel_body, const float actuator_state[ANDI_NUM_ACT])
Evaluate the state-dependent contribution F_x * x_dot for stabilization.
struct Gains3rdOrder k_att_rm
float nu[ANDI_OUTPUTS]
float act_max[ANDI_NUM_ACT_TOT]
float act_min[ANDI_NUM_ACT_TOT]
#define ANDI_NUM_ACT
#define ANDI_OUTPUTS
float gains[MAX_SAMPLES_LEARNING]
int16_t pprz_t
Definition paparazzi.h:6
#define MAX_PPRZ
Definition paparazzi.h:8
Paparazzi floating point algebra.
Generic interface for radio control modules.
Rotorcraft navigation functions.
General attitude stabilization interface for rotorcrafts.
static const ShellCommand commands[]
Definition shell_arch.c:77
struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp)
float th_sp_to_thrust_f(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
struct StabilizationSetpoint stab_sp_from_rates_f(struct FloatRates *rates)
struct FloatQuat stab_sp_to_quat_f(struct StabilizationSetpoint *sp)
#define THRUST_AXIS_Z
const float ACTUATOR_PREF[ANDI_NUM_ACT]
struct AttQuat attitude_bounds
static float ec_k3_order3_f(const float omega_n, const float zeta, const float omega_a)
float actuator_state[ANDI_NUM_ACT]
const uint8_t ACTUATOR_DELAY[ANDI_NUM_ACT]
void stabilization_attitude_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Attitude control run function.
struct FirstOrderComplementaryVect3 attitude_rates_cf
float du_max[ANDI_NUM_ACT]
struct PolesOrder2Vect3 andi_p_rate_rm
static float control_error_thrust(const struct ThrustRef *thrust_ref, const float thrust_state, const float k_thrust_ec)
Computes the thrust control command based on desired and current thrust.
float andi_p_thrust_ec
void stabilization_andi_enter(void)
Initializes the ANDI stabilization controller state upon entering stabilization mode.
float andi_p_thrust_rm
static void generate_reference_attitude(float dt, const struct FloatQuat *att_des, const struct GainsOrder3Vect3 *k_att_rm, const struct AttQuat *bounds, struct AttQuat *att_ref)
Generates a bounded second-order reference signal for quaternion-based attitude control.
static void compute_wls_v_scaler(float v_scaler[ANDI_NUM_ACT], const float v[ANDI_NUM_ACT])
Compute output scaling factors for normalizing weighted least squares outputs.
struct GainsOrder3Vect3 andi_k_att_rm
void stabilization_attitude_enter(void)
Attitude control enter function.
static void compute_wls_lower_bounds(float u_d_min[ANDI_NUM_ACT], const float act_state[ANDI_NUM_ACT], const float act_min[ANDI_NUM_ACT], const float act_rate_min[ANDI_NUM_ACT], const float actuator_bandwidth[ANDI_NUM_ACT])
Compute lower bounds for actuator rate commands based on actuator state and constraints.
struct FloatRates angular_rates_obm
float andi_k_thrust_ec
static float ec_k1_order3_f(const float omega_n, const float zeta, const float omega_a)
const bool ACTUATOR_IS_SERVO[ANDI_NUM_ACT]
static void fetch_actuators_t4(float actuator_meas[ANDI_NUM_ACT], const struct ActuatorsT4In *actuators_t4_in_ptr)
Extract actuator states from ActuatorsT4In struct.
struct LinState linear_state_cf
void stabilization_andi_run(bool use_rate_control, bool in_flight, struct StabilizationSetpoint *stab_setpoint, struct ThrustSetpoint *thrust_setpoint, int32_t *cmd)
Main ANDI stabilization control loop.
float actuator_state_lpf[ANDI_NUM_ACT]
static struct GainsOrder2Vect3 compute_reference_gains_order_2_vect_3(const struct PolesOrder2Vect3 *poles)
Compute reference-model gains for a 2nd-order 3D system.
void stabilization_rate_enter(void)
struct PolesOrder3Vect3 andi_p_att_ec
static float rm_k1_order2_f(const float omega_a, const float zeta)
struct StabilizationSetpoint stabilization_rate_read_rc(struct RadioControl *rc)
static void generate_reference_thrust(float dt, float thrust_des, const float k_thrust_rm, const struct ThrustRef *bounds_min, const struct ThrustRef *bounds_max, struct ThrustRef *thrust_ref)
Generates a bounded second-order reference signal for thrust control.
static struct GainsOrder3Vect3 compute_error_gains_order_3_vect_3(const struct PolesOrder3Vect3 *poles)
Compute error-compensation gains for a 3rd-order 3D system.
struct PolesOrder2Vect3 andi_p_rate_ec
const float THRUST_MIN
struct FirstOrderComplementaryVect3 linear_vel_cf
struct FloatRates rates_des
struct GainsOrder3Vect3 andi_k_att_ec
struct Butterworth2ComplementaryVect3 linear_accel_cf
float nu_ec[ANDI_OUTPUTS]
static void generate_reference_rate(float dt, const struct FloatRates *rate_des, const struct GainsOrder2Vect3 *k_rate_rm, const struct AttQuat *bounds, struct AttQuat *att_ref)
Generates a bounded second-order reference signal for attitude rate control.
float u_cmd[ANDI_NUM_ACT]
struct WLS_t wls_stab_p
static void send_wls_u_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
void stabilization_rate_run(bool in_flight, struct StabilizationSetpoint *rate_sp, struct ThrustSetpoint *thrust, int32_t *cmd)
void stabilization_andi_init(void)
struct ActuatorsT4In actuators_t4_obs
const float RC_RATE_MAX[ANDI_OUTPUTS]
const bool USE_STATE_DYNAMICS
static struct GainsOrder2Vect3 compute_error_gains_order_2_vect_3(const struct PolesOrder2Vect3 *poles)
Compute error-compensation gains for a 2nd-order 3D system.
struct Butterworth2ComplementaryVect3 attitude_accel_cf
static struct FloatVect3 control_error_attitude(const struct AttQuat *att_ref, const struct AttStateQuat *att_state, const struct GainsOrder3Vect3 *k_att_ec)
Computes the attitude rate error control command.
const float WLS_WV[ANDI_OUTPUTS]
struct ThrustRef thrust_ref
static void send_eff_mat_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
static float rm_k2_order3_f(const float omega_n, const float zeta, const float omega_a)
float nu_obm[ANDI_OUTPUTS]
static float rm_k1_order3_f(const float omega_n, const float zeta, const float omega_a)
static void actuators_t4_in_callback(uint8_t sender_id, struct ActuatorsT4In *actuators_t4_in_ptr, float *actuators_t4_extra_data_in_ptr)
nu_obj: Total pseudo command sent to the actuators nu_ec: Pseudo command from the error controller nu...
static struct FloatVect3 control_error_rate(const struct AttQuat *att_ref, const struct AttStateQuat *att_state, const struct GainsOrder2Vect3 *k_rate_ec)
Computes the control error command for attitude rate regulation.
struct ThrustRef thrust_bounds_max
float nu_obj[ANDI_OUTPUTS]
struct FloatRates rates_prev
static void send_stab_attitude_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
struct AttStateQuat attitude_state_cf
struct FloatVect3 linear_velocity_obm
float actuator_meas[ANDI_NUM_ACT]
struct PolesOrder3Vect3 andi_p_att_rm
struct TransportDelay actuator_obm_delay[ANDI_NUM_ACT]
struct FirstOrderZOHLowPass actuator_obm_zohlpf[ANDI_NUM_ACT]
struct AttQuat attitude_ref
float andi_k_thrust_rm
float WLS_WU[ANDI_NUM_ACT]
Normalized actuator cost for WLS allocation.
static float rm_k2_order2_f(const float omega_a, const float zeta)
static float ec_k1_order2_f(const float omega_a, const float zeta)
abi_event actuators_t4_in_event
static void send_wls_v_stabilization_andi(struct transport_tx *trans, struct link_device *dev)
float thrust_des
static float ec_k2_order3_f(const float omega_n, const float zeta, const float omega_a)
const bool SCHEDULE_EFF
bool autopilot_in_flight_end_detection(bool motors_on)
float du_min[ANDI_NUM_ACT]
struct ThrustRef thrust_bounds_min
static void compute_wls_u_scaler(float u_scaler[ANDI_NUM_ACT], const float act_min[ANDI_NUM_ACT], const float act_max[ANDI_NUM_ACT])
Compute input scaling factors for normalizing weighted least squares.
static float ec_k2_order2_f(const float omega_a, const float zeta)
const float ANDI_RELAX_OBM
float du_cmd[ANDI_NUM_ACT]
struct FloatQuat attitude_des
struct GainsOrder2Vect3 andi_k_rate_ec
static void compute_wls_upper_bounds(float u_d_max[ANDI_NUM_ACT], const float act_state[ANDI_NUM_ACT], const float act_max[ANDI_NUM_ACT], const float act_rate_max[ANDI_NUM_ACT], const float actuator_bandwidth[ANDI_NUM_ACT])
Compute upper bounds for actuator rate commands based on actuator state and constraints.
float ce_mat[ANDI_NUM_ACT *ANDI_OUTPUTS]
static struct GainsOrder3Vect3 compute_reference_gains_order_3_vect_3(const struct PolesOrder3Vect3 *poles)
Compute reference-model gains for a 3rd-order 3D system.
struct GainsOrder2Vect3 andi_k_rate_rm
static float rm_k3_order3_f(const float omega_n, const float zeta, const float omega_a)
float thrust_state
ANDI stabilization controller for tiltbody rotorcraft.
struct FloatRates att_d
struct FloatVect3 k1
struct FloatVect3 vel
struct FloatQuat att
struct FloatVect3 acc
struct FloatVect3 att_2d
struct FloatQuat att
struct FloatRates att_d
struct FloatVect3 att_3d
struct FloatVect3 att_2d
struct FloatVect3 k2
Structure representing attitude in quaternion form along with its first three derivatives,...
Structure representing attitude in quaternion form along with its first two derivatives,...
Structure defining second-order gain parameters for vectorized 3D quantities.
Structure defining third-order gain parameters for vectorized 3D quantities.
Structure representing linear state with velocity and acceleration vectors.
Structure defining second-order pole parameters for vectorized 3D quantities.
Structure defining third-order pole parameters for vectorized 3D quantities.
Structure representing (specific) thrust reference and its derivative.
Read an attitude setpoint from the RC.
static struct FloatRates att_state
#define RC_RATE_R
#define RC_RATE_Q
#define YAW_RATE_DEADBAND_EXCEEDED(_rc)
#define RC_RATE_P
#define ROLL_RATE_DEADBAND_EXCEEDED(_rc)
#define PITCH_RATE_DEADBAND_EXCEEDED(_rc)
API to get/set the generic vehicle states.
Stabilization setpoint.
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
Transport delay filter implementation.
Transport delay filter type definitions and array operations.
static void reset_transport_delay_array(const uint8_t n, struct TransportDelay td_array[restrict n], const float initial_value[restrict n])
Reset an array of TransportDelay structures to specific initial values.
static void update_transport_delay_array(uint8_t n, struct TransportDelay td_array[restrict n], const float input_array[restrict n])
Update an array of TransportDelay structures with input values.
static void get_transport_delay_array(const uint8_t n, const struct TransportDelay td_array[restrict n], float output_array[restrict n])
Get output values from an array of TransportDelay structures.
static void init_transport_delay_array(uint8_t n, struct TransportDelay td_array[restrict n], const uint8_t delay_samples[restrict n], float initial_value[restrict n])
Initialize an array of TransportDelay structures.
int int32_t
Typedef defining 32 bit int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
void wls_alloc(struct WLS_t *WLS_p, float **B, float *u_guess, float *W_init, int imax)
active set algorithm for control allocation
Definition wls_alloc.c:119