Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
36 
38 #include "state.h"
39 #include "generated/airframe.h"
42 #include "modules/core/abi.h"
44 #include "math/wls/wls_alloc.h"
45 #include <stdio.h>
46 
47 // Factor that the estimated G matrix is allowed to deviate from initial one
48 #define INDI_ALLOWED_G_FACTOR 2.0
49 
50 #ifdef STABILIZATION_INDI_FILT_CUTOFF_P
51 #define STABILIZATION_INDI_FILTER_ROLL_RATE TRUE
52 #else
53 #define STABILIZATION_INDI_FILT_CUTOFF_P 20.0
54 #endif
55 
56 #ifdef STABILIZATION_INDI_FILT_CUTOFF_Q
57 #define STABILIZATION_INDI_FILTER_PITCH_RATE TRUE
58 #else
59 #define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0
60 #endif
61 
62 #ifdef STABILIZATION_INDI_FILT_CUTOFF_R
63 #define STABILIZATION_INDI_FILTER_YAW_RATE TRUE
64 #else
65 #define STABILIZATION_INDI_FILT_CUTOFF_R 20.0
66 #endif
67 
68 // Default is WLS
69 #ifndef STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
70 #define STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE FALSE
71 #endif
72 
73 #ifndef STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
74 #define STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER FALSE
75 #endif
76 
77 // Airspeed [m/s] at which the forward flight throttle limit is used instead of
78 // the hover throttle limit.
79 #ifndef STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD
80 #define STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD 8.0
81 #endif
82 
83 #if INDI_OUTPUTS > 4
84 #ifndef STABILIZATION_INDI_G1_THRUST_X
85 #error "You must define STABILIZATION_INDI_G1_THRUST_X for your number of INDI_OUTPUTS"
86 #endif
87 #endif
88 
89 #ifdef SetCommandsFromRC
90 #warning SetCommandsFromRC not used: STAB_INDI writes actuators directly
91 #endif
92 
93 #ifdef SetAutoCommandsFromRC
94 #warning SetAutoCommandsFromRC not used: STAB_INDI writes actuators directly
95 #endif
96 
97 
98 #if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
99 #if INDI_NUM_ACT > WLS_N_U
100 #error Matrix-WLS_N_U too small or not defined: define WLS_N_U >= INDI_NUM_ACT in airframe file
101 #endif
102 #if INDI_OUTPUTS > WLS_N_V
103 #error Matrix-WLS_N_V too small or not defined: define WLS_N_U >= INDI_OUTPUTS in airframe file
104 #endif
105 #endif
106 
107 float u_min_stab_indi[INDI_NUM_ACT];
108 float u_max_stab_indi[INDI_NUM_ACT];
109 float u_pref_stab_indi[INDI_NUM_ACT];
110 float indi_v[INDI_OUTPUTS];
111 float *Bwls[INDI_OUTPUTS];
112 int num_iter = 0;
113 
114 static void lms_estimation(void);
115 static void get_actuator_state(void);
116 static void calc_g1_element(float dx_error, int8_t i, int8_t j, float mu_extra);
117 static void calc_g2_element(float dx_error, int8_t j, float mu_extra);
118 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
119 static void calc_g1g2_pseudo_inv(void);
120 #endif
121 static void bound_g_mat(void);
122 
124 struct Indi_gains indi_gains = {
125  .att = {
126  STABILIZATION_INDI_REF_ERR_P,
127  STABILIZATION_INDI_REF_ERR_Q,
128  STABILIZATION_INDI_REF_ERR_R
129  },
130  .rate = {
131  STABILIZATION_INDI_REF_RATE_P,
132  STABILIZATION_INDI_REF_RATE_Q,
133  STABILIZATION_INDI_REF_RATE_R
134  },
135 };
136 
137 #if STABILIZATION_INDI_USE_ADAPTIVE
138 bool indi_use_adaptive = true;
139 #else
140 bool indi_use_adaptive = false;
141 #endif
142 
143 #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
144 float act_rate_limit[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_RATE_LIMIT;
145 #endif
146 
147 #ifdef STABILIZATION_INDI_ACT_IS_SERVO
148 bool act_is_servo[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO;
149 #else
150 bool act_is_servo[INDI_NUM_ACT] = {0};
151 #endif
152 
153 #ifdef STABILIZATION_INDI_ACT_IS_THRUSTER_X
154 bool act_is_thruster_x[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_THRUSTER_X;
155 #else
156 bool act_is_thruster_x[INDI_NUM_ACT] = {0};
157 #endif
158 
159 bool act_is_thruster_z[INDI_NUM_ACT];
160 
161 #ifdef STABILIZATION_INDI_ACT_PREF
162 // Preferred (neutral, least energy) actuator value
163 float act_pref[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_PREF;
164 #else
165 // Assume 0 is neutral
166 float act_pref[INDI_NUM_ACT] = {0.0};
167 #endif
168 
169 #ifdef STABILIZATION_INDI_ACT_DYN
170 #warning STABILIZATION_INDI_ACT_DYN is deprecated, use STABILIZATION_INDI_ACT_FREQ instead.
171 #warning You now have to define the continuous time corner frequency in rad/s of the actuators.
172 #warning "Use -ln(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old values."
173 float act_dyn_discrete[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
174 #else
175 float act_first_order_cutoff[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_FREQ;
176 float act_dyn_discrete[INDI_NUM_ACT]; // will be computed from freq at init
177 #endif
178 
179 #ifdef STABILIZATION_INDI_WLS_PRIORITIES
180 static float Wv[INDI_OUTPUTS] = STABILIZATION_INDI_WLS_PRIORITIES;
181 #else
182 //State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
183 #if INDI_OUTPUTS == 5
184 static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100, 100};
185 #else
186 static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100};
187 #endif
188 #endif
189 
193 #ifdef STABILIZATION_INDI_WLS_WU
194 float indi_Wu[INDI_NUM_ACT] = STABILIZATION_INDI_WLS_WU;
195 #else
196 float indi_Wu[INDI_NUM_ACT] = {[0 ... INDI_NUM_ACT - 1] = 1.0};
197 #endif
198 
202 #ifdef STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT
203 float stablization_indi_yaw_dist_limit = STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT;
204 #else // Put a rediculously high limit
206 #endif
207 
208 // variables needed for control
209 float actuator_state_filt_vect[INDI_NUM_ACT];
210 struct FloatRates angular_accel_ref = {0., 0., 0.};
211 struct FloatRates angular_rate_ref = {0., 0., 0.};
212 float angular_acceleration[3] = {0., 0., 0.};
213 float actuator_state[INDI_NUM_ACT];
214 float indi_u[INDI_NUM_ACT];
215 
216 float q_filt = 0.0;
217 float r_filt = 0.0;
218 
219 float stabilization_indi_filter_freq = 20.0; //Hz, for setting handler
220 
221 // variables needed for estimation
222 float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS];
223 float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS];
224 float actuator_state_filt_vectd[INDI_NUM_ACT];
225 float actuator_state_filt_vectdd[INDI_NUM_ACT];
226 float estimation_rate_d[INDI_NUM_ACT];
227 float estimation_rate_dd[INDI_NUM_ACT];
228 float du_estimation[INDI_NUM_ACT];
229 float ddu_estimation[INDI_NUM_ACT];
230 
231 // The learning rate per axis (roll, pitch, yaw, thrust)
232 float mu1[INDI_OUTPUTS] = {0.00001, 0.00001, 0.000003, 0.000002};
233 // The learning rate for the propeller inertia (scaled by 512 wrt mu1)
234 float mu2 = 0.002;
235 
236 // other variables
237 float act_obs[INDI_NUM_ACT];
238 
239 // Number of actuators used to provide thrust
242 
243 static struct Int32Eulers stab_att_sp_euler;
244 static struct Int32Quat stab_att_sp_quat;
245 
246 // Register actuator feedback if we rely on RPM information
247 #if STABILIZATION_INDI_RPM_FEEDBACK
248 #ifndef STABILIZATION_INDI_ACT_FEEDBACK_ID
249 #define STABILIZATION_INDI_ACT_FEEDBACK_ID ABI_BROADCAST
250 #endif
251 PRINT_CONFIG_VAR(STABILIZATION_INDI_ACT_FEEDBACK_ID)
252 
254 static void act_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act);
255 PRINT_CONFIG_MSG("STABILIZATION_INDI_RPM_FEEDBACK")
256 #endif
257 
258 float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS];
259 float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
260 #ifdef STABILIZATION_INDI_G1
261 float g1[INDI_OUTPUTS][INDI_NUM_ACT] = STABILIZATION_INDI_G1;
262 #else // old defines TODO remove
263 #if INDI_OUTPUTS == 5
264 float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
265  STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW,
266  STABILIZATION_INDI_G1_THRUST, STABILIZATION_INDI_G1_THRUST_X
267  };
268 #else
269 float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
270  STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
271  };
272 #endif
273 #endif
274 
275 float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
276 float g1_est[INDI_OUTPUTS][INDI_NUM_ACT];
277 float g2_est[INDI_NUM_ACT];
278 float g1_init[INDI_OUTPUTS][INDI_NUM_ACT];
279 float g2_init[INDI_NUM_ACT];
280 
287 #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
288 Butterworth2LowPass rates_filt_so[3];
289 #else
290 static struct FirstOrderLowPass rates_filt_fo[3];
291 #endif
292 struct FloatVect3 body_accel_f;
293 
294 void init_filters(void);
295 void sum_g1_g2(void);
296 
297 #if PERIODIC_TELEMETRY
299 static void send_eff_mat_g_indi(struct transport_tx *trans, struct link_device *dev)
300 {
301  float zero = 0.0;
302  pprz_msg_send_EFF_MAT_G(trans, dev, AC_ID,
303  1, &zero,
304  1, &zero,
305  1, &zero,
306  INDI_NUM_ACT, g1g2[0],
307  INDI_NUM_ACT, g1g2[1],
308  INDI_NUM_ACT, g1g2[2],
309  INDI_NUM_ACT, g1g2[3],
310  INDI_NUM_ACT, g2_est);
311 }
312 
313 static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
314 {
315  struct Int32Quat *quat = stateGetNedToBodyQuat_i();
316  pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
321  &(quat->qi),
322  &(quat->qx),
323  &(quat->qy),
324  &(quat->qz));
325 }
326 
327 static void send_att_full_indi(struct transport_tx *trans, struct link_device *dev)
328 {
329  float zero = 0.0;
330  struct FloatRates *body_rates = stateGetBodyRates_f();
331  struct FloatEulers att, att_sp;
332 #if GUIDANCE_INDI_HYBRID
334  struct FloatQuat stab_att_sp_quat_f;
335  QUAT_FLOAT_OF_BFP(stab_att_sp_quat_f, stab_att_sp_quat);
336  float_eulers_of_quat_zxy(&att_sp, &stab_att_sp_quat_f);
337 #else
338  att = *stateGetNedToBodyEulers_f();
340 #endif
341  pprz_msg_send_STAB_ATTITUDE(trans, dev, AC_ID,
342  &att.phi, &att.theta, &att.psi, // att
343  &att_sp.phi, &att_sp.theta, &att_sp.psi, // att.ref
344  &body_rates->p, // rate
345  &body_rates->q,
346  &body_rates->r,
347  &angular_rate_ref.p, // rate.ref
350  &angular_acceleration[0], // ang.acc
353  &angular_accel_ref.p, // ang.acc.ref
356  1, &zero, // inputs
357  INDI_NUM_ACT, indi_u); // out
358 }
359 #endif
360 
365 {
366  // Initialize filters
367  init_filters();
368 
369  int8_t i;
370 // If the deprecated STABILIZATION_INDI_ACT_DYN is used, convert it to the new FREQUENCY format
371 #ifdef STABILIZATION_INDI_ACT_FREQ
372  // Initialize the array of pointers to the rows of g1g2
373  for (i = 0; i < INDI_NUM_ACT; i++) {
374  act_dyn_discrete[i] = 1-exp(-act_first_order_cutoff[i]/PERIODIC_FREQUENCY);
375  }
376 #endif
377 
378 #if STABILIZATION_INDI_RPM_FEEDBACK
379  AbiBindMsgACT_FEEDBACK(STABILIZATION_INDI_ACT_FEEDBACK_ID, &act_feedback_ev, act_feedback_cb);
380 #endif
381 
384  float_vect_zero(estimation_rate_d, INDI_NUM_ACT);
385  float_vect_zero(estimation_rate_dd, INDI_NUM_ACT);
387 
388  //Calculate G1G2
389  sum_g1_g2();
390 
391  // Do not compute if not needed
392 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
393  //Calculate G1G2_PSEUDO_INVERSE
394  calc_g1g2_pseudo_inv();
395 #endif
396 
397  // Initialize the array of pointers to the rows of g1g2
398  for (i = 0; i < INDI_OUTPUTS; i++) {
399  Bwls[i] = g1g2[i];
400  }
401 
402  // Initialize the estimator matrices
403  float_vect_copy(g1_est[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
404  float_vect_copy(g2_est, g2, INDI_NUM_ACT);
405  // Remember the initial matrices
406  float_vect_copy(g1_init[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
407  float_vect_copy(g2_init, g2, INDI_NUM_ACT);
408 
409  // Assume all non-servos are delivering thrust
410  num_thrusters = INDI_NUM_ACT;
411  num_thrusters_x = 0;
412  for (i = 0; i < INDI_NUM_ACT; i++) {
415 
417 
419  }
420 
421 #if PERIODIC_TELEMETRY
425 #endif
426 }
427 
437 {
438  float_vect_zero(du_estimation, INDI_NUM_ACT);
439  float_vect_zero(ddu_estimation, INDI_NUM_ACT);
440 }
441 
443 {
445  float tau = 1.0 / (2.0 * M_PI * freq);
446  float sample_time = 1.0 / PERIODIC_FREQUENCY;
447 #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
448  init_butterworth_2_low_pass(&rates_filt_so[0], tau, sample_time, stateGetBodyRates_f()->p);
449  init_butterworth_2_low_pass(&rates_filt_so[1], tau, sample_time, stateGetBodyRates_f()->q);
450  init_butterworth_2_low_pass(&rates_filt_so[2], tau, sample_time, stateGetBodyRates_f()->r);
451 #else
453  init_first_order_low_pass(&rates_filt_fo[1], tau, sample_time, stateGetBodyRates_f()->q);
454  init_first_order_low_pass(&rates_filt_fo[2], tau, sample_time, stateGetBodyRates_f()->r);
455 #endif
456 }
457 
461 void init_filters(void)
462 {
463  // tau = 1/(2*pi*Fc)
464  float tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF);
465  float tau_est = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF);
466  float sample_time = 1.0 / PERIODIC_FREQUENCY;
467  // Filtering of the gyroscope
468  int8_t i;
469  for (i = 0; i < 3; i++) {
470  init_butterworth_2_low_pass(&measurement_lowpass_filters[i], tau, sample_time, 0.0);
471  init_butterworth_2_low_pass(&estimation_output_lowpass_filters[i], tau_est, sample_time, 0.0);
472  }
473 
474  // Filtering of the actuators
475  for (i = 0; i < INDI_NUM_ACT; i++) {
476  init_butterworth_2_low_pass(&actuator_lowpass_filters[i], tau, sample_time, 0.0);
477  init_butterworth_2_low_pass(&estimation_input_lowpass_filters[i], tau_est, sample_time, 0.0);
478  }
479 
480  // Filtering the bodyx acceleration with same cutoff as gyroscope
482 
483  // Filtering of the accel body z
484  init_butterworth_2_low_pass(&acceleration_lowpass_filter, tau_est, sample_time, 0.0);
485 
486 #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
487  tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_P);
488  init_butterworth_2_low_pass(&rates_filt_so[0], tau, sample_time, 0.0);
489  tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_Q);
490  init_butterworth_2_low_pass(&rates_filt_so[1], tau, sample_time, 0.0);
491  tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_R);
492  init_butterworth_2_low_pass(&rates_filt_so[2], tau, sample_time, 0.0);
493 #else
494  // Init rate filter for feedback
495  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)};
496 
497  init_first_order_low_pass(&rates_filt_fo[0], time_constants[0], sample_time, stateGetBodyRates_f()->p);
498  init_first_order_low_pass(&rates_filt_fo[1], time_constants[1], sample_time, stateGetBodyRates_f()->q);
499  init_first_order_low_pass(&rates_filt_fo[2], time_constants[2], sample_time, stateGetBodyRates_f()->r);
500 #endif
501 }
502 
511 void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
512 {
513 
514  // Propagate actuator filters
516  float actuator_state_filt_vect_prev[INDI_NUM_ACT];
517  for (int i = 0; i < INDI_NUM_ACT; i++) {
521  actuator_state_filt_vect_prev[i] = actuator_lowpass_filters[i].o[1];
522 
523  // calculate derivatives for estimation
524  float actuator_state_filt_vectd_prev = actuator_state_filt_vectd[i];
526  PERIODIC_FREQUENCY;
527  actuator_state_filt_vectdd[i] = (actuator_state_filt_vectd[i] - actuator_state_filt_vectd_prev) * PERIODIC_FREQUENCY;
528  }
529 
530  // Use the last actuator state for this computation
531  float g2_times_u_act_filt = float_vect_dot_product(g2, actuator_state_filt_vect_prev, INDI_NUM_ACT)/INDI_G_SCALING;
532 
533  // Predict angular acceleration u*B
534  float angular_acc_prediction_filt[INDI_OUTPUTS];
535  float_mat_vect_mul(angular_acc_prediction_filt, Bwls, actuator_state_filt_vect, INDI_OUTPUTS, INDI_NUM_ACT);
536  angular_acc_prediction_filt[2] -= g2_times_u_act_filt;
537 
538  /* Propagate the filter on the gyroscopes */
539  struct FloatRates *body_rates = stateGetBodyRates_f();
540  float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r};
541 
542  // Get the acceleration in body axes
543  struct Int32Vect3 *body_accel_i;
544  body_accel_i = stateGetAccelBody_i();
545  ACCELS_FLOAT_OF_BFP(body_accel_f, *body_accel_i);
546 
547  int8_t i;
548  for (i = 0; i < 3; i++) {
551 
553 
554  //Calculate the angular acceleration via finite difference
556  - measurement_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
557 
558  // Calculate derivatives for estimation
559  float estimation_rate_d_prev = estimation_rate_d[i];
561  PERIODIC_FREQUENCY;
562  estimation_rate_dd[i] = (estimation_rate_d[i] - estimation_rate_d_prev) * PERIODIC_FREQUENCY;
563  }
564 
565  // subtract u*B from angular acceleration
566  float angular_acc_disturbance_estimate[INDI_OUTPUTS];
567  float_vect_diff(angular_acc_disturbance_estimate, angular_acceleration, angular_acc_prediction_filt, 3);
568 
569  if (in_flight) {
570  // Limit the estimated disturbance in yaw for drones that are stable in sideslip
571  BoundAbs(angular_acc_disturbance_estimate[2], stablization_indi_yaw_dist_limit);
572  } else {
573  // Not in flight, so don't estimate disturbance
574  float_vect_zero(angular_acc_disturbance_estimate, INDI_OUTPUTS);
575  }
576 
577  //The rates used for feedback are by default the measured rates.
578  //If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.
579  //Note that due to the delay, the PD controller may need relaxed gains.
580  struct FloatRates rates_filt;
581 #if STABILIZATION_INDI_FILTER_ROLL_RATE
582 #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
583  rates_filt.p = update_butterworth_2_low_pass(&rates_filt_so[0], body_rates->p);
584 #else
585  rates_filt.p = update_first_order_low_pass(&rates_filt_fo[0], body_rates->p);
586 #endif
587 #else
588  rates_filt.p = body_rates->p;
589 #endif
590 #if STABILIZATION_INDI_FILTER_PITCH_RATE
591 #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
592  rates_filt.q = update_butterworth_2_low_pass(&rates_filt_so[1], body_rates->q);
593 #else
594  rates_filt.q = update_first_order_low_pass(&rates_filt_fo[1], body_rates->q);
595 #endif
596 #else
597  rates_filt.q = body_rates->q;
598 #endif
599 #if STABILIZATION_INDI_FILTER_YAW_RATE
600 #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
601  rates_filt.r = update_butterworth_2_low_pass(&rates_filt_so[2], body_rates->r);
602 #else
603  rates_filt.r = update_first_order_low_pass(&rates_filt_fo[2], body_rates->r);
604 #endif
605 #else
606  rates_filt.r = body_rates->r;
607 #endif
608 
609  // calculate the virtual control (reference acceleration) based on a PD controller
610  struct FloatRates rate_sp = stab_sp_to_rates_f(sp);
611  angular_accel_ref.p = (rate_sp.p - rates_filt.p) * indi_gains.rate.p;
612  angular_accel_ref.q = (rate_sp.q - rates_filt.q) * indi_gains.rate.q;
613  angular_accel_ref.r = (rate_sp.r - rates_filt.r) * indi_gains.rate.r;
614 
615  // compute virtual thrust
616  struct FloatVect3 v_thrust = { 0.f, 0.f, 0.f };
617  if (thrust->type == THRUST_INCR_SP) {
618  v_thrust.x = th_sp_to_incr_f(thrust, 0, THRUST_AXIS_X);
619  v_thrust.y = th_sp_to_incr_f(thrust, 0, THRUST_AXIS_Y);
620  v_thrust.z = th_sp_to_incr_f(thrust, 0, THRUST_AXIS_Z);
621 
622  // Compute estimated thrust
623  struct FloatVect3 thrust_filt = { 0.f, 0.f, 0.f };
624  for (i = 0; i < INDI_NUM_ACT; i++) {
626 #if INDI_OUTPUTS == 5
628 #endif
629  }
630  // Add the current estimated thrust to the increment
631  VECT3_ADD(v_thrust, thrust_filt);
632  } else {
633  // build incremental thrust
634  float th_cmd_z = (float)th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z);
635  for (i = 0; i < INDI_NUM_ACT; i++) {
636  v_thrust.z += th_cmd_z * Bwls[3][i];
637 #if INDI_OUTPUTS == 5
638  // TODO set X thrust from RC in the thrust input setpoint
639  cmd[COMMAND_THRUST_X] = radio_control.values[RADIO_CONTROL_THRUST_X];
640  v_thrust.x += cmd[COMMAND_THRUST_X] * Bwls[4][i];
641 #endif
642  }
643  v_thrust.y = 0.f;
644  }
645 
646  // This term compensates for the spinup torque in the yaw axis
647  float g2_times_u = float_vect_dot_product(g2, indi_u, INDI_NUM_ACT)/INDI_G_SCALING;
648 
649  if (in_flight) {
650  // Limit the estimated disturbance in yaw for drones that are stable in sideslip
651  BoundAbs(angular_acc_disturbance_estimate[2], stablization_indi_yaw_dist_limit);
652  } else {
653  // Not in flight, so don't estimate disturbance
654  float_vect_zero(angular_acc_disturbance_estimate, INDI_OUTPUTS);
655  }
656 
657  // The control objective in array format
658  indi_v[0] = (angular_accel_ref.p - angular_acc_disturbance_estimate[0]);
659  indi_v[1] = (angular_accel_ref.q - angular_acc_disturbance_estimate[1]);
660  indi_v[2] = (angular_accel_ref.r - angular_acc_disturbance_estimate[2]) + g2_times_u;
661  indi_v[3] = v_thrust.z;
662 #if INDI_OUTPUTS == 5
663  indi_v[4] = v_thrust.x;
664 #endif
665 
666 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
667  // Calculate the increment for each actuator
668  for (i = 0; i < INDI_NUM_ACT; i++) {
669  indi_u[i] = (g1g2_pseudo_inv[i][0] * indi_v[0])
670  + (g1g2_pseudo_inv[i][1] * indi_v[1])
671  + (g1g2_pseudo_inv[i][2] * indi_v[2])
672  + (g1g2_pseudo_inv[i][3] * indi_v[3]);
673  }
674 #else
676 
677  // WLS Control Allocator
678  num_iter =
680  INDI_NUM_ACT, INDI_OUTPUTS);
681 #endif
682 
683  // Bound the inputs to the actuators
684  for (i = 0; i < INDI_NUM_ACT; i++) {
685  if (act_is_servo[i]) {
686  BoundAbs(indi_u[i], MAX_PPRZ);
687  } else {
688  if (autopilot_get_motors_on()) {
689  Bound(indi_u[i], 0, MAX_PPRZ);
690  } else {
691  indi_u[i] = -MAX_PPRZ;
692  }
693  }
694  }
695 
696  // Use online effectiveness estimation only when flying
697  if (in_flight && indi_use_adaptive) {
698  lms_estimation();
699  }
700 
701  /*Commit the actuator command*/
702  for (i = 0; i < INDI_NUM_ACT; i++) {
703  actuators_pprz[i] = (int16_t) indi_u[i];
704  }
705 
706  //update thrust command such that the current is correctly estimated
707  cmd[COMMAND_THRUST] = 0;
708  for (i = 0; i < INDI_NUM_ACT; i++) {
709  cmd[COMMAND_THRUST] += actuator_state[i] * (int32_t) act_is_thruster_z[i];
710  }
711  cmd[COMMAND_THRUST] /= num_thrusters;
712 
713 }
714 
719 {
720  // Calculate the min and max increments
721  for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
724  u_pref_stab_indi[i] = act_pref[i];
725 
726 #ifdef GUIDANCE_INDI_MIN_THROTTLE
727  float airspeed = stateGetAirspeed_f();
728  //limit minimum thrust ap can give
729  if (!act_is_servo[i]) {
732  u_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE;
733  } else {
734  u_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD;
735  }
736  }
737  }
738 #endif
739  }
740 }
741 
750 void stabilization_indi_attitude_run(bool in_flight, struct StabilizationSetpoint *att_sp, struct ThrustSetpoint *thrust, int32_t *cmd)
751 {
752  stab_att_sp_euler = stab_sp_to_eulers_i(att_sp); // stab_att_sp_euler.psi still used in ref..
753  stab_att_sp_quat = stab_sp_to_quat_i(att_sp); // quat attitude setpoint
754 
755  /* attitude error in float */
756  struct FloatQuat att_err;
757  struct FloatQuat *att_quat = stateGetNedToBodyQuat_f();
758  struct FloatQuat quat_sp = stab_sp_to_quat_f(att_sp);
759 
760  float_quat_inv_comp_norm_shortest(&att_err, att_quat, &quat_sp);
761 
762  struct FloatVect3 att_fb;
763 #if TILT_TWIST_CTRL
764  struct FloatQuat tilt;
765  struct FloatQuat twist;
766  float_quat_tilt_twist(&tilt, &twist, &att_err);
767  att_fb.x = tilt.qx;
768  att_fb.y = tilt.qy;
769  att_fb.z = twist.qz;
770 #else
771  att_fb.x = att_err.qx;
772  att_fb.y = att_err.qy;
773  att_fb.z = att_err.qz;
774 #endif
775 
776  // local variable to compute rate setpoints based on attitude error
777  struct FloatRates rate_sp;
778  // calculate the virtual control (reference acceleration) based on a PD controller
779  rate_sp.p = indi_gains.att.p * att_fb.x / indi_gains.rate.p;
780  rate_sp.q = indi_gains.att.q * att_fb.y / indi_gains.rate.q;
781  rate_sp.r = indi_gains.att.r * att_fb.z / indi_gains.rate.r;
782 
783  // Add feed-forward rates to the attitude feedback part
784  struct FloatRates ff_rates = stab_sp_to_rates_f(att_sp);
785  RATES_ADD(rate_sp, ff_rates);
786 
787  // Store for telemetry
788  angular_rate_ref.p = rate_sp.p;
789  angular_rate_ref.q = rate_sp.q;
790  angular_rate_ref.r = rate_sp.r;
791 
792  // Possibly we can use some bounding here
793  /*BoundAbs(rate_sp.r, 5.0);*/
794 
795  /* compute the INDI command */
796  struct StabilizationSetpoint sp = stab_sp_from_rates_f(&rate_sp);
797  stabilization_indi_rate_run(in_flight, &sp, thrust, cmd);
798 }
799 
807 {
808 #if STABILIZATION_INDI_RPM_FEEDBACK
809  float_vect_copy(actuator_state, act_obs, INDI_NUM_ACT);
810 #else
811  //actuator dynamics
812  int8_t i;
813  float UNUSED prev_actuator_state;
814  for (i = 0; i < INDI_NUM_ACT; i++) {
815  prev_actuator_state = actuator_state[i];
816 
818  + act_dyn_discrete[i] * (indi_u[i] - actuator_state[i]);
819 
820 #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
821  if ((actuator_state[i] - prev_actuator_state) > act_rate_limit[i]) {
822  actuator_state[i] = prev_actuator_state + act_rate_limit[i];
823  } else if ((actuator_state[i] - prev_actuator_state) < -act_rate_limit[i]) {
824  actuator_state[i] = prev_actuator_state - act_rate_limit[i];
825  }
826 #endif
827  }
828 
829 #endif
830 }
831 
842 void calc_g1_element(float ddx_error, int8_t i, int8_t j, float mu)
843 {
844  g1_est[i][j] = g1_est[i][j] - du_estimation[j] * mu * ddx_error;
845 }
846 
856 void calc_g2_element(float ddx_error, int8_t j, float mu)
857 {
858  g2_est[j] = g2_est[j] - ddu_estimation[j] * mu * ddx_error;
859 }
860 
866 void lms_estimation(void)
867 {
868 
869  // Get the acceleration in body axes
870  struct Int32Vect3 *body_accel_i;
871  body_accel_i = stateGetAccelBody_i();
872  ACCELS_FLOAT_OF_BFP(body_accel_f, *body_accel_i);
873 
874  // Filter the acceleration in z axis
876 
877  // Calculate the derivative of the acceleration via finite difference
878  float indi_accel_d = (acceleration_lowpass_filter.o[0]
879  - acceleration_lowpass_filter.o[1]) * PERIODIC_FREQUENCY;
880 
881  // Use xml setting for adaptive mu for lms
882  // Set default value if not defined
883 #ifndef STABILIZATION_INDI_ADAPTIVE_MU
884  float adaptive_mu_lr = 0.001;
885 #else
886  float adaptive_mu_lr = STABILIZATION_INDI_ADAPTIVE_MU;
887 #endif
888 
889  // scale the inputs to avoid numerical errors
890  float_vect_smul(du_estimation, actuator_state_filt_vectd, adaptive_mu_lr, INDI_NUM_ACT);
891  float_vect_smul(ddu_estimation, actuator_state_filt_vectdd, adaptive_mu_lr / PERIODIC_FREQUENCY, INDI_NUM_ACT);
892 
893  float ddx_estimation[INDI_OUTPUTS] = {estimation_rate_dd[0], estimation_rate_dd[1], estimation_rate_dd[2], indi_accel_d};
894 
895  //Estimation of G
896  // TODO: only estimate when du_norm2 is large enough (enough input)
897  /*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];*/
898  int8_t i;
899  for (i = 0; i < INDI_OUTPUTS; i++) {
900  // Calculate the error between prediction and measurement
901  float ddx_error = - ddx_estimation[i];
902  int8_t j;
903  for (j = 0; j < INDI_NUM_ACT; j++) {
904  ddx_error += g1_est[i][j] * du_estimation[j];
905  if (i == 2) {
906  // Changing the momentum of the rotors gives a counter torque
907  ddx_error += g2_est[j] * ddu_estimation[j];
908  }
909  }
910 
911  // when doing the yaw axis, also use G2
912  if (i == 2) {
913  for (j = 0; j < INDI_NUM_ACT; j++) {
914  calc_g2_element(ddx_error, j, mu2);
915  }
916  } else if (i == 3) {
917  // If the acceleration change is very large (rough landing), don't adapt
918  if (fabs(indi_accel_d) > 60.0) {
919  ddx_error = 0.0;
920  }
921  }
922 
923  // Calculate the row of the G1 matrix corresponding to this axis
924  for (j = 0; j < INDI_NUM_ACT; j++) {
925  calc_g1_element(ddx_error, i, j, mu1[i]);
926  }
927  }
928 
929  bound_g_mat();
930 
931  // Save the calculated matrix to G1 and G2
932  // until thrust is included, first part of the array
933  float_vect_copy(g1[0], g1_est[0], INDI_OUTPUTS * INDI_NUM_ACT);
934  float_vect_copy(g2, g2_est, INDI_NUM_ACT);
935 
936  // Calculate sum of G1 and G2 for Bwls
937  sum_g1_g2();
938 
939 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
940  // Calculate the inverse of (G1+G2)
941  calc_g1g2_pseudo_inv();
942 #endif
943 }
944 
949 void sum_g1_g2(void)
950 {
951  int8_t i;
952  int8_t j;
953  for (i = 0; i < INDI_OUTPUTS; i++) {
954  for (j = 0; j < INDI_NUM_ACT; j++) {
955  if (i != 2) {
956  g1g2[i][j] = g1[i][j] / INDI_G_SCALING;
957  } else {
958  g1g2[i][j] = (g1[i][j] + g2[j]) / INDI_G_SCALING;
959  }
960  }
961  }
962 }
963 
964 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
969 void calc_g1g2_pseudo_inv(void)
970 {
971  //G1G2*transpose(G1G2)
972  //calculate matrix multiplication of its transpose INDI_OUTPUTSxnum_act x num_actxINDI_OUTPUTS
973  float element = 0;
974  int8_t row;
975  int8_t col;
976  int8_t i;
977  for (row = 0; row < INDI_OUTPUTS; row++) {
978  for (col = 0; col < INDI_OUTPUTS; col++) {
979  element = 0;
980  for (i = 0; i < INDI_NUM_ACT; i++) {
981  element = element + g1g2[row][i] * g1g2[col][i];
982  }
983  g1g2_trans_mult[row][col] = element;
984  }
985  }
986 
987  //there are numerical errors if the scaling is not right.
988  float_vect_scale(g1g2_trans_mult[0], 1000.0, INDI_OUTPUTS * INDI_OUTPUTS);
989 
990  //inverse of 4x4 matrix
992 
993  //scale back
994  float_vect_scale(g1g2inv[0], 1000.0, INDI_OUTPUTS * INDI_OUTPUTS);
995 
996  //G1G2'*G1G2inv
997  //calculate matrix multiplication INDI_NUM_ACTxINDI_OUTPUTS x INDI_OUTPUTSxINDI_OUTPUTS
998  for (row = 0; row < INDI_NUM_ACT; row++) {
999  for (col = 0; col < INDI_OUTPUTS; col++) {
1000  element = 0;
1001  for (i = 0; i < INDI_OUTPUTS; i++) {
1002  element = element + g1g2[i][row] * g1g2inv[col][i];
1003  }
1004  g1g2_pseudo_inv[row][col] = element;
1005  }
1006  }
1007 }
1008 #endif
1009 
1010 #if STABILIZATION_INDI_RPM_FEEDBACK
1011 static void act_feedback_cb(uint8_t sender_id UNUSED, struct act_feedback_t *feedback, uint8_t num_act)
1012 {
1013  int8_t i;
1014  for (i = 0; i < num_act; i++) {
1015  // Sanity check that index is valid
1016  if (feedback[i].idx < INDI_NUM_ACT && feedback[i].set.rpm) {
1017  int8_t idx = feedback[i].idx;
1018  act_obs[idx] = (feedback[i].rpm - get_servo_min(idx));
1019  act_obs[idx] *= (MAX_PPRZ / (float)(get_servo_max(idx) - get_servo_min(idx)));
1020  Bound(act_obs[idx], 0, MAX_PPRZ);
1021  }
1022  }
1023 }
1024 #endif
1025 
1026 static void bound_g_mat(void)
1027 {
1028  int8_t i;
1029  int8_t j;
1030  for (j = 0; j < INDI_NUM_ACT; j++) {
1031  float max_limit;
1032  float min_limit;
1033 
1034  // Limit the values of the estimated G1 matrix
1035  for (i = 0; i < INDI_OUTPUTS; i++) {
1036  if (g1_init[i][j] > 0.0) {
1037  max_limit = g1_init[i][j] * INDI_ALLOWED_G_FACTOR;
1038  min_limit = g1_init[i][j] / INDI_ALLOWED_G_FACTOR;
1039  } else {
1040  max_limit = g1_init[i][j] / INDI_ALLOWED_G_FACTOR;
1041  min_limit = g1_init[i][j] * INDI_ALLOWED_G_FACTOR;
1042  }
1043 
1044  if (g1_est[i][j] > max_limit) {
1045  g1_est[i][j] = max_limit;
1046  }
1047  if (g1_est[i][j] < min_limit) {
1048  g1_est[i][j] = min_limit;
1049  }
1050  }
1051 
1052  // Do the same for the G2 matrix
1053  if (g2_init[j] > 0.0) {
1054  max_limit = g2_init[j] * INDI_ALLOWED_G_FACTOR;
1055  min_limit = g2_init[j] / INDI_ALLOWED_G_FACTOR;
1056  } else {
1057  max_limit = g2_init[j] / INDI_ALLOWED_G_FACTOR;
1058  min_limit = g2_init[j] * INDI_ALLOWED_G_FACTOR;
1059  }
1060 
1061  if (g2_est[j] > max_limit) {
1062  g2_est[j] = max_limit;
1063  }
1064  if (g2_est[j] < min_limit) {
1065  g2_est[j] = min_limit;
1066  }
1067  }
1068 }
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:295
uint8_t last_wp UNUSED
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_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
static void float_vect_smul(float *o, const float *a, const float s, const int n)
o = a * s
static void float_mat_vect_mul(float *o, float **a, float *b, int m, int n)
o = a * b
static void float_vect_scale(float *a, const float s, const int n)
a *= s
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
void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
static void float_vect_diff(float *o, const float *a, const float *b, const int n)
o = a - b
void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, struct FloatQuat *quat)
Tilt twist decomposition of a quaternion (z axis)
bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4])
4x4 Matrix inverse
euler angles
Roation quaternion.
angular rates
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:344
#define QUAT_FLOAT_OF_BFP(_qf, _qi)
Definition: pprz_algebra.h:745
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:709
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
euler angles
Rotation quaternion.
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1113
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
Definition: state.h:953
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
Butterworth2LowPass thrust_filt
static float p[2][2]
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
Simple first order low pass filter with bilinear transform.
float o[2]
output history
static float update_first_order_low_pass(struct FirstOrderLowPass *filter, float value)
Update first order low pass filter state with a new value.
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
static void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, float sample_time, float value)
Init first order low pass filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
First order low pass filter structure.
Second order low pass filter structure.
Hardware independent API for actuators (servos, motor controllers).
int32_t rpm
RPM.
Definition: actuators.h:51
uint8_t idx
General index of the actuators (generated in airframe.h)
Definition: actuators.h:45
static float mu
Physical model parameters.
Definition: nps_fdm_rover.c:60
static uint32_t idx
#define MAX_PPRZ
Definition: paparazzi.h:8
static abi_event act_feedback_ev
Paparazzi floating point algebra.
struct RadioControl radio_control
Definition: radio_control.c:33
Generic interface for radio control modules.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:67
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
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)
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.
Butterworth2LowPass acceleration_body_x_filter
static float Wv[INDI_OUTPUTS]
float act_pref[INDI_NUM_ACT]
float * Bwls[INDI_OUTPUTS]
int32_t stabilization_att_indi_cmd[COMMANDS_NB]
float u_min_stab_indi[INDI_NUM_ACT]
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
bool act_is_thruster_x[INDI_NUM_ACT]
bool act_is_servo[INDI_NUM_ACT]
static struct FirstOrderLowPass rates_filt_fo[3]
float angular_acceleration[3]
int32_t num_thrusters
float actuator_state_filt_vectd[INDI_NUM_ACT]
float indi_Wu[INDI_NUM_ACT]
Weighting of different actuators in the cost function.
float r_filt
#define STABILIZATION_INDI_FILT_CUTOFF_P
static void bound_g_mat(void)
float u_max_stab_indi[INDI_NUM_ACT]
float mu1[INDI_OUTPUTS]
#define STABILIZATION_INDI_FILT_CUTOFF_Q
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 struct Int32Eulers stab_att_sp_euler
float estimation_rate_d[INDI_NUM_ACT]
float du_estimation[INDI_NUM_ACT]
float actuator_state_filt_vectdd[INDI_NUM_ACT]
Butterworth2LowPass measurement_lowpass_filters[3]
void stabilization_indi_update_filt_freq(float freq)
bool act_is_thruster_z[INDI_NUM_ACT]
float g2[INDI_NUM_ACT]
struct FloatVect3 body_accel_f
float u_pref_stab_indi[INDI_NUM_ACT]
float ddu_estimation[INDI_NUM_ACT]
float estimation_rate_dd[INDI_NUM_ACT]
float mu2
float act_obs[INDI_NUM_ACT]
int32_t num_thrusters_x
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
float stablization_indi_yaw_dist_limit
Limit the maximum specific moment that can be compensated (units rad/s^2)
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]
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
bool indi_use_adaptive
float g2_init[INDI_NUM_ACT]
Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT]
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]
static struct Int32Quat stab_att_sp_quat
float actuator_state[INDI_NUM_ACT]
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
float g1_est[INDI_OUTPUTS][INDI_NUM_ACT]
float indi_v[INDI_OUTPUTS]
float stabilization_indi_filter_freq
int num_iter
float indi_u[INDI_NUM_ACT]
static void get_actuator_state(void)
Function that tries to get actuator feedback.
Butterworth2LowPass actuator_lowpass_filters[INDI_NUM_ACT]
void init_filters(void)
Function that resets the filters to zeros.
Butterworth2LowPass estimation_output_lowpass_filters[3]
#define INDI_G_SCALING
#define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF
#define STABILIZATION_INDI_FILT_CUTOFF
struct FloatRates rate
struct FloatRates att
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Stabilization setpoint.
Definition: stabilization.h:53
union StabilizationSetpoint::@278 sp
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
Definition: stabilization.h:82
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_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.
Definition: vl53l1_types.h:83
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
int wls_alloc(float *u, float *v, float *umin, float *umax, float **B, float *u_guess, float *W_init, float *Wv, float *Wu, float *up, float gamma_sq, int imax, int n_u, int n_v)
active set algorithm for control allocation
Definition: wls_alloc.c:108