Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
stabilization_indi.c
Go to the documentation of this file.
1/*
2 * Copyright (C) Ewoud Smeur <ewoud_smeur@msn.com>
3 * MAVLab Delft University of Technology
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
31
35
37#include "state.h"
38#include "generated/airframe.h"
41#include "modules/core/abi.h"
43#include "math/wls/wls_alloc.h"
44#include <stdio.h>
45
46// Factor that the estimated G matrix is allowed to deviate from initial one
47#define INDI_ALLOWED_G_FACTOR 2.0
48
49#ifdef STABILIZATION_INDI_FILT_CUTOFF_P
50#define STABILIZATION_INDI_FILTER_ROLL_RATE TRUE
51#else
52#define STABILIZATION_INDI_FILT_CUTOFF_P 20.0
53#endif
54
55#ifdef STABILIZATION_INDI_FILT_CUTOFF_Q
56#define STABILIZATION_INDI_FILTER_PITCH_RATE TRUE
57#else
58#define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0
59#endif
60
61#ifdef STABILIZATION_INDI_FILT_CUTOFF_R
62#define STABILIZATION_INDI_FILTER_YAW_RATE TRUE
63#else
64#define STABILIZATION_INDI_FILT_CUTOFF_R 20.0
65#endif
66
67// Default is WLS
68#ifndef STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
69#define STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE FALSE
70#endif
71
72#ifndef STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
73#define STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER FALSE
74#endif
75
76// Airspeed [m/s] at which the forward flight throttle limit is used instead of
77// the hover throttle limit.
78#ifndef STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD
79#define STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD 8.0
80#endif
81
82#ifndef STABILIZATION_INDI_USE_ADAPTIVE
83#define STABILIZATION_INDI_USE_ADAPTIVE false
84#endif
85
86#ifndef STABILIZATION_INDI_ADAPTIVE_MU
87#define STABILIZATION_INDI_ADAPTIVE_MU 0.001f
88#endif
89
90#ifndef STABILIZATION_INDI_ACT_IS_SERVO
91#define STABILIZATION_INDI_ACT_IS_SERVO {0}
92#endif
93
94#ifndef STABILIZATION_INDI_ACT_IS_THRUSTER_X
95#define STABILIZATION_INDI_ACT_IS_THRUSTER_X {0}
96#endif
97
98#ifndef STABILIZATION_INDI_ACT_IS_THRUSTER_Y
99#define STABILIZATION_INDI_ACT_IS_THRUSTER_Y {0}
100#endif
101
105#ifndef STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT
106#define STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT 99999.f
107#endif
108
109#if INDI_OUTPUTS == 5
110#ifndef STABILIZATION_INDI_G1_THRUST_X
111#error "You must define STABILIZATION_INDI_G1_THRUST_X for your number of INDI_OUTPUTS"
112#endif
113#endif
114
115#ifdef SetCommandsFromRC
116#warning SetCommandsFromRC not used: STAB_INDI writes actuators directly
117#endif
118
119#ifdef SetAutoCommandsFromRC
120#warning SetAutoCommandsFromRC not used: STAB_INDI writes actuators directly
121#endif
122
123#if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE // e.g. use WLS
124
125// Weighting of different control objectives in the cost function
126#ifndef STABILIZATION_INDI_WLS_PRIORITIES
127#if INDI_OUTPUTS == 5
128#define STABILIZATION_INDI_WLS_PRIORITIES {1000, 1000, 1, 100, 100} // roll, pitch, yaw, thrust_z, thrust_x
129#else
130#define STABILIZATION_INDI_WLS_PRIORITIES {1000, 1000, 1, 100} // default: roll, pitch, yaw, thrust_z
131#endif
132#endif
133
134// Weighting of different actuators in the cost function
135#ifndef STABILIZATION_INDI_WLS_WU
136#define STABILIZATION_INDI_WLS_WU {[0 ... INDI_NUM_ACT - 1] = 1.0}
137#endif
138
139// Preferred (neutral, least energy) actuator value
140// Assume 0 is neutral
141#ifndef STABILIZATION_INDI_ACT_PREF
142#define STABILIZATION_INDI_ACT_PREF {0.0}
143#endif
144
145#if INDI_NUM_ACT > WLS_N_U_MAX
146#error Matrix-WLS_N_U_MAX too small or not defined: define WLS_N_U_MAX >= INDI_NUM_ACT in airframe file
147#endif
148#if INDI_OUTPUTS > WLS_N_V_MAX
149#error Matrix-WLS_N_V_MAX too small or not defined: define WLS_N_V_MAX >= INDI_OUTPUTS in airframe file
150#endif
151
153 .nu = INDI_NUM_ACT,
154 .nv = INDI_OUTPUTS,
155 .gamma_sq = 10000.0,
156 .v = {0.0},
159 .u_pref = {0.0},
160 .u_min = {0.0},
161 .u_max = {0.0},
162 .PC = 0.0,
163 .SC = 0.0,
164 .iter = 0
165};
166
168
169#endif // !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE (e.g. use WLS)
170
171float indi_v[INDI_OUTPUTS];
172float *Bwls[INDI_OUTPUTS];
173
174
175static void lms_estimation(void);
176static void calc_g1_element(float dx_error, int8_t i, int8_t j, float mu_extra);
177static void calc_g2_element(float dx_error, int8_t j, float mu_extra);
178#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
179static void calc_g1g2_pseudo_inv(void);
180#endif
181static void bound_g_mat(void);
182
185 .att = {
186 STABILIZATION_INDI_REF_ERR_P,
187 STABILIZATION_INDI_REF_ERR_Q,
188 STABILIZATION_INDI_REF_ERR_R
189 },
190 .rate = {
191 STABILIZATION_INDI_REF_RATE_P,
192 STABILIZATION_INDI_REF_RATE_Q,
193 STABILIZATION_INDI_REF_RATE_R
194 },
195};
196
198
199#ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
200float act_rate_limit[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_RATE_LIMIT;
201#endif
202
203// Vector of boolean telling if an actuator is a servo
205
206// [3 x INDI_NUM_ACT] matrix representing the contribution (True/False) of an
207// actuator to the total 3D thrust vector
208#ifdef STABILIZATION_INDI_ACT_THRUST_MAT
209static bool act_thrust_mat[3][INDI_NUM_ACT] = STABILIZATION_INDI_ACT_THRUST_MAT;
210#else // backward compatibility
211static bool act_thrust_mat[3][INDI_NUM_ACT] = {
214#ifdef STABILIZATION_INDI_ACT_IS_THRUSTER_Z
215 STABILIZATION_INDI_ACT_IS_THRUSTER_Z
216#else
217 {0}
218#endif
219};
220#endif
221
222#ifdef STABILIZATION_INDI_COMMANDS
223static uint8_t act_to_commands[INDI_NUM_ACT] = STABILIZATION_INDI_COMMANDS;
224#endif
225
226#ifdef STABILIZATION_INDI_ACT_DYN
227#warning STABILIZATION_INDI_ACT_DYN is deprecated, use STABILIZATION_INDI_ACT_FREQ instead.
228#warning You now have to define the continuous time corner frequency in rad/s of the actuators.
229#warning "Use -ln(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old values."
230float act_dyn_discrete[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
231#else
232float act_first_order_cutoff[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_FREQ;
233float act_dyn_discrete[INDI_NUM_ACT]; // will be computed from freq at init
234#endif
235
237
238// variables needed for control
239float actuator_state_filt_vect[INDI_NUM_ACT];
240struct FloatRates angular_accel_ref = {0., 0., 0.};
241struct FloatRates angular_rate_ref = {0., 0., 0.};
242float angular_acceleration[3] = {0., 0., 0.};
243float actuator_state[INDI_NUM_ACT];
244float indi_u[INDI_NUM_ACT];
245struct FloatVect3 stab_thrust_filt = { 0.f, 0.f, 0.f };
246
247float q_filt = 0.0;
248float r_filt = 0.0;
249
250float stabilization_indi_filter_freq = 20.0; //Hz, for setting handler
251
252// variables needed for estimation
253float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS];
254float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS];
255float actuator_state_filt_vectd[INDI_NUM_ACT];
256float actuator_state_filt_vectdd[INDI_NUM_ACT];
257float estimation_rate_d[INDI_NUM_ACT];
258float estimation_rate_dd[INDI_NUM_ACT];
259float du_estimation[INDI_NUM_ACT];
260float ddu_estimation[INDI_NUM_ACT];
261
262// The learning rate per axis (roll, pitch, yaw, thrust)
263float mu1[INDI_OUTPUTS] = {0.00001, 0.00001, 0.000003, 0.000002};
264// The learning rate for the propeller inertia (scaled by 512 wrt mu1)
265float mu2 = 0.002;
266
267// other variables
268float act_obs[INDI_NUM_ACT];
269
270// Number of actuators used to provide thrust
272
275
276// Register actuator feedback if we rely on RPM information
277#if STABILIZATION_INDI_RPM_FEEDBACK
278#ifndef STABILIZATION_INDI_ACT_FEEDBACK_ID
279#define STABILIZATION_INDI_ACT_FEEDBACK_ID ABI_BROADCAST
280#endif
281PRINT_CONFIG_VAR(STABILIZATION_INDI_ACT_FEEDBACK_ID)
282
284static void act_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act);
285PRINT_CONFIG_MSG("STABILIZATION_INDI_RPM_FEEDBACK")
286#endif
287
288float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS];
289float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
290#ifdef STABILIZATION_INDI_G1
291float g1[INDI_OUTPUTS][INDI_NUM_ACT] = STABILIZATION_INDI_G1;
292#else // old defines TODO remove
293#if INDI_OUTPUTS == 5
294float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
295 STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW,
296 STABILIZATION_INDI_G1_THRUST, STABILIZATION_INDI_G1_THRUST_X
297 };
298
299#else
300float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
301 STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
302 };
303#endif
304#endif
305
306float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
307float g1_est[INDI_OUTPUTS][INDI_NUM_ACT];
308float g2_est[INDI_NUM_ACT];
309float g1_init[INDI_OUTPUTS][INDI_NUM_ACT];
310float g2_init[INDI_NUM_ACT];
311
312// Can be used to directly control each actuator from the control algorithm
313int16_t actuators_pprz[INDI_NUM_ACT+1];
314
320#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
321Butterworth2LowPass rates_filt_so[3];
322#else
324#endif
326
327static void init_filters(void);
328static struct FloatRates filter_rates(struct FloatRates *rates);
329static void sum_g1_g2(void);
330
331#if PERIODIC_TELEMETRY
333#if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
334static void send_wls_v_stab(struct transport_tx *trans, struct link_device *dev)
335{
336 send_wls_v("stab", &wls_stab_p, trans, dev);
337}
338static void send_wls_u_stab(struct transport_tx *trans, struct link_device *dev)
339{
340 send_wls_u("stab", &wls_stab_p, trans, dev);
341}
342#endif
343static void send_eff_mat_g_indi(struct transport_tx *trans, struct link_device *dev)
344{
345 pprz_msg_send_EFF_MAT_STAB(trans, dev, AC_ID,
346 INDI_NUM_ACT, g1g2[0],
347 INDI_NUM_ACT, g1g2[1],
348 INDI_NUM_ACT, g1g2[2],
349 INDI_NUM_ACT, g2_est);
350
351
352#if INDI_OUTPUTS <= 5
353 float zero = 0;
354#endif
355#if INDI_OUTPUTS > 3
356 pprz_msg_send_EFF_MAT_STAB_THRUST(trans, dev, AC_ID,
357#if INDI_OUTPUTS == 4
358 1, &zero,
359 1, &zero,
360 INDI_NUM_ACT, g1g2[3]
361#elif INDI_OUTPUTS == 5
362 INDI_NUM_ACT, g1g2[4],
363 1, &zero,
364 INDI_NUM_ACT, g1g2[3]
365#elif INDI_OUTPUTS == 6
366 INDI_NUM_ACT, g1g2[3],
367 INDI_NUM_ACT, g1g2[4],
368 INDI_NUM_ACT, g1g2[5]
369#endif
370 );
371#endif
372}
373
374static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
375{
376 struct Int32Quat *quat = stateGetNedToBodyQuat_i();
377 pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
382 &(quat->qi),
383 &(quat->qx),
384 &(quat->qy),
385 &(quat->qz));
386}
387
388static void send_att_full_indi(struct transport_tx *trans, struct link_device *dev)
389{
390 float zero = 0.0;
391 struct FloatRates *body_rates = stateGetBodyRates_f();
392 struct FloatEulers att, att_sp;
393#if GUIDANCE_INDI_HYBRID
395 struct FloatQuat stab_att_sp_quat_f;
396 QUAT_FLOAT_OF_BFP(stab_att_sp_quat_f, stab_att_sp_quat);
397 float_eulers_of_quat_zxy(&att_sp, &stab_att_sp_quat_f);
398#else
401#endif
402 float temp_att[3] = {att.phi, att.theta, att.psi};
403 float temp_att_ref[3] = {att_sp.phi, att_sp.theta, att_sp.psi};
404 float temp_rate[3] = {body_rates->p, body_rates->q, body_rates->r};
405 float temp_rate_ref[3] = {angular_rate_ref.p, angular_rate_ref.q, angular_rate_ref.r};
406 float temp_ang_acc_ref[3] = {angular_accel_ref.p, angular_accel_ref.q, angular_accel_ref.r};
407 pprz_msg_send_STAB_ATTITUDE(trans, dev, AC_ID,
408 1, &zero, // att des
409 3, temp_att, // att
410 3, temp_att_ref, // att ref
411 3, temp_rate, // rate
412 3, temp_rate_ref, // rate ref
413 3, angular_acceleration, // ang.acc = rate.diff
414 3, temp_ang_acc_ref, // ang.acc.ref
415 1, &zero, // jerk ref
416 1, &zero); // u
417}
418#endif
419
424{
425 // Initialize filters
426 init_filters();
427
428 int8_t i;
429// If the deprecated STABILIZATION_INDI_ACT_DYN is used, convert it to the new FREQUENCY format
430#ifdef STABILIZATION_INDI_ACT_FREQ
431 // Initialize the array of pointers to the rows of g1g2
432 for (i = 0; i < INDI_NUM_ACT; i++) {
433 act_dyn_discrete[i] = 1-exp(-act_first_order_cutoff[i]/PERIODIC_FREQUENCY);
434 }
435#endif
436
437#if STABILIZATION_INDI_RPM_FEEDBACK
438 AbiBindMsgACT_FEEDBACK(STABILIZATION_INDI_ACT_FEEDBACK_ID, &act_feedback_ev, act_feedback_cb);
439#endif
440
443 float_vect_zero(estimation_rate_d, INDI_NUM_ACT);
444 float_vect_zero(estimation_rate_dd, INDI_NUM_ACT);
446
447 //Calculate G1G2
448 sum_g1_g2();
449
450 // Do not compute if not needed
451#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
452 //Calculate G1G2_PSEUDO_INVERSE
453 calc_g1g2_pseudo_inv();
454#endif
455
456 // Initialize the array of pointers to the rows of g1g2
457 for (i = 0; i < INDI_OUTPUTS; i++) {
458 Bwls[i] = g1g2[i];
459 }
460
461 // Initialize the estimator matrices
462 float_vect_copy(g1_est[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
463 float_vect_copy(g2_est, g2, INDI_NUM_ACT);
464 // Remember the initial matrices
465 float_vect_copy(g1_init[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
466 float_vect_copy(g2_init, g2, INDI_NUM_ACT);
467
468 num_thrusters = 0; // sum of Z thrusters
469 for (i = 0; i < INDI_NUM_ACT; i++) {
470#ifndef STABILIZATION_INDI_ACT_IS_THRUSTER_Z
471 // Assume non-servos and non thrust-x/y motors, not already set to true, are delivering (Z) thrust
472 act_thrust_mat[2][i] = act_thrust_mat[2][i] || (!act_thrust_mat[0][i] && !act_thrust_mat[1][i] && !act_is_servo[i]);
473#endif
474 if (act_thrust_mat[2][i]) {
476 }
477 }
478
479#if PERIODIC_TELEMETRY
483 #if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
486 #endif
487#endif
488}
489
499{
500 float_vect_zero(du_estimation, INDI_NUM_ACT);
501 float_vect_zero(ddu_estimation, INDI_NUM_ACT);
502}
503
505{
507 float tau = 1.0 / (2.0 * M_PI * freq);
508 float sample_time = 1.0 / PERIODIC_FREQUENCY;
509#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
510 init_butterworth_2_low_pass(&rates_filt_so[0], tau, sample_time, stateGetBodyRates_f()->p);
511 init_butterworth_2_low_pass(&rates_filt_so[1], tau, sample_time, stateGetBodyRates_f()->q);
512 init_butterworth_2_low_pass(&rates_filt_so[2], tau, sample_time, stateGetBodyRates_f()->r);
513#else
517#endif
518}
519
523static void init_filters(void)
524{
525 // tau = 1/(2*pi*Fc)
526 float tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF);
527 float tau_est = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF);
528 float sample_time = 1.0 / PERIODIC_FREQUENCY;
529 // Filtering of the gyroscope
530 int8_t i;
531 for (i = 0; i < 3; i++) {
534 }
535
536 // Filtering of the actuators
537 for (i = 0; i < INDI_NUM_ACT; i++) {
538 init_butterworth_2_low_pass(&actuator_lowpass_filters[i], tau, sample_time, 0.0);
540 }
541
542 // Filtering of the accel body z
543 init_butterworth_2_low_pass(&acceleration_lowpass_filter, tau_est, sample_time, 0.0);
544
545#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
546 tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_P);
547 init_butterworth_2_low_pass(&rates_filt_so[0], tau, sample_time, 0.0);
548 tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_Q);
549 init_butterworth_2_low_pass(&rates_filt_so[1], tau, sample_time, 0.0);
550 tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_R);
551 init_butterworth_2_low_pass(&rates_filt_so[2], tau, sample_time, 0.0);
552#else
553 // Init rate filter for feedback
554 float time_constants[3] = {1.0 / (2 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_P), 1.0 / (2 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_Q), 1.0 / (2 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_R)};
555
556 init_first_order_low_pass(&rates_filt_fo[0], time_constants[0], sample_time, stateGetBodyRates_f()->p);
557 init_first_order_low_pass(&rates_filt_fo[1], time_constants[1], sample_time, stateGetBodyRates_f()->q);
558 init_first_order_low_pass(&rates_filt_fo[2], time_constants[2], sample_time, stateGetBodyRates_f()->r);
559#endif
560}
561
562static struct FloatRates filter_rates(struct FloatRates *rates)
563{
564 struct FloatRates rates_filt;
565#if STABILIZATION_INDI_FILTER_ROLL_RATE
566#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
567 rates_filt.p = update_butterworth_2_low_pass(&rates_filt_so[0], rates->p);
568#else
569 rates_filt.p = update_first_order_low_pass(&rates_filt_fo[0], rates->p);
570#endif
571#else
572 rates_filt.p = rates->p;
573#endif
574#if STABILIZATION_INDI_FILTER_PITCH_RATE
575#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
576 rates_filt.q = update_butterworth_2_low_pass(&rates_filt_so[1], rates->q);
577#else
578 rates_filt.q = update_first_order_low_pass(&rates_filt_fo[1], rates->q);
579#endif
580#else
581 rates_filt.q = rates->q;
582#endif
583#if STABILIZATION_INDI_FILTER_YAW_RATE
584#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
585 rates_filt.r = update_butterworth_2_low_pass(&rates_filt_so[2], rates->r);
586#else
587 rates_filt.r = update_first_order_low_pass(&rates_filt_fo[2], rates->r);
588#endif
589#else
590 rates_filt.r = rates->r;
591#endif
592 return rates_filt;
593}
594
603void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
604{
605
606 // Propagate actuator filters
608 float actuator_state_filt_vect_prev[INDI_NUM_ACT];
609 for (int i = 0; i < INDI_NUM_ACT; i++) {
613 actuator_state_filt_vect_prev[i] = actuator_lowpass_filters[i].o[1];
614
615 // calculate derivatives for estimation
616 float actuator_state_filt_vectd_prev = actuator_state_filt_vectd[i];
618 PERIODIC_FREQUENCY;
619 actuator_state_filt_vectdd[i] = (actuator_state_filt_vectd[i] - actuator_state_filt_vectd_prev) * PERIODIC_FREQUENCY;
620 }
621
622 // Use the last actuator state for this computation
623 float g2_times_u_act_filt = float_vect_dot_product(g2, actuator_state_filt_vect_prev, INDI_NUM_ACT)/INDI_G_SCALING;
624
625 // Predict angular acceleration u*B
626 float angular_acc_prediction_filt[INDI_OUTPUTS];
627 float_mat_vect_mul(angular_acc_prediction_filt, Bwls, actuator_state_filt_vect, INDI_OUTPUTS, INDI_NUM_ACT);
628 angular_acc_prediction_filt[2] -= g2_times_u_act_filt;
629
630 /* Propagate the filter on the gyroscopes */
631 struct FloatRates *body_rates = stateGetBodyRates_f();
632 float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r};
633
634 // Get the acceleration in body axes
635 struct Int32Vect3 *body_accel_i;
636 body_accel_i = stateGetAccelBody_i();
637 ACCELS_FLOAT_OF_BFP(body_accel_f, *body_accel_i);
638
639 int8_t i;
640 for (i = 0; i < 3; i++) {
643
644 // Calculate the angular acceleration via finite difference
646 - measurement_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
647
648 // Calculate derivatives for estimation
649 float estimation_rate_d_prev = estimation_rate_d[i];
651 PERIODIC_FREQUENCY;
652 estimation_rate_dd[i] = (estimation_rate_d[i] - estimation_rate_d_prev) * PERIODIC_FREQUENCY;
653 }
654
655 // subtract u*B from angular acceleration
656 float angular_acc_disturbance_estimate[INDI_OUTPUTS];
657 float_vect_diff(angular_acc_disturbance_estimate, angular_acceleration, angular_acc_prediction_filt, 3);
658
659 if (in_flight) {
660 // Limit the estimated disturbance in yaw for drones that are stable in sideslip
661 BoundAbs(angular_acc_disturbance_estimate[2], stablization_indi_yaw_dist_limit);
662 } else {
663 // Not in flight, so don't estimate disturbance
664 float_vect_zero(angular_acc_disturbance_estimate, INDI_OUTPUTS);
665 }
666
667 // The rates used for feedback are by default the measured rates.
668 // If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.
669 // Note that due to the delay, the controller may need relaxed gains.
670 struct FloatRates rates_filt = filter_rates(body_rates);
671
672 // calculate the virtual control (reference acceleration) based on a controller
673 struct FloatRates rates_sp = stab_sp_to_rates_f(sp);
675
676 // Compute estimated thrust
678 for (i = 0; i < INDI_NUM_ACT; i++) {
679#if INDI_OUTPUTS == 4
681#endif
682#if INDI_OUTPUTS == 5 // FIXME change order of Z and X, or better detect that automatically ?
685#endif
686#if INDI_OUTPUTS == 6
690#endif
691 }
692
693 // compute virtual thrust
694 struct FloatVect3 v_thrust = { 0.f, 0.f, 0.f };
695 if (thrust->type == THRUST_INCR_SP) {
696 v_thrust.x = th_sp_to_incr_f(thrust, 0, THRUST_AXIS_X);
697 v_thrust.y = th_sp_to_incr_f(thrust, 0, THRUST_AXIS_Y);
698 v_thrust.z = th_sp_to_incr_f(thrust, 0, THRUST_AXIS_Z);
699 // Add the current estimated thrust to the increment
700 VECT3_ADD(v_thrust, stab_thrust_filt);
701 }
702 else {
703 // build incremental thrust
704 struct FloatVect3 th_cmd;
705 th_cmd.x = (float)th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_X);
706 th_cmd.y = (float)th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Y);
707 th_cmd.z = (float)th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z);
708#if (INDI_OUTPUTS == 5) && (defined RADIO_CONTROL_THRUST_X) && (defined COMMAND_THRUST_X)
709 // TODO set X thrust from RC in the thrust input setpoint
710 cmd[COMMAND_THRUST_X] = radio_control.values[RADIO_CONTROL_THRUST_X];
711 th_cmd.x = (float)cmd[COMMAND_THRUST_X];
712#endif
713 // compute v_thrust based on the efficiency matrix
714 // this will work if internal forces are not equilibrated
715 // which is usually the case for Z thrust
716 for (i = 0; i < INDI_NUM_ACT; i++) {
717#if INDI_OUTPUTS == 4
718 v_thrust.z += th_cmd.z * Bwls[3][i] * (int32_t) act_thrust_mat[2][i];
719#endif
720#if INDI_OUTPUTS == 5 // FIXME change order of Z and X, or better detect that automatically ?
721 v_thrust.x += th_cmd.x * Bwls[4][i] * (int32_t) act_thrust_mat[0][i];
722 v_thrust.z += th_cmd.z * Bwls[3][i] * (int32_t) act_thrust_mat[2][i];
723#endif
724#if INDI_OUTPUTS == 6
725 v_thrust.x += th_cmd.x * Bwls[3][i] * (int32_t) act_thrust_mat[0][i];
726 v_thrust.y += th_cmd.y * Bwls[4][i] * (int32_t) act_thrust_mat[1][i];
727 v_thrust.z += th_cmd.z * Bwls[5][i] * (int32_t) act_thrust_mat[2][i];
728#endif
729 }
730 // overwritte v_thrust if needed
731 // this can be the case for multirotor with fixed tilted motors
732#ifdef STABILIZATION_INDI_V_THRUST_X
733 v_thrust.x = th_cmd.x * STABILIZATION_INDI_V_THRUST_X / MAX_PPRZ;
734#endif
735#ifdef STABILIZATION_INDI_V_THRUST_Y
736 v_thrust.y = th_cmd.y * STABILIZATION_INDI_V_THRUST_Y / MAX_PPRZ;
737#endif
738#ifdef STABILIZATION_INDI_V_THRUST_Z
739 v_thrust.z = th_cmd.z * STABILIZATION_INDI_V_THRUST_Z / MAX_PPRZ;
740#endif
741 }
742
743 // This term compensates for the spinup torque in the yaw axis
744 float g2_times_u = float_vect_dot_product(g2, indi_u, INDI_NUM_ACT)/INDI_G_SCALING;
745
746 if (in_flight) {
747 // Limit the estimated disturbance in yaw for drones that are stable in sideslip
748 BoundAbs(angular_acc_disturbance_estimate[2], stablization_indi_yaw_dist_limit);
749 } else {
750 // Not in flight, so don't estimate disturbance
751 float_vect_zero(angular_acc_disturbance_estimate, INDI_OUTPUTS);
752 }
753
754 // The control objective in array format
755 indi_v[0] = (angular_accel_ref.p - angular_acc_disturbance_estimate[0]);
756 indi_v[1] = (angular_accel_ref.q - angular_acc_disturbance_estimate[1]);
757 indi_v[2] = (angular_accel_ref.r - angular_acc_disturbance_estimate[2]) + g2_times_u;
758#if INDI_OUTPUTS == 4
759 indi_v[3] = v_thrust.z;
760#endif
761#if INDI_OUTPUTS == 5 // FIXME order of Z/X is reversed
762 indi_v[3] = v_thrust.z;
763 indi_v[4] = v_thrust.x;
764#endif
765#if INDI_OUTPUTS == 6
766 indi_v[3] = v_thrust.x;
767 indi_v[4] = v_thrust.y;
768 indi_v[5] = v_thrust.z;
769#endif
770
771#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
772 // Calculate the increment for each actuator
773 for (i = 0; i < INDI_NUM_ACT; i++) {
774 indi_u[i] = (g1g2_pseudo_inv[i][0] * indi_v[0])
775 + (g1g2_pseudo_inv[i][1] * indi_v[1])
776 + (g1g2_pseudo_inv[i][2] * indi_v[2])
777 + (g1g2_pseudo_inv[i][3] * indi_v[3]);
778 }
779#else
781
782 // WLS Control Allocator
783 for (i = 0; i < INDI_OUTPUTS; i++) {
784 wls_stab_p.v[i] = indi_v[i];
785 }
786 wls_alloc(&wls_stab_p, Bwls, 0, 0, 10);
787 for (i = 0; i < INDI_NUM_ACT; i++) {
788 indi_u[i] = wls_stab_p.u[i];
789 }
790#endif
791
792 // Use online effectiveness estimation only when flying
793 if (in_flight && indi_use_adaptive) {
795 }
796
797 // update actuators and thrust commands
798 cmd[COMMAND_THRUST] = 0;
799 for (i = 0; i < INDI_NUM_ACT; i++) {
800 // bound actuator command
801 if (act_is_servo[i]) {
802 BoundAbs(indi_u[i], MAX_PPRZ);
803 } else {
805 Bound(indi_u[i], 0, MAX_PPRZ);
806 } else {
807 indi_u[i] = -MAX_PPRZ;
808 }
809 }
810 }
811
812 /*Commit the actuator command*/
814}
815
824void stabilization_indi_attitude_run(bool in_flight, struct StabilizationSetpoint *att_sp, struct ThrustSetpoint *thrust, int32_t *cmd)
825{
826 stab_att_sp_euler = stab_sp_to_eulers_i(att_sp); // stab_att_sp_euler.psi still used in ref..
827 stab_att_sp_quat = stab_sp_to_quat_i(att_sp); // quat attitude setpoint
828
829 struct FloatQuat quat_att = *stateGetNedToBodyQuat_f();
830 struct FloatQuat quat_sp = stab_sp_to_quat_f(att_sp);
831 struct FloatRates rates_ff = stab_sp_to_rates_f(att_sp);
832
833 // calculate the virtual control (reference acceleration) based on a controller
834 angular_rate_ref = stabilization_indi_attitude_controller(quat_att, quat_sp, rates_ff);
835
836 /* compute the INDI command */
838 stabilization_indi_rate_run(in_flight, &sp, thrust, cmd);
839}
840
846struct FloatRates WEAK stabilization_indi_attitude_controller(struct FloatQuat att, struct FloatQuat att_sp, struct FloatRates rates_ff)
847{
848 /* attitude error */
849 struct FloatQuat att_err;
850 float_quat_inv_comp_norm_shortest(&att_err, &att, &att_sp);
851
852 struct FloatVect3 att_fb;
853#if TILT_TWIST_CTRL
854 struct FloatQuat tilt;
855 struct FloatQuat twist;
856 float_quat_tilt_twist(&tilt, &twist, &att_err);
857 att_fb.x = tilt.qx;
858 att_fb.y = tilt.qy;
859 att_fb.z = twist.qz;
860#else
861 att_fb.x = att_err.qx;
862 att_fb.y = att_err.qy;
863 att_fb.z = att_err.qz;
864#endif
865
866 struct FloatRates rates_ref;
867 rates_ref.p = indi_gains.att.p * att_fb.x / indi_gains.rate.p;
868 rates_ref.q = indi_gains.att.q * att_fb.y / indi_gains.rate.q;
869 rates_ref.r = indi_gains.att.r * att_fb.z / indi_gains.rate.r;
870 // add feed-forward term
871 RATES_ADD(rates_ref, rates_ff);
872 return rates_ref;
873}
874
881{
882 struct FloatRates accel_ref;
883 accel_ref.p = (sp.p - rates.p) * indi_gains.rate.p;
884 accel_ref.q = (sp.q - rates.q) * indi_gains.rate.q;
885 accel_ref.r = (sp.r - rates.r) * indi_gains.rate.r;
886 return accel_ref;
887}
888
889
893#if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
895{
896 // Calculate the min and max increments
897 for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
898 wls_stab_p.u_min[i] = -MAX_PPRZ * act_is_servo[i];
899 wls_stab_p.u_max[i] = MAX_PPRZ;
900 wls_stab_p.u_pref[i] = act_pref[i];
901
902#ifdef GUIDANCE_INDI_MIN_THROTTLE
903 float airspeed = stateGetAirspeed_f();
904 //limit minimum thrust ap can give
905 if (!act_is_servo[i]) {
908 wls_stab_p.u_min[i] = GUIDANCE_INDI_MIN_THROTTLE;
909 } else {
910 wls_stab_p.u_min[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD;
911 }
912 }
913 }
914#endif
915 }
916}
917#endif
918
926{
927#if STABILIZATION_INDI_RPM_FEEDBACK
928 float_vect_copy(actuator_state, act_obs, INDI_NUM_ACT);
929#else
930 //actuator dynamics
931 int8_t i;
932 float UNUSED prev_actuator_state;
933 for (i = 0; i < INDI_NUM_ACT; i++) {
934 prev_actuator_state = actuator_state[i];
935
937 + act_dyn_discrete[i] * (indi_u[i] - actuator_state[i]);
938
939#ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
940 if ((actuator_state[i] - prev_actuator_state) > act_rate_limit[i]) {
941 actuator_state[i] = prev_actuator_state + act_rate_limit[i];
942 } else if ((actuator_state[i] - prev_actuator_state) < -act_rate_limit[i]) {
943 actuator_state[i] = prev_actuator_state - act_rate_limit[i];
944 }
945#endif
946 }
947
948#endif
949}
950
951#ifdef STABILIZATION_INDI_COMMANDS
957void set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flight __attribute__((unused)), bool motors_on)
958{
959 for (int i = 0; i < INDI_NUM_ACT; i++) {
960 bool is_motor = (act_thrust_mat[0][i] || act_thrust_mat[1][i] || act_thrust_mat[2][i]) && !act_is_servo[i];
961 if (is_motor && !motors_on) {
962 cmd_out[act_to_commands[i]] = MIN_PPRZ;
963 } else {
964 cmd_out[act_to_commands[i]] = cmd_in[act_to_commands[i]];
965 }
966 }
967 // for GCS display
968 if (!motors_on) {
969 cmd_in[COMMAND_THRUST] = 0;
970 }
971 cmd_out[COMMAND_THRUST] = cmd_in[COMMAND_THRUST];
972}
973#endif
974
985void calc_g1_element(float ddx_error, int8_t i, int8_t j, float mu)
986{
987 g1_est[i][j] = g1_est[i][j] - du_estimation[j] * mu * ddx_error;
988}
989
999void calc_g2_element(float ddx_error, int8_t j, float mu)
1000{
1001 g2_est[j] = g2_est[j] - ddu_estimation[j] * mu * ddx_error;
1002}
1003
1010{
1011
1012 // Get the acceleration in body axes
1013 struct Int32Vect3 *body_accel_i;
1014 body_accel_i = stateGetAccelBody_i();
1015 ACCELS_FLOAT_OF_BFP(body_accel_f, *body_accel_i);
1016
1017 // Filter the acceleration in z axis
1019
1020 // Calculate the derivative of the acceleration via finite difference
1021 float indi_accel_d = (acceleration_lowpass_filter.o[0]
1022 - acceleration_lowpass_filter.o[1]) * PERIODIC_FREQUENCY;
1023
1024 // scale the inputs to avoid numerical errors
1027
1028 float ddx_estimation[INDI_OUTPUTS] = {estimation_rate_dd[0], estimation_rate_dd[1], estimation_rate_dd[2], indi_accel_d};
1029
1030 //Estimation of G
1031 // TODO: only estimate when du_norm2 is large enough (enough input)
1032 /*float du_norm2 = du_estimation[0]*du_estimation[0] + du_estimation[1]*du_estimation[1] +du_estimation[2]*du_estimation[2] + du_estimation[3]*du_estimation[3];*/
1033 int8_t i;
1034 for (i = 0; i < INDI_OUTPUTS; i++) {
1035 // Calculate the error between prediction and measurement
1036 float ddx_error = - ddx_estimation[i];
1037 int8_t j;
1038 for (j = 0; j < INDI_NUM_ACT; j++) {
1039 ddx_error += g1_est[i][j] * du_estimation[j];
1040 if (i == 2) {
1041 // Changing the momentum of the rotors gives a counter torque
1042 ddx_error += g2_est[j] * ddu_estimation[j];
1043 }
1044 }
1045
1046 // when doing the yaw axis, also use G2
1047 if (i == 2) {
1048 for (j = 0; j < INDI_NUM_ACT; j++) {
1049 calc_g2_element(ddx_error, j, mu2);
1050 }
1051 } else if (i == 3) {
1052 // If the acceleration change is very large (rough landing), don't adapt
1053 if (fabs(indi_accel_d) > 60.0) {
1054 ddx_error = 0.0;
1055 }
1056 }
1057
1058 // Calculate the row of the G1 matrix corresponding to this axis
1059 for (j = 0; j < INDI_NUM_ACT; j++) {
1060 calc_g1_element(ddx_error, i, j, mu1[i]);
1061 }
1062 }
1063
1064 bound_g_mat();
1065
1066 // Save the calculated matrix to G1 and G2
1067 // until thrust is included, first part of the array
1068 float_vect_copy(g1[0], g1_est[0], INDI_OUTPUTS * INDI_NUM_ACT);
1069 float_vect_copy(g2, g2_est, INDI_NUM_ACT);
1070
1071 // Calculate sum of G1 and G2 for Bwls
1072 sum_g1_g2();
1073
1074#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
1075 // Calculate the inverse of (G1+G2)
1076 calc_g1g2_pseudo_inv();
1077#endif
1078}
1079
1084void sum_g1_g2(void)
1085{
1086 int8_t i;
1087 int8_t j;
1088 for (i = 0; i < INDI_OUTPUTS; i++) {
1089 for (j = 0; j < INDI_NUM_ACT; j++) {
1090 if (i != 2) {
1091 g1g2[i][j] = g1[i][j] / INDI_G_SCALING;
1092 } else {
1093 g1g2[i][j] = (g1[i][j] + g2[j]) / INDI_G_SCALING;
1094 }
1095 }
1096 }
1097}
1098
1099#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
1104void calc_g1g2_pseudo_inv(void)
1105{
1106 //G1G2*transpose(G1G2)
1107 //calculate matrix multiplication of its transpose INDI_OUTPUTSxnum_act x num_actxINDI_OUTPUTS
1108 float element = 0;
1109 int8_t row;
1110 int8_t col;
1111 int8_t i;
1112 for (row = 0; row < INDI_OUTPUTS; row++) {
1113 for (col = 0; col < INDI_OUTPUTS; col++) {
1114 element = 0;
1115 for (i = 0; i < INDI_NUM_ACT; i++) {
1116 element = element + g1g2[row][i] * g1g2[col][i];
1117 }
1118 g1g2_trans_mult[row][col] = element;
1119 }
1120 }
1121
1122 //there are numerical errors if the scaling is not right.
1123 float_vect_scale(g1g2_trans_mult[0], 1000.0, INDI_OUTPUTS * INDI_OUTPUTS);
1124
1125 //inverse of 4x4 matrix
1127
1128 //scale back
1129 float_vect_scale(g1g2inv[0], 1000.0, INDI_OUTPUTS * INDI_OUTPUTS);
1130
1131 //G1G2'*G1G2inv
1132 //calculate matrix multiplication INDI_NUM_ACTxINDI_OUTPUTS x INDI_OUTPUTSxINDI_OUTPUTS
1133 for (row = 0; row < INDI_NUM_ACT; row++) {
1134 for (col = 0; col < INDI_OUTPUTS; col++) {
1135 element = 0;
1136 for (i = 0; i < INDI_OUTPUTS; i++) {
1137 element = element + g1g2[i][row] * g1g2inv[col][i];
1138 }
1139 g1g2_pseudo_inv[row][col] = element;
1140 }
1141 }
1142}
1143#endif
1144
1145#if STABILIZATION_INDI_RPM_FEEDBACK
1146static void act_feedback_cb(uint8_t sender_id UNUSED, struct act_feedback_t *feedback, uint8_t num_act)
1147{
1148 int8_t i;
1149 for (i = 0; i < num_act; i++) {
1150 // Sanity check that index is valid
1151 if (feedback[i].idx < INDI_NUM_ACT && feedback[i].set.rpm) {
1152 int8_t idx = feedback[i].idx;
1153 act_obs[idx] = (feedback[i].rpm - get_servo_min(idx));
1154 act_obs[idx] *= (MAX_PPRZ / (float)(get_servo_max(idx) - get_servo_min(idx)));
1155 Bound(act_obs[idx], 0, MAX_PPRZ);
1156 }
1157 }
1158}
1159#endif
1160
1161static void bound_g_mat(void)
1162{
1163 int8_t i;
1164 int8_t j;
1165 for (j = 0; j < INDI_NUM_ACT; j++) {
1166 float max_limit;
1167 float min_limit;
1168
1169 // Limit the values of the estimated G1 matrix
1170 for (i = 0; i < INDI_OUTPUTS; i++) {
1171 if (g1_init[i][j] > 0.0) {
1172 max_limit = g1_init[i][j] * INDI_ALLOWED_G_FACTOR;
1173 min_limit = g1_init[i][j] / INDI_ALLOWED_G_FACTOR;
1174 } else {
1175 max_limit = g1_init[i][j] / INDI_ALLOWED_G_FACTOR;
1176 min_limit = g1_init[i][j] * INDI_ALLOWED_G_FACTOR;
1177 }
1178
1179 if (g1_est[i][j] > max_limit) {
1180 g1_est[i][j] = max_limit;
1181 }
1182 if (g1_est[i][j] < min_limit) {
1183 g1_est[i][j] = min_limit;
1184 }
1185 }
1186
1187 // Do the same for the G2 matrix
1188 if (g2_init[j] > 0.0) {
1189 max_limit = g2_init[j] * INDI_ALLOWED_G_FACTOR;
1190 min_limit = g2_init[j] / INDI_ALLOWED_G_FACTOR;
1191 } else {
1192 max_limit = g2_init[j] / INDI_ALLOWED_G_FACTOR;
1193 min_limit = g2_init[j] * INDI_ALLOWED_G_FACTOR;
1194 }
1195
1196 if (g2_est[j] > max_limit) {
1197 g2_est[j] = max_limit;
1198 }
1199 if (g2_est[j] < min_limit) {
1200 g2_est[j] = min_limit;
1201 }
1202 }
1203}
1204
1206 for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
1207 actuators_pprz[i] = (int16_t) indi_u[i];
1208
1209#ifdef STABILIZATION_INDI_COMMANDS
1210 // copy to actuators to specified commands
1211 cmd[act_to_commands[i]] = actuators_pprz[i];
1212#endif
1213 }
1214
1215 //update thrust command such that the current is correctly estimated
1216 cmd[COMMAND_THRUST] = 0;
1217 for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
1218 cmd[COMMAND_THRUST] += actuator_state[i] * (int32_t) act_thrust_mat[2][i];
1219 }
1220 cmd[COMMAND_THRUST] /= num_thrusters;
1221}
1222
1223
1225 return actuator_state;
1226}
1227
1229 return act_dyn_discrete;
1230}
1231
1233 return indi_u;
1234}
1235
1239
1243
1244
1246#ifdef STABILIZATION_INDI_COMMANDS
1247 return act_to_commands;
1248#else
1249 return NULL;
1250#endif
1251}
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
bool autopilot_get_motors_on(void)
get motors status
Definition autopilot.c:303
#define UNUSED(x)
static struct uart_periph * dev
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
float psi
in radians
static void float_vect_zero(float *a, const int n)
a = 0
void float_eulers_of_quat_zxy(struct FloatEulers *e, const struct FloatQuat *q)
euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
static void float_vect_smul(float *o, const float *a, const float s, const int n)
o = a * s
#define FLOAT_VECT3_ZERO(_v)
static void float_vect_scale(float *a, const float s, const int n)
a *= s
static void float_mat_vect_mul(float *o, float **a, const float *b, const int m, const int n)
o = a * b
static float float_vect_dot_product(const float *a, const float *b, const int n)
a.b
static void float_vect_copy(float *a, const float *b, const int n)
a = b
static void float_vect_diff(float *o, const float *a, const float *b, const int n)
o = a - b
bool float_mat_inv_4d(float invOut[4][4], const float mat_in[4][4])
4x4 Matrix inverse
void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, const struct FloatQuat *quat)
Tilt twist decomposition of quaternion.
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.
euler angles
Roation quaternion.
angular rates
#define RATES_ADD(_a, _b)
#define QUAT_FLOAT_OF_BFP(_qf, _qi)
#define VECT3_ADD(_a, _b)
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
euler angles
Rotation quaternion.
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition state.h:1295
static struct FloatEulers * stateGetNedToBodyEulersZxy_f(void)
Get vehicle body attitude ZXY euler angles (float).
Definition state.h:1331
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition state.h:1313
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition state.h:1392
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
Definition state.h:1105
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition state.h:1615
static float p[2][2]
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
Simple first order low pass filter with bilinear transform.
struct SecondOrderLowPass Butterworth2LowPass
Second order Butterworth low pass filter.
static void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, const float sample_time, float value)
Init first order low pass filter.
static float update_first_order_low_pass(struct FirstOrderLowPass *filter, const float value)
Update first order low pass filter state with a new value.
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, const float tau, const float sample_time, const float value)
Init a second order Butterworth filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, const float value)
Update second order Butterworth low pass filter state with a new value.
First order low pass filter structure.
Hardware independent API for actuators (servos, motor controllers).
int32_t rpm
RPM.
Definition actuators.h:54
uint8_t idx
General index of the actuators (generated in airframe.h).
Definition actuators.h:45
static uint32_t idx
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
int16_t pprz_t
Definition paparazzi.h:6
#define MAX_PPRZ
Definition paparazzi.h:8
#define MIN_PPRZ
Definition paparazzi.h:9
static abi_event act_feedback_ev
Paparazzi floating point algebra.
struct RadioControl radio_control
Generic interface for radio control modules.
void WEAK set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flight, bool motors_on)
Display descent speed in failsafe mode if needed.
struct HorizontalGuidance guidance_h
Definition guidance_h.c:45
#define GUIDANCE_H_MODE_NAV
Definition guidance_h.h:58
#define GUIDANCE_H_MODE_HOVER
Definition guidance_h.h:57
struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp)
float th_sp_to_incr_f(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp)
struct Int32Eulers stab_sp_to_eulers_i(struct StabilizationSetpoint *sp)
struct StabilizationSetpoint stab_sp_from_rates_f(struct FloatRates *rates)
struct FloatQuat stab_sp_to_quat_f(struct StabilizationSetpoint *sp)
int32_t th_sp_to_thrust_i(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
#define THRUST_AXIS_X
#define THRUST_AXIS_Z
#define THRUST_AXIS_Y
float actuator_state[ANDI_NUM_ACT]
struct WLS_t wls_stab_p
static struct FloatEulers stab_att_sp_euler
Butterworth2LowPass_int measurement_lowpass_filters[INDI_DOF]
Butterworth2LowPass_int actuator_lowpass_filters[INDI_DOF]
static struct Int32Quat stab_att_sp_quat
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
Quaternion transformation functions.
Read an attitude setpoint from the RC.
static void calc_g1_element(float dx_error, int8_t i, int8_t j, float mu_extra)
#define STABILIZATION_INDI_WLS_WU
Butterworth2LowPass acceleration_lowpass_filter
static void send_eff_mat_g_indi(struct transport_tx *trans, struct link_device *dev)
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
void stabilization_indi_enter(void)
Function that resets important values upon engaging INDI.
float act_pref[INDI_NUM_ACT]
struct FloatRates WEAK stabilization_indi_attitude_controller(struct FloatQuat att, struct FloatQuat att_sp, struct FloatRates rates_ff)
Default PD angular rate controller.
#define STABILIZATION_INDI_ACT_PREF
float * Bwls[INDI_OUTPUTS]
int32_t stabilization_att_indi_cmd[COMMANDS_NB]
struct Indi_gains indi_gains
void WEAK stabilization_indi_set_wls_settings(void)
Function that sets the u_min, u_max and u_pref if function not elsewhere defined.
#define STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD
float * stabilization_indi_get_act_state(void)
static bool act_thrust_mat[3][INDI_NUM_ACT]
bool act_is_servo[INDI_NUM_ACT]
static struct FirstOrderLowPass rates_filt_fo[3]
float angular_acceleration[3]
static void send_wls_v_stab(struct transport_tx *trans, struct link_device *dev)
int32_t num_thrusters
float actuator_state_filt_vectd[INDI_NUM_ACT]
#define STABILIZATION_INDI_ADAPTIVE_MU
float r_filt
#define STABILIZATION_INDI_FILT_CUTOFF_P
static void bound_g_mat(void)
float mu1[INDI_OUTPUTS]
#define STABILIZATION_INDI_FILT_CUTOFF_Q
#define STABILIZATION_INDI_ACT_IS_THRUSTER_X
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]
static void calc_g2_element(float dx_error, int8_t j, float mu_extra)
float q_filt
static void send_wls_u_stab(struct transport_tx *trans, struct link_device *dev)
bool * stabilization_indi_get_act_is_thruster_z(void)
float estimation_rate_d[INDI_NUM_ACT]
static void init_filters(void)
Function that resets the filters to zeros.
#define STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT
Limit the maximum specific moment that can be compensated (units rad/s^2).
float du_estimation[INDI_NUM_ACT]
float * stabilization_indi_get_act_dyn(void)
float actuator_state_filt_vectdd[INDI_NUM_ACT]
void WEAK stabilization_indi_commit_actuator_cmd(int32_t *cmd)
void stabilization_indi_update_filt_freq(float freq)
static struct FloatRates filter_rates(struct FloatRates *rates)
float g2[INDI_NUM_ACT]
struct FloatVect3 body_accel_f
float ddu_estimation[INDI_NUM_ACT]
float estimation_rate_dd[INDI_NUM_ACT]
float mu2
float act_obs[INDI_NUM_ACT]
float g1[INDI_OUTPUTS][INDI_NUM_ACT]
void stabilization_indi_init(void)
Function that initializes important values upon engaging INDI.
struct FloatRates angular_rate_ref
struct FloatVect3 stab_thrust_filt
#define STABILIZATION_INDI_WLS_PRIORITIES
float stablization_indi_yaw_dist_limit
float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS]
void stabilization_indi_attitude_run(bool in_flight, struct StabilizationSetpoint *att_sp, struct ThrustSetpoint *thrust, int32_t *cmd)
float actuator_state_filt_vect[INDI_NUM_ACT]
#define STABILIZATION_INDI_USE_ADAPTIVE
#define STABILIZATION_INDI_ACT_IS_SERVO
void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
float act_first_order_cutoff[INDI_NUM_ACT]
float act_dyn_discrete[INDI_NUM_ACT]
struct FloatRates angular_accel_ref
float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS]
#define STABILIZATION_INDI_FILT_CUTOFF_R
#define STABILIZATION_INDI_ACT_IS_THRUSTER_Y
bool indi_use_adaptive
float g2_init[INDI_NUM_ACT]
Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT]
static void sum_g1_g2(void)
Function that sums g1 and g2 to obtain the g1g2 matrix It also undoes the scaling that was done to ma...
float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS]
float g2_est[INDI_NUM_ACT]
float g1_init[INDI_OUTPUTS][INDI_NUM_ACT]
struct FloatRates WEAK stabilization_indi_rate_controller(struct FloatRates rates, struct FloatRates sp)
Default PD angular acceleration controller.
uint8_t * stabilization_indi_get_act_to_commands(void)
static void lms_estimation(void)
Function that estimates the control effectiveness of each actuator online.
static void send_att_full_indi(struct transport_tx *trans, struct link_device *dev)
#define INDI_ALLOWED_G_FACTOR
int32_t stabilization_indi_get_num_thrusters(void)
float g1_est[INDI_OUTPUTS][INDI_NUM_ACT]
float indi_v[INDI_OUTPUTS]
void WEAK stabilization_indi_get_actuator_state(void)
Function that tries to get actuator feedback.
float stabilization_indi_filter_freq
int16_t actuators_pprz[INDI_NUM_ACT+1]
PPRZ command to each actuator Can be used to directly control actuators from the control algorithm if...
float indi_u[INDI_NUM_ACT]
float * stabilization_indi_get_indi_u(void)
Butterworth2LowPass estimation_output_lowpass_filters[3]
#define INDI_G_SCALING
#define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF
#define STABILIZATION_INDI_FILT_CUTOFF
API to get/set the generic vehicle states.
Stabilization setpoint.
union StabilizationSetpoint::@354356152067340015216250042204214023005305374154 sp
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
enum ThrustSetpoint::@227227372251370165225315201346301141210232313211 type
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
static void act_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act)
RPM callback for RPM based control throttle curves.
int int32_t
Typedef defining 32 bit int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.
void send_wls_v(char *name, struct WLS_t *WLS_p, struct transport_tx *trans, struct link_device *dev)
Definition wls_alloc.c:61
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
void send_wls_u(char *name, struct WLS_t *WLS_p, struct transport_tx *trans, struct link_device *dev)
Definition wls_alloc.c:71