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 
37 
39 #include "state.h"
40 #include "generated/airframe.h"
43 #include "modules/core/abi.h"
45 #include "math/wls/wls_alloc.h"
46 #include <stdio.h>
47 
48 // Factor that the estimated G matrix is allowed to deviate from initial one
49 #define INDI_ALLOWED_G_FACTOR 2.0
50 
51 #ifdef STABILIZATION_INDI_FILT_CUTOFF_P
52 #define STABILIZATION_INDI_FILTER_ROLL_RATE TRUE
53 #else
54 #define STABILIZATION_INDI_FILT_CUTOFF_P 20.0
55 #endif
56 
57 #ifdef STABILIZATION_INDI_FILT_CUTOFF_Q
58 #define STABILIZATION_INDI_FILTER_PITCH_RATE TRUE
59 #else
60 #define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0
61 #endif
62 
63 #ifdef STABILIZATION_INDI_FILT_CUTOFF_R
64 #define STABILIZATION_INDI_FILTER_YAW_RATE TRUE
65 #else
66 #define STABILIZATION_INDI_FILT_CUTOFF_R 20.0
67 #endif
68 
69 // Default is WLS
70 #ifndef STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
71 #define STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE FALSE
72 #endif
73 
74 #ifndef STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
75 #define STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER FALSE
76 #endif
77 
78 // Airspeed [m/s] at which the forward flight throttle limit is used instead of
79 // the hover throttle limit.
80 #ifndef STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD
81 #define STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD 8.0
82 #endif
83 
84 #if INDI_OUTPUTS > 4
85 #ifndef STABILIZATION_INDI_G1_THRUST_X
86 #error "You must define STABILIZATION_INDI_G1_THRUST_X for your number of INDI_OUTPUTS"
87 #endif
88 #endif
89 
90 #ifdef SetCommandsFromRC
91 #warning SetCommandsFromRC not used: STAB_INDI writes actuators directly
92 #endif
93 
94 #ifdef SetAutoCommandsFromRC
95 #warning SetAutoCommandsFromRC not used: STAB_INDI writes actuators directly
96 #endif
97 
98 
99 #if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
100 #if INDI_NUM_ACT > WLS_N_U
101 #error Matrix-WLS_N_U too small or not defined: define WLS_N_U >= INDI_NUM_ACT in airframe file
102 #endif
103 #if INDI_OUTPUTS > WLS_N_V
104 #error Matrix-WLS_N_V too small or not defined: define WLS_N_U >= INDI_OUTPUTS in airframe file
105 #endif
106 #endif
107 
108 float du_min_stab_indi[INDI_NUM_ACT];
109 float du_max_stab_indi[INDI_NUM_ACT];
110 float du_pref_stab_indi[INDI_NUM_ACT];
111 float indi_v[INDI_OUTPUTS];
112 float *Bwls[INDI_OUTPUTS];
113 int num_iter = 0;
114 
115 static void lms_estimation(void);
116 static void get_actuator_state(void);
117 static void calc_g1_element(float dx_error, int8_t i, int8_t j, float mu_extra);
118 static void calc_g2_element(float dx_error, int8_t j, float mu_extra);
119 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
120 static void calc_g1g2_pseudo_inv(void);
121 #endif
122 static void bound_g_mat(void);
123 
125 struct Indi_gains indi_gains = {
126  .att = {
127  STABILIZATION_INDI_REF_ERR_P,
128  STABILIZATION_INDI_REF_ERR_Q,
129  STABILIZATION_INDI_REF_ERR_R
130  },
131  .rate = {
132  STABILIZATION_INDI_REF_RATE_P,
133  STABILIZATION_INDI_REF_RATE_Q,
134  STABILIZATION_INDI_REF_RATE_R
135  },
136 };
137 
138 #if STABILIZATION_INDI_USE_ADAPTIVE
139 bool indi_use_adaptive = true;
140 #else
141 bool indi_use_adaptive = false;
142 #endif
143 
144 #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
145 float act_rate_limit[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_RATE_LIMIT;
146 #endif
147 
148 #ifdef STABILIZATION_INDI_ACT_IS_SERVO
149 bool act_is_servo[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO;
150 #else
151 bool act_is_servo[INDI_NUM_ACT] = {0};
152 #endif
153 
154 #ifdef STABILIZATION_INDI_ACT_IS_THRUSTER_X
155 bool act_is_thruster_x[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_THRUSTER_X;
156 #else
157 bool act_is_thruster_x[INDI_NUM_ACT] = {0};
158 #endif
159 
160 bool act_is_thruster_z[INDI_NUM_ACT];
161 
162 #ifdef STABILIZATION_INDI_ACT_PREF
163 // Preferred (neutral, least energy) actuator value
164 float act_pref[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_PREF;
165 #else
166 // Assume 0 is neutral
167 float act_pref[INDI_NUM_ACT] = {0.0};
168 #endif
169 
170 #ifdef STABILIZATION_INDI_ACT_DYN
171 #warning STABILIZATION_INDI_ACT_DYN is deprecated, use STABILIZATION_INDI_ACT_FREQ instead.
172 #warning You now have to define the continuous time corner frequency in rad/s of the actuators.
173 #warning "Use -ln(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old values."
174 float act_dyn_discrete[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
175 #else
176 float act_first_order_cutoff[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_FREQ;
177 float act_dyn_discrete[INDI_NUM_ACT]; // will be computed from freq at init
178 #endif
179 
180 #ifdef STABILIZATION_INDI_WLS_PRIORITIES
181 static float Wv[INDI_OUTPUTS] = STABILIZATION_INDI_WLS_PRIORITIES;
182 #else
183 //State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
184 #if INDI_OUTPUTS == 5
185 static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100, 100};
186 #else
187 static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100};
188 #endif
189 #endif
190 
194 #ifdef STABILIZATION_INDI_WLS_WU
195 float indi_Wu[INDI_NUM_ACT] = STABILIZATION_INDI_WLS_WU;
196 #else
197 float indi_Wu[INDI_NUM_ACT] = {[0 ... INDI_NUM_ACT - 1] = 1.0};
198 #endif
199 
200 // variables needed for control
201 float actuator_state_filt_vect[INDI_NUM_ACT];
202 struct FloatRates angular_accel_ref = {0., 0., 0.};
203 struct FloatRates angular_rate_ref = {0., 0., 0.};
204 float angular_acceleration[3] = {0., 0., 0.};
205 float actuator_state[INDI_NUM_ACT];
206 float indi_u[INDI_NUM_ACT];
207 float indi_du[INDI_NUM_ACT];
209 
210 float q_filt = 0.0;
211 float r_filt = 0.0;
212 
213 // variables needed for estimation
214 float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS];
215 float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS];
216 float actuator_state_filt_vectd[INDI_NUM_ACT];
217 float actuator_state_filt_vectdd[INDI_NUM_ACT];
218 float estimation_rate_d[INDI_NUM_ACT];
219 float estimation_rate_dd[INDI_NUM_ACT];
220 float du_estimation[INDI_NUM_ACT];
221 float ddu_estimation[INDI_NUM_ACT];
222 
223 // The learning rate per axis (roll, pitch, yaw, thrust)
224 float mu1[INDI_OUTPUTS] = {0.00001, 0.00001, 0.000003, 0.000002};
225 // The learning rate for the propeller inertia (scaled by 512 wrt mu1)
226 float mu2 = 0.002;
227 
228 // other variables
229 float act_obs[INDI_NUM_ACT];
230 
231 // Number of actuators used to provide thrust
234 
238 
239 // Register actuator feedback if we rely on RPM information
240 #if STABILIZATION_INDI_RPM_FEEDBACK
241 #ifndef STABILIZATION_INDI_ACT_FEEDBACK_ID
242 #define STABILIZATION_INDI_ACT_FEEDBACK_ID ABI_BROADCAST
243 #endif
244 PRINT_CONFIG_VAR(STABILIZATION_INDI_ACT_FEEDBACK_ID)
245 
247 static void act_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act);
248 PRINT_CONFIG_MSG("STABILIZATION_INDI_RPM_FEEDBACK")
249 #endif
250 
252 static void thrust_cb(uint8_t sender_id, struct FloatVect3 thrust_increment);
255 
256 float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS];
257 float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
258 #ifdef STABILIZATION_INDI_G1
259 float g1[INDI_OUTPUTS][INDI_NUM_ACT] = STABILIZATION_INDI_G1;
260 #else // old defines TODO remove
261 #if INDI_OUTPUTS == 5
262 float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
263  STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW,
264  STABILIZATION_INDI_G1_THRUST, STABILIZATION_INDI_G1_THRUST_X
265  };
266 #else
267 float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
268  STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
269  };
270 #endif
271 #endif
272 
273 float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
274 float g1_est[INDI_OUTPUTS][INDI_NUM_ACT];
275 float g2_est[INDI_NUM_ACT];
276 float g1_init[INDI_OUTPUTS][INDI_NUM_ACT];
277 float g2_init[INDI_NUM_ACT];
278 
284 #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
285 Butterworth2LowPass rates_filt_so[3];
286 #else
287 static struct FirstOrderLowPass rates_filt_fo[3];
288 #endif
289 struct FloatVect3 body_accel_f;
290 
291 void init_filters(void);
292 void sum_g1_g2(void);
293 
294 #if PERIODIC_TELEMETRY
296 static void send_eff_mat_g_indi(struct transport_tx *trans, struct link_device *dev)
297 {
298  float zero = 0.0;
299  pprz_msg_send_EFF_MAT_G(trans, dev, AC_ID,
300  1, &zero,
301  1, &zero,
302  1, &zero,
303  INDI_NUM_ACT, g1g2[0],
304  INDI_NUM_ACT, g1g2[1],
305  INDI_NUM_ACT, g1g2[2],
306  INDI_NUM_ACT, g1g2[3],
307  INDI_NUM_ACT, g2_est);
308 }
309 
310 static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
311 {
312  struct Int32Quat *quat = stateGetNedToBodyQuat_i();
313  pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
318  &(quat->qi),
319  &(quat->qx),
320  &(quat->qy),
321  &(quat->qz));
322 }
323 
324 static void send_att_full_indi(struct transport_tx *trans, struct link_device *dev)
325 {
326  float zero = 0.0;
327  struct FloatRates *body_rates = stateGetBodyRates_f();
328  pprz_msg_send_STAB_ATTITUDE(trans, dev, AC_ID,
329  &zero, &zero, &zero, // att
330  &zero, &zero, &zero, // att.ref
331  &body_rates->p, // rate
332  &body_rates->q,
333  &body_rates->r,
334  &angular_rate_ref.p, // rate.ref
337  &angular_acceleration[0], // ang.acc
340  &angular_accel_ref.p, // ang.acc.ref
343  1, &zero, // inputs
344  INDI_NUM_ACT, indi_u); // out
345 }
346 #endif
347 
352 {
353  // Initialize filters
354  init_filters();
355 
356  int8_t i;
357 // If the deprecated STABILIZATION_INDI_ACT_DYN is used, convert it to the new FREQUENCY format
358 #ifdef STABILIZATION_INDI_ACT_FREQ
359  // Initialize the array of pointers to the rows of g1g2
360  for (i = 0; i < INDI_NUM_ACT; i++) {
361  act_dyn_discrete[i] = 1-exp(-act_first_order_cutoff[i]/PERIODIC_FREQUENCY);
362  }
363 #endif
364 
365 #if STABILIZATION_INDI_RPM_FEEDBACK
366  AbiBindMsgACT_FEEDBACK(STABILIZATION_INDI_ACT_FEEDBACK_ID, &act_feedback_ev, act_feedback_cb);
367 #endif
368  AbiBindMsgTHRUST(THRUST_INCREMENT_ID, &thrust_ev, thrust_cb);
369 
372  float_vect_zero(estimation_rate_d, INDI_NUM_ACT);
373  float_vect_zero(estimation_rate_dd, INDI_NUM_ACT);
375 
376  //Calculate G1G2
377  sum_g1_g2();
378 
379  // Do not compute if not needed
380 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
381  //Calculate G1G2_PSEUDO_INVERSE
382  calc_g1g2_pseudo_inv();
383 #endif
384 
385  // Initialize the array of pointers to the rows of g1g2
386  for (i = 0; i < INDI_OUTPUTS; i++) {
387  Bwls[i] = g1g2[i];
388  }
389 
390  // Initialize the estimator matrices
391  float_vect_copy(g1_est[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
392  float_vect_copy(g2_est, g2, INDI_NUM_ACT);
393  // Remember the initial matrices
394  float_vect_copy(g1_init[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
395  float_vect_copy(g2_init, g2, INDI_NUM_ACT);
396 
397  // Assume all non-servos are delivering thrust
398  num_thrusters = INDI_NUM_ACT;
399  num_thrusters_x = 0;
400  for (i = 0; i < INDI_NUM_ACT; i++) {
403 
405 
407  }
408 
409 #if PERIODIC_TELEMETRY
413 #endif
414 }
415 
425 {
426  /* reset psi setpoint to current psi angle */
428 
429  float_vect_zero(du_estimation, INDI_NUM_ACT);
430  float_vect_zero(ddu_estimation, INDI_NUM_ACT);
431 }
432 
436 void init_filters(void)
437 {
438  // tau = 1/(2*pi*Fc)
439  float tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF);
440  float tau_est = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF);
441  float sample_time = 1.0 / PERIODIC_FREQUENCY;
442  // Filtering of the gyroscope
443  int8_t i;
444  for (i = 0; i < 3; i++) {
445  init_butterworth_2_low_pass(&measurement_lowpass_filters[i], tau, sample_time, 0.0);
446  init_butterworth_2_low_pass(&estimation_output_lowpass_filters[i], tau_est, sample_time, 0.0);
447  }
448 
449  // Filtering of the actuators
450  for (i = 0; i < INDI_NUM_ACT; i++) {
451  init_butterworth_2_low_pass(&actuator_lowpass_filters[i], tau, sample_time, 0.0);
452  init_butterworth_2_low_pass(&estimation_input_lowpass_filters[i], tau_est, sample_time, 0.0);
453  }
454 
455  // Filtering of the accel body z
456  init_butterworth_2_low_pass(&acceleration_lowpass_filter, tau_est, sample_time, 0.0);
457 
458 #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
459  tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_P);
460  init_butterworth_2_low_pass(&rates_filt_so[0], tau, sample_time, 0.0);
461  tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_Q);
462  init_butterworth_2_low_pass(&rates_filt_so[1], tau, sample_time, 0.0);
463  tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_R);
464  init_butterworth_2_low_pass(&rates_filt_so[2], tau, sample_time, 0.0);
465 #else
466  // Init rate filter for feedback
467  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)};
468 
469  init_first_order_low_pass(&rates_filt_fo[0], time_constants[0], sample_time, stateGetBodyRates_f()->p);
470  init_first_order_low_pass(&rates_filt_fo[1], time_constants[1], sample_time, stateGetBodyRates_f()->q);
471  init_first_order_low_pass(&rates_filt_fo[2], time_constants[2], sample_time, stateGetBodyRates_f()->r);
472 #endif
473 }
474 
479 {
480  /* set failsafe to zero roll/pitch and current heading */
483  stab_att_sp_quat.qx = 0;
484  stab_att_sp_quat.qy = 0;
486 }
487 
494 {
495  // stab_att_sp_euler.psi still used in ref..
496  stab_att_sp_euler = *rpy;
497 
500 }
501 
506 {
507  stab_att_sp_quat = *quat;
510 }
511 
519 {
520  // stab_att_sp_euler.psi still used in ref..
522 
523  // compute sp_euler phi/theta for debugging/telemetry
524  /* Rotate horizontal commands to body frame by psi */
526  int32_t s_psi, c_psi;
527  PPRZ_ITRIG_SIN(s_psi, psi);
528  PPRZ_ITRIG_COS(c_psi, psi);
529  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
530  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
531 
534 }
535 
542 {
546 }
547 
555 void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
556 {
557  /* Propagate the filter on the gyroscopes */
558  struct FloatRates *body_rates = stateGetBodyRates_f();
559  float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r};
560  int8_t i;
561  for (i = 0; i < 3; i++) {
564 
565  //Calculate the angular acceleration via finite difference
567  - measurement_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
568 
569  // Calculate derivatives for estimation
570  float estimation_rate_d_prev = estimation_rate_d[i];
572  PERIODIC_FREQUENCY;
573  estimation_rate_dd[i] = (estimation_rate_d[i] - estimation_rate_d_prev) * PERIODIC_FREQUENCY;
574  }
575 
576  //The rates used for feedback are by default the measured rates.
577  //If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.
578  //Note that due to the delay, the PD controller may need relaxed gains.
579  struct FloatRates rates_filt;
580 #if STABILIZATION_INDI_FILTER_ROLL_RATE
581 #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
582  rates_filt.p = update_butterworth_2_low_pass(&rates_filt_so[0], body_rates->p);
583 #else
584  rates_filt.p = update_first_order_low_pass(&rates_filt_fo[0], body_rates->p);
585 #endif
586 #else
587  rates_filt.p = body_rates->p;
588 #endif
589 #if STABILIZATION_INDI_FILTER_PITCH_RATE
590 #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
591  rates_filt.q = update_butterworth_2_low_pass(&rates_filt_so[1], body_rates->q);
592 #else
593  rates_filt.q = update_first_order_low_pass(&rates_filt_fo[1], body_rates->q);
594 #endif
595 #else
596  rates_filt.q = body_rates->q;
597 #endif
598 #if STABILIZATION_INDI_FILTER_YAW_RATE
599 #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER
600  rates_filt.r = update_butterworth_2_low_pass(&rates_filt_so[2], body_rates->r);
601 #else
602  rates_filt.r = update_first_order_low_pass(&rates_filt_fo[2], body_rates->r);
603 #endif
604 #else
605  rates_filt.r = body_rates->r;
606 #endif
607 
608  //calculate the virtual control (reference acceleration) based on a PD controller
609  angular_accel_ref.p = (rate_sp.p - rates_filt.p) * indi_gains.rate.p;
610  angular_accel_ref.q = (rate_sp.q - rates_filt.q) * indi_gains.rate.q;
611  angular_accel_ref.r = (rate_sp.r - rates_filt.r) * indi_gains.rate.r;
612 
613  g2_times_du = 0.0;
614  for (i = 0; i < INDI_NUM_ACT; i++) {
615  g2_times_du += g2[i] * indi_du[i];
616  }
617  //G2 is scaled by INDI_G_SCALING to make it readable
619 
620  float use_increment = 0.0;
621  if (in_flight) {
622  use_increment = 1.0;
623  }
624 
625  struct FloatVect3 v_thrust;
626  v_thrust.x = 0.0;
627  v_thrust.y = 0.0;
628  v_thrust.z = 0.0;
630  v_thrust = indi_thrust_increment;
631 
632  //update thrust command such that the current is correctly estimated
633  stabilization_cmd[COMMAND_THRUST] = 0;
634  for (i = 0; i < INDI_NUM_ACT; i++) {
635  stabilization_cmd[COMMAND_THRUST] += actuator_state[i] * (int32_t) act_is_thruster_z[i];
636  }
637  stabilization_cmd[COMMAND_THRUST] /= num_thrusters;
638 
639 #if INDI_OUTPUTS == 5
640  stabilization_cmd[COMMAND_THRUST_X] = 0;
641  for (i = 0; i < INDI_NUM_ACT; i++) {
642  stabilization_cmd[COMMAND_THRUST_X] += actuator_state[i] * (int32_t) act_is_thruster_x[i];
643  }
644  stabilization_cmd[COMMAND_THRUST_X] /= num_thrusters_x;
645 #endif
646 
647  } else {
648  // incremental thrust
649  for (i = 0; i < INDI_NUM_ACT; i++) {
650  v_thrust.z +=
651  (stabilization_cmd[COMMAND_THRUST] - use_increment * actuator_state_filt_vect[i]) * Bwls[3][i];
652 #if INDI_OUTPUTS == 5
653  stabilization_cmd[COMMAND_THRUST_X] = radio_control.values[RADIO_CONTROL_THRUST_X];
654  v_thrust.x +=
655  (stabilization_cmd[COMMAND_THRUST_X] - use_increment * actuator_state_filt_vect[i]) * Bwls[4][i];
656 #endif
657  }
658  }
659 
660  // The control objective in array format
664  indi_v[3] = v_thrust.z;
665 #if INDI_OUTPUTS == 5
666  indi_v[4] = v_thrust.x;
667 #endif
668 
669 
670 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
671  // Calculate the increment for each actuator
672  for (i = 0; i < INDI_NUM_ACT; i++) {
673  indi_du[i] = (g1g2_pseudo_inv[i][0] * indi_v[0])
674  + (g1g2_pseudo_inv[i][1] * indi_v[1])
675  + (g1g2_pseudo_inv[i][2] * indi_v[2])
676  + (g1g2_pseudo_inv[i][3] * indi_v[3]);
677  }
678 #else
680 
681 
682  // WLS Control Allocator
683  num_iter =
685  INDI_NUM_ACT, INDI_OUTPUTS);
686 #endif
687 
688  if (in_flight) {
689  // Add the increments to the actuators
691  } else {
692  // Not in flight, so don't increment
693  float_vect_copy(indi_u, indi_du, INDI_NUM_ACT);
694  }
695 
696  // Bound the inputs to the actuators
697  for (i = 0; i < INDI_NUM_ACT; i++) {
698  if (act_is_servo[i]) {
699  BoundAbs(indi_u[i], MAX_PPRZ);
700  } else {
701  if (autopilot_get_motors_on()) {
702  Bound(indi_u[i], 0, MAX_PPRZ);
703  } else {
704  indi_u[i] = -MAX_PPRZ;
705  }
706  }
707  }
708 
709  // Propagate actuator filters
711  for (i = 0; i < INDI_NUM_ACT; i++) {
715 
716  // calculate derivatives for estimation
717  float actuator_state_filt_vectd_prev = actuator_state_filt_vectd[i];
719  PERIODIC_FREQUENCY;
720  actuator_state_filt_vectdd[i] = (actuator_state_filt_vectd[i] - actuator_state_filt_vectd_prev) * PERIODIC_FREQUENCY;
721  }
722 
723  // Use online effectiveness estimation only when flying
724  if (in_flight && indi_use_adaptive) {
725  lms_estimation();
726  }
727 
728  /*Commit the actuator command*/
729  for (i = 0; i < INDI_NUM_ACT; i++) {
730  actuators_pprz[i] = (int16_t) indi_u[i];
731  }
732 }
733 
740 {
741  // Calculate the min and max increments
742  for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
746 
747 #ifdef GUIDANCE_INDI_MIN_THROTTLE
748  float airspeed = stateGetAirspeed_f();
749  //limit minimum thrust ap can give
750  if (!act_is_servo[i]) {
753  du_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE - use_increment * actuator_state_filt_vect[i];
754  } else {
755  du_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD - use_increment * actuator_state_filt_vect[i];
756  }
757  }
758  }
759 #endif
760  }
761 }
762 
769 void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
770 {
771  /* attitude error */
772  struct FloatQuat att_err;
773  struct FloatQuat *att_quat = stateGetNedToBodyQuat_f();
774  struct FloatQuat quat_sp_f;
775 
776  QUAT_FLOAT_OF_BFP(quat_sp_f, quat_sp);
777  float_quat_inv_comp_norm_shortest(&att_err, att_quat, &quat_sp_f);
778 
779  struct FloatVect3 att_fb;
780 
781 #if TILT_TWIST_CTRL
782  struct FloatQuat tilt;
783  struct FloatQuat twist;
784  float_quat_tilt_twist(&tilt, &twist, &att_err);
785  att_fb.x = tilt.qx;
786  att_fb.y = tilt.qy;
787  att_fb.z = twist.qz;
788 #else
789  att_fb.x = att_err.qx;
790  att_fb.y = att_err.qy;
791  att_fb.z = att_err.qz;
792 #endif
793 
794  // local variable to compute rate setpoints based on attitude error
795  struct FloatRates rate_sp;
796 
797  // calculate the virtual control (reference acceleration) based on a PD controller
798  rate_sp.p = indi_gains.att.p * att_fb.x / indi_gains.rate.p;
799  rate_sp.q = indi_gains.att.q * att_fb.y / indi_gains.rate.q;
800  rate_sp.r = indi_gains.att.r * att_fb.z / indi_gains.rate.r;
801 
802  // Add feed-forward rates to the attitude feedback part
803  RATES_ADD(rate_sp, stab_att_ff_rates);
804 
805  // Store for telemetry
806  angular_rate_ref.p = rate_sp.p;
807  angular_rate_ref.q = rate_sp.q;
808  angular_rate_ref.r = rate_sp.r;
809 
810  // Possibly we can use some bounding here
811  /*BoundAbs(rate_sp.r, 5.0);*/
812 
813  /* compute the INDI command */
814  stabilization_indi_rate_run(rate_sp, in_flight);
815 
816  // Reset thrust increment boolean
818 }
819 
820 // This function reads rc commands
821 void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
822 {
823  struct FloatQuat q_sp;
824 #if USE_EARTH_BOUND_RC_SETPOINT
825  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
826 #else
827  stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
828 #endif
829 
831 }
832 
840 {
841 #if STABILIZATION_INDI_RPM_FEEDBACK
842  float_vect_copy(actuator_state, act_obs, INDI_NUM_ACT);
843 #else
844  //actuator dynamics
845  int8_t i;
846  float UNUSED prev_actuator_state;
847  for (i = 0; i < INDI_NUM_ACT; i++) {
848  prev_actuator_state = actuator_state[i];
849 
851  + act_dyn_discrete[i] * (indi_u[i] - actuator_state[i]);
852 
853 #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
854  if ((actuator_state[i] - prev_actuator_state) > act_rate_limit[i]) {
855  actuator_state[i] = prev_actuator_state + act_rate_limit[i];
856  } else if ((actuator_state[i] - prev_actuator_state) < -act_rate_limit[i]) {
857  actuator_state[i] = prev_actuator_state - act_rate_limit[i];
858  }
859 #endif
860  }
861 
862 #endif
863 }
864 
875 void calc_g1_element(float ddx_error, int8_t i, int8_t j, float mu)
876 {
877  g1_est[i][j] = g1_est[i][j] - du_estimation[j] * mu * ddx_error;
878 }
879 
889 void calc_g2_element(float ddx_error, int8_t j, float mu)
890 {
891  g2_est[j] = g2_est[j] - ddu_estimation[j] * mu * ddx_error;
892 }
893 
899 void lms_estimation(void)
900 {
901 
902  // Get the acceleration in body axes
903  struct Int32Vect3 *body_accel_i;
904  body_accel_i = stateGetAccelBody_i();
905  ACCELS_FLOAT_OF_BFP(body_accel_f, *body_accel_i);
906 
907  // Filter the acceleration in z axis
909 
910  // Calculate the derivative of the acceleration via finite difference
911  float indi_accel_d = (acceleration_lowpass_filter.o[0]
912  - acceleration_lowpass_filter.o[1]) * PERIODIC_FREQUENCY;
913 
914  // Use xml setting for adaptive mu for lms
915  // Set default value if not defined
916 #ifndef STABILIZATION_INDI_ADAPTIVE_MU
917  float adaptive_mu_lr = 0.001;
918 #else
919  float adaptive_mu_lr = STABILIZATION_INDI_ADAPTIVE_MU;
920 #endif
921 
922  // scale the inputs to avoid numerical errors
923  float_vect_smul(du_estimation, actuator_state_filt_vectd, adaptive_mu_lr, INDI_NUM_ACT);
924  float_vect_smul(ddu_estimation, actuator_state_filt_vectdd, adaptive_mu_lr / PERIODIC_FREQUENCY, INDI_NUM_ACT);
925 
926  float ddx_estimation[INDI_OUTPUTS] = {estimation_rate_dd[0], estimation_rate_dd[1], estimation_rate_dd[2], indi_accel_d};
927 
928  //Estimation of G
929  // TODO: only estimate when du_norm2 is large enough (enough input)
930  /*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];*/
931  int8_t i;
932  for (i = 0; i < INDI_OUTPUTS; i++) {
933  // Calculate the error between prediction and measurement
934  float ddx_error = - ddx_estimation[i];
935  int8_t j;
936  for (j = 0; j < INDI_NUM_ACT; j++) {
937  ddx_error += g1_est[i][j] * du_estimation[j];
938  if (i == 2) {
939  // Changing the momentum of the rotors gives a counter torque
940  ddx_error += g2_est[j] * ddu_estimation[j];
941  }
942  }
943 
944  // when doing the yaw axis, also use G2
945  if (i == 2) {
946  for (j = 0; j < INDI_NUM_ACT; j++) {
947  calc_g2_element(ddx_error, j, mu2);
948  }
949  } else if (i == 3) {
950  // If the acceleration change is very large (rough landing), don't adapt
951  if (fabs(indi_accel_d) > 60.0) {
952  ddx_error = 0.0;
953  }
954  }
955 
956  // Calculate the row of the G1 matrix corresponding to this axis
957  for (j = 0; j < INDI_NUM_ACT; j++) {
958  calc_g1_element(ddx_error, i, j, mu1[i]);
959  }
960  }
961 
962  bound_g_mat();
963 
964  // Save the calculated matrix to G1 and G2
965  // until thrust is included, first part of the array
966  float_vect_copy(g1[0], g1_est[0], INDI_OUTPUTS * INDI_NUM_ACT);
967  float_vect_copy(g2, g2_est, INDI_NUM_ACT);
968 
969  // Calculate sum of G1 and G2 for Bwls
970  sum_g1_g2();
971 
972 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
973  // Calculate the inverse of (G1+G2)
974  calc_g1g2_pseudo_inv();
975 #endif
976 }
977 
982 void sum_g1_g2(void)
983 {
984  int8_t i;
985  int8_t j;
986  for (i = 0; i < INDI_OUTPUTS; i++) {
987  for (j = 0; j < INDI_NUM_ACT; j++) {
988  if (i != 2) {
989  g1g2[i][j] = g1[i][j] / INDI_G_SCALING;
990  } else {
991  g1g2[i][j] = (g1[i][j] + g2[j]) / INDI_G_SCALING;
992  }
993  }
994  }
995 }
996 
997 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
1002 void calc_g1g2_pseudo_inv(void)
1003 {
1004  //G1G2*transpose(G1G2)
1005  //calculate matrix multiplication of its transpose INDI_OUTPUTSxnum_act x num_actxINDI_OUTPUTS
1006  float element = 0;
1007  int8_t row;
1008  int8_t col;
1009  int8_t i;
1010  for (row = 0; row < INDI_OUTPUTS; row++) {
1011  for (col = 0; col < INDI_OUTPUTS; col++) {
1012  element = 0;
1013  for (i = 0; i < INDI_NUM_ACT; i++) {
1014  element = element + g1g2[row][i] * g1g2[col][i];
1015  }
1016  g1g2_trans_mult[row][col] = element;
1017  }
1018  }
1019 
1020  //there are numerical errors if the scaling is not right.
1021  float_vect_scale(g1g2_trans_mult[0], 1000.0, INDI_OUTPUTS * INDI_OUTPUTS);
1022 
1023  //inverse of 4x4 matrix
1025 
1026  //scale back
1027  float_vect_scale(g1g2inv[0], 1000.0, INDI_OUTPUTS * INDI_OUTPUTS);
1028 
1029  //G1G2'*G1G2inv
1030  //calculate matrix multiplication INDI_NUM_ACTxINDI_OUTPUTS x INDI_OUTPUTSxINDI_OUTPUTS
1031  for (row = 0; row < INDI_NUM_ACT; row++) {
1032  for (col = 0; col < INDI_OUTPUTS; col++) {
1033  element = 0;
1034  for (i = 0; i < INDI_OUTPUTS; i++) {
1035  element = element + g1g2[i][row] * g1g2inv[col][i];
1036  }
1037  g1g2_pseudo_inv[row][col] = element;
1038  }
1039  }
1040 }
1041 #endif
1042 
1043 #if STABILIZATION_INDI_RPM_FEEDBACK
1044 static void act_feedback_cb(uint8_t sender_id UNUSED, struct act_feedback_t *feedback, uint8_t num_act)
1045 {
1046  int8_t i;
1047  for (i = 0; i < num_act; i++) {
1048  // Sanity check that index is valid
1049  if (feedback[i].idx < INDI_NUM_ACT && feedback[i].set.rpm) {
1050  int8_t idx = feedback[i].idx;
1051  act_obs[idx] = (feedback[i].rpm - get_servo_min(idx));
1052  act_obs[idx] *= (MAX_PPRZ / (float)(get_servo_max(idx) - get_servo_min(idx)));
1053  Bound(act_obs[idx], 0, MAX_PPRZ);
1054  }
1055  }
1056 }
1057 #endif
1058 
1062 static void thrust_cb(uint8_t UNUSED sender_id, struct FloatVect3 thrust_increment)
1063 {
1064  indi_thrust_increment = thrust_increment;
1066 }
1067 
1068 static void bound_g_mat(void)
1069 {
1070  int8_t i;
1071  int8_t j;
1072  for (j = 0; j < INDI_NUM_ACT; j++) {
1073  float max_limit;
1074  float min_limit;
1075 
1076  // Limit the values of the estimated G1 matrix
1077  for (i = 0; i < INDI_OUTPUTS; i++) {
1078  if (g1_init[i][j] > 0.0) {
1079  max_limit = g1_init[i][j] * INDI_ALLOWED_G_FACTOR;
1080  min_limit = g1_init[i][j] / INDI_ALLOWED_G_FACTOR;
1081  } else {
1082  max_limit = g1_init[i][j] / INDI_ALLOWED_G_FACTOR;
1083  min_limit = g1_init[i][j] * INDI_ALLOWED_G_FACTOR;
1084  }
1085 
1086  if (g1_est[i][j] > max_limit) {
1087  g1_est[i][j] = max_limit;
1088  }
1089  if (g1_est[i][j] < min_limit) {
1090  g1_est[i][j] = min_limit;
1091  }
1092  }
1093 
1094  // Do the same for the G2 matrix
1095  if (g2_init[j] > 0.0) {
1096  max_limit = g2_init[j] * INDI_ALLOWED_G_FACTOR;
1097  min_limit = g2_init[j] / INDI_ALLOWED_G_FACTOR;
1098  } else {
1099  max_limit = g2_init[j] / INDI_ALLOWED_G_FACTOR;
1100  min_limit = g2_init[j] * INDI_ALLOWED_G_FACTOR;
1101  }
1102 
1103  if (g2_est[j] > max_limit) {
1104  g2_est[j] = max_limit;
1105  }
1106  if (g2_est[j] < min_limit) {
1107  g2_est[j] = min_limit;
1108  }
1109  }
1110 }
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
#define THRUST_INCREMENT_ID
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:290
uint8_t last_wp UNUSED
float q
in rad/s
float p
in rad/s
float r
in rad/s
static void float_vect_sum(float *o, const float *a, const float *b, const int n)
o = a + b
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_vect_scale(float *a, const float s, const int n)
a *= s
static void float_vect_copy(float *a, const float *b, const int n)
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
#define FLOAT_RATES_ZERO(_r)
Roation quaternion.
angular rates
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:752
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:344
#define QUAT_FLOAT_OF_BFP(_qf, _qi)
Definition: pprz_algebra.h:745
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
#define INT32_TRIG_FRAC
void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e)
Quaternion from Euler angles.
euler angles
Rotation quaternion.
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1113
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
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
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
static float use_increment
Definition: oneloop_andi.c:409
#define MAX_PPRZ
Definition: paparazzi.h:8
static abi_event act_feedback_ev
Paparazzi floating point algebra.
#define PPRZ_ITRIG_SIN(_s, _a)
#define PPRZ_ITRIG_COS(_c, _a)
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:50
#define GUIDANCE_H_MODE_NAV
Definition: guidance_h.h:60
#define GUIDANCE_H_MODE_HOVER
Definition: guidance_h.h:59
General attitude stabilization interface for rotorcrafts.
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:34
struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp)
struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp)
struct Int32Eulers stab_sp_to_eulers_i(struct StabilizationSetpoint *sp)
void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading)
Quaternion transformation functions.
int32_t stabilization_attitude_get_heading_i(void)
void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree, bool coordinated_turn)
void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree, bool coordinated_turn)
Read attitude setpoint from RC as quaternion Interprets the stick positions as axes.
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_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
void stabilization_indi_enter(void)
Function that resets important values upon engaging INDI.
static float Wv[INDI_OUTPUTS]
float act_pref[INDI_NUM_ACT]
float * Bwls[INDI_OUTPUTS]
int32_t stabilization_att_indi_cmd[COMMANDS_NB]
struct Indi_gains indi_gains
#define STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD
void stabilization_indi_set_failsafe_setpoint(void)
Function that calculates the failsafe setpoint.
bool act_is_thruster_x[INDI_NUM_ACT]
float du_min_stab_indi[INDI_NUM_ACT]
bool act_is_servo[INDI_NUM_ACT]
void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
static struct FirstOrderLowPass rates_filt_fo[3]
float angular_acceleration[3]
float du_pref_stab_indi[INDI_NUM_ACT]
int32_t num_thrusters
void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
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 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
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
float du_max_stab_indi[INDI_NUM_ACT]
void WEAK stabilization_indi_set_wls_settings(float use_increment)
float estimation_rate_d[INDI_NUM_ACT]
float du_estimation[INDI_NUM_ACT]
float actuator_state_filt_vectdd[INDI_NUM_ACT]
static void thrust_cb(uint8_t sender_id, struct FloatVect3 thrust_increment)
abi_event thrust_ev
Butterworth2LowPass measurement_lowpass_filters[3]
bool act_is_thruster_z[INDI_NUM_ACT]
bool indi_thrust_increment_set
void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
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]
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
void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp)
Set attitude setpoint from stabilization setpoint struct.
float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS]
void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
float actuator_state_filt_vect[INDI_NUM_ACT]
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]
float g2_times_du
struct FloatVect3 indi_thrust_increment
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]
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
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]
void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat)
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]
struct FloatRates stab_att_ff_rates
void init_filters(void)
Function that resets the filters to zeros.
Butterworth2LowPass estimation_output_lowpass_filters[3]
float indi_du[INDI_NUM_ACT]
#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:42
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
float heading
Definition: wedgebug.c:258
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