Paparazzi UAS  v6.2_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 "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 float du_min[INDI_NUM_ACT];
70 float du_max[INDI_NUM_ACT];
71 float du_pref[INDI_NUM_ACT];
72 float indi_v[INDI_OUTPUTS];
73 float *Bwls[INDI_OUTPUTS];
74 int num_iter = 0;
75 
76 static void lms_estimation(void);
77 static void get_actuator_state(void);
78 static void calc_g1_element(float dx_error, int8_t i, int8_t j, float mu_extra);
79 static void calc_g2_element(float dx_error, int8_t j, float mu_extra);
80 static void calc_g1g2_pseudo_inv(void);
81 static void bound_g_mat(void);
82 
85  .att = {
86  STABILIZATION_INDI_REF_ERR_P,
87  STABILIZATION_INDI_REF_ERR_Q,
88  STABILIZATION_INDI_REF_ERR_R
89  },
90  .rate = {
91  STABILIZATION_INDI_REF_RATE_P,
92  STABILIZATION_INDI_REF_RATE_Q,
93  STABILIZATION_INDI_REF_RATE_R
94  },
95 };
96 
97 #if STABILIZATION_INDI_USE_ADAPTIVE
98 bool indi_use_adaptive = true;
99 #else
100 bool indi_use_adaptive = false;
101 #endif
102 
103 #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
104 float act_rate_limit[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_RATE_LIMIT;
105 #endif
106 
107 #ifdef STABILIZATION_INDI_ACT_IS_SERVO
108 bool act_is_servo[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO;
109 #else
110 bool act_is_servo[INDI_NUM_ACT] = {0};
111 #endif
112 
113 #ifdef STABILIZATION_INDI_ACT_PREF
114 // Preferred (neutral, least energy) actuator value
115 float act_pref[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_PREF;
116 #else
117 // Assume 0 is neutral
118 float act_pref[INDI_NUM_ACT] = {0.0};
119 #endif
120 
121 float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
122 
123 #ifdef STABILIZATION_INDI_WLS_PRIORITIES
124 static float Wv[INDI_OUTPUTS] = STABILIZATION_INDI_WLS_PRIORITIES;
125 #else
126 //State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
127 static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100};
128 #endif
129 
130 // variables needed for control
131 float actuator_state_filt_vect[INDI_NUM_ACT];
132 struct FloatRates angular_accel_ref = {0., 0., 0.};
133 float angular_acceleration[3] = {0., 0., 0.};
134 float actuator_state[INDI_NUM_ACT];
135 float indi_u[INDI_NUM_ACT];
136 float indi_du[INDI_NUM_ACT];
138 
139 float q_filt = 0.0;
140 float r_filt = 0.0;
141 
142 // variables needed for estimation
143 float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS];
144 float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS];
145 float actuator_state_filt_vectd[INDI_NUM_ACT];
146 float actuator_state_filt_vectdd[INDI_NUM_ACT];
147 float estimation_rate_d[INDI_NUM_ACT];
148 float estimation_rate_dd[INDI_NUM_ACT];
149 float du_estimation[INDI_NUM_ACT];
150 float ddu_estimation[INDI_NUM_ACT];
151 
152 // The learning rate per axis (roll, pitch, yaw, thrust)
153 float mu1[INDI_OUTPUTS] = {0.00001, 0.00001, 0.000003, 0.000002};
154 // The learning rate for the propeller inertia (scaled by 512 wrt mu1)
155 float mu2 = 0.002;
156 
157 // other variables
158 float act_obs[INDI_NUM_ACT];
159 
160 // Number of actuators used to provide thrust
162 
165 
167 static void rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act);
168 
170 static void thrust_cb(uint8_t sender_id, float thrust_increment);
173 
174 float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS];
175 float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
176 float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
177  STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
178  };
179 float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
180 float g1_est[INDI_OUTPUTS][INDI_NUM_ACT];
181 float g2_est[INDI_NUM_ACT];
182 float g1_init[INDI_OUTPUTS][INDI_NUM_ACT];
183 float g2_init[INDI_NUM_ACT];
184 
191 
193 
194 void init_filters(void);
195 
196 #if PERIODIC_TELEMETRY
198 static void send_indi_g(struct transport_tx *trans, struct link_device *dev)
199 {
200  pprz_msg_send_INDI_G(trans, dev, AC_ID, INDI_NUM_ACT, g1_est[0],
201  INDI_NUM_ACT, g1_est[1],
202  INDI_NUM_ACT, g1_est[2],
203  INDI_NUM_ACT, g1_est[3],
204  INDI_NUM_ACT, g2_est);
205 }
206 
207 static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
208 {
209  struct Int32Quat *quat = stateGetNedToBodyQuat_i();
210  pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
215  &(quat->qi),
216  &(quat->qx),
217  &(quat->qy),
218  &(quat->qz));
219 }
220 #endif
221 
226 {
227  // Initialize filters
228  init_filters();
229 
230  AbiBindMsgRPM(RPM_SENSOR_ID, &rpm_ev, rpm_cb);
231  AbiBindMsgTHRUST(THRUST_INCREMENT_ID, &thrust_ev, thrust_cb);
232 
235  float_vect_zero(estimation_rate_d, INDI_NUM_ACT);
236  float_vect_zero(estimation_rate_dd, INDI_NUM_ACT);
238 
239  //Calculate G1G2_PSEUDO_INVERSE
241 
242  // Initialize the array of pointers to the rows of g1g2
243  uint8_t i;
244  for (i = 0; i < INDI_OUTPUTS; i++) {
245  Bwls[i] = g1g2[i];
246  }
247 
248  // Initialize the estimator matrices
249  float_vect_copy(g1_est[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
250  float_vect_copy(g2_est, g2, INDI_NUM_ACT);
251  // Remember the initial matrices
252  float_vect_copy(g1_init[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
253  float_vect_copy(g2_init, g2, INDI_NUM_ACT);
254 
255  // Assume all non-servos are delivering thrust
256  num_thrusters = INDI_NUM_ACT;
257  for (i = 0; i < INDI_NUM_ACT; i++) {
259  }
260 
261 #if PERIODIC_TELEMETRY
264 #endif
265 }
266 
276 {
277  /* reset psi setpoint to current psi angle */
279 
280  float_vect_zero(du_estimation, INDI_NUM_ACT);
281  float_vect_zero(ddu_estimation, INDI_NUM_ACT);
282 }
283 
287 void init_filters(void)
288 {
289  // tau = 1/(2*pi*Fc)
290  float tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF);
291  float tau_est = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF);
292  float sample_time = 1.0 / PERIODIC_FREQUENCY;
293  // Filtering of the gyroscope
294  int8_t i;
295  for (i = 0; i < 3; i++) {
296  init_butterworth_2_low_pass(&measurement_lowpass_filters[i], tau, sample_time, 0.0);
297  init_butterworth_2_low_pass(&estimation_output_lowpass_filters[i], tau_est, sample_time, 0.0);
298  }
299 
300  // Filtering of the actuators
301  for (i = 0; i < INDI_NUM_ACT; i++) {
302  init_butterworth_2_low_pass(&actuator_lowpass_filters[i], tau, sample_time, 0.0);
303  init_butterworth_2_low_pass(&estimation_input_lowpass_filters[i], tau_est, sample_time, 0.0);
304  }
305 
306  // Filtering of the accel body z
307  init_butterworth_2_low_pass(&acceleration_lowpass_filter, tau_est, sample_time, 0.0);
308 
309  // Init rate filter for feedback
310  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)};
311 
312  init_first_order_low_pass(&rates_filt_fo[0], time_constants[0], sample_time, stateGetBodyRates_f()->p);
313  init_first_order_low_pass(&rates_filt_fo[1], time_constants[1], sample_time, stateGetBodyRates_f()->q);
314  init_first_order_low_pass(&rates_filt_fo[2], time_constants[2], sample_time, stateGetBodyRates_f()->r);
315 }
316 
321 {
322  /* set failsafe to zero roll/pitch and current heading */
325  stab_att_sp_quat.qx = 0;
326  stab_att_sp_quat.qy = 0;
328 }
329 
336 {
337  // stab_att_sp_euler.psi still used in ref..
338  stab_att_sp_euler = *rpy;
339 
341 }
342 
350 {
351  // stab_att_sp_euler.psi still used in ref..
353 
354  // compute sp_euler phi/theta for debugging/telemetry
355  /* Rotate horizontal commands to body frame by psi */
357  int32_t s_psi, c_psi;
358  PPRZ_ITRIG_SIN(s_psi, psi);
359  PPRZ_ITRIG_COS(c_psi, psi);
360  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
361  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
362 
364 }
365 
373 void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
374 {
375  /* Propagate the filter on the gyroscopes */
376  struct FloatRates *body_rates = stateGetBodyRates_f();
377  float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r};
378  int8_t i;
379  for (i = 0; i < 3; i++) {
382 
383  //Calculate the angular acceleration via finite difference
385  - measurement_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
386 
387  // Calculate derivatives for estimation
388  float estimation_rate_d_prev = estimation_rate_d[i];
390  PERIODIC_FREQUENCY;
391  estimation_rate_dd[i] = (estimation_rate_d[i] - estimation_rate_d_prev) * PERIODIC_FREQUENCY;
392  }
393 
394  //The rates used for feedback are by default the measured rates.
395  //If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.
396  //Note that due to the delay, the PD controller may need relaxed gains.
397  struct FloatRates rates_filt;
398 #if STABILIZATION_INDI_FILTER_ROLL_RATE
399  rates_filt.p = update_first_order_low_pass(&rates_filt_fo[0], body_rates->p);
400 #else
401  rates_filt.p = body_rates->p;
402 #endif
403 #if STABILIZATION_INDI_FILTER_PITCH_RATE
404  rates_filt.q = update_first_order_low_pass(&rates_filt_fo[1], body_rates->q);
405 #else
406  rates_filt.q = body_rates->q;
407 #endif
408 #if STABILIZATION_INDI_FILTER_YAW_RATE
409  rates_filt.r = update_first_order_low_pass(&rates_filt_fo[2], body_rates->r);
410 #else
411  rates_filt.r = body_rates->r;
412 #endif
413 
414  //calculate the virtual control (reference acceleration) based on a PD controller
415  angular_accel_ref.p = (rate_sp.p - rates_filt.p) * indi_gains.rate.p;
416  angular_accel_ref.q = (rate_sp.q - rates_filt.q) * indi_gains.rate.q;
417  angular_accel_ref.r = (rate_sp.r - rates_filt.r) * indi_gains.rate.r;
418 
419  g2_times_du = 0.0;
420  for (i = 0; i < INDI_NUM_ACT; i++) {
421  g2_times_du += g2[i] * indi_du[i];
422  }
423  //G2 is scaled by INDI_G_SCALING to make it readable
425 
426  float v_thrust = 0.0;
427  if (indi_thrust_increment_set && in_flight) {
428  v_thrust = indi_thrust_increment;
429 
430  //update thrust command such that the current is correctly estimated
431  stabilization_cmd[COMMAND_THRUST] = 0;
432  for (i = 0; i < INDI_NUM_ACT; i++) {
433  stabilization_cmd[COMMAND_THRUST] += actuator_state[i] * -((int32_t) act_is_servo[i] - 1);
434  }
435  stabilization_cmd[COMMAND_THRUST] /= num_thrusters;
436 
437  } else {
438  // incremental thrust
439  for (i = 0; i < INDI_NUM_ACT; i++) {
440  v_thrust +=
441  (stabilization_cmd[COMMAND_THRUST] - actuator_state_filt_vect[i]) * Bwls[3][i];
442  }
443  }
444 
445  // The control objective in array format
449  indi_v[3] = v_thrust;
450 
451  if (in_flight) {
452 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
453  // Calculate the increment for each actuator
454  for (i = 0; i < INDI_NUM_ACT; i++) {
455  indi_du[i] = (g1g2_pseudo_inv[i][0] * indi_v[0])
456  + (g1g2_pseudo_inv[i][1] * indi_v[1])
457  + (g1g2_pseudo_inv[i][2] * indi_v[2])
458  + (g1g2_pseudo_inv[i][3] * indi_v[3]);
459  }
460 #else
461  // Calculate the min and max increments
462  for (i = 0; i < INDI_NUM_ACT; i++) {
466 
467 #ifdef GUIDANCE_INDI_MIN_THROTTLE
468  float airspeed = stateGetAirspeed_f();
469  //limit minimum thrust ap can give
470  if (!act_is_servo[i]) {
472  if (airspeed < 8.0) {
473  du_min[i] = GUIDANCE_INDI_MIN_THROTTLE - actuator_state_filt_vect[i];
474  } else {
475  du_min[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD - actuator_state_filt_vect[i];
476  }
477  }
478  }
479 #endif
480  }
481 
482  // WLS Control Allocator
483  num_iter =
484  wls_alloc(indi_du, indi_v, du_min, du_max, Bwls, 0, 0, Wv, 0, du_pref, 10000, 10);
485 #endif
486 
487  // Add the increments to the actuators
489 
490  // Bound the inputs to the actuators
491  for (i = 0; i < INDI_NUM_ACT; i++) {
492  if (act_is_servo[i]) {
493  BoundAbs(indi_u[i], MAX_PPRZ);
494  } else {
495  if (autopilot_get_motors_on()) {
496  Bound(indi_u[i], 0, MAX_PPRZ);
497  }
498  else {
499  indi_u[i] = -MAX_PPRZ;
500  }
501  }
502  }
503 
504  } else {
505  //Don't increment if not flying (not armed)
506  float_vect_zero(indi_u, INDI_NUM_ACT);
507  float_vect_zero(indi_du, INDI_NUM_ACT);
508  }
509 
510  // Propagate actuator filters
512  for (i = 0; i < INDI_NUM_ACT; i++) {
516 
517  // calculate derivatives for estimation
518  float actuator_state_filt_vectd_prev = actuator_state_filt_vectd[i];
520  PERIODIC_FREQUENCY;
521  actuator_state_filt_vectdd[i] = (actuator_state_filt_vectd[i] - actuator_state_filt_vectd_prev) * PERIODIC_FREQUENCY;
522  }
523 
524  // Use online effectiveness estimation only when flying
525  if (in_flight && indi_use_adaptive) {
526  lms_estimation();
527  }
528 
529  /*Commit the actuator command*/
530  for (i = 0; i < INDI_NUM_ACT; i++) {
531  actuators_pprz[i] = (int16_t) indi_u[i];
532  }
533 
534  // Set the stab_cmd to 42 to indicate that it is not used
535  stabilization_cmd[COMMAND_ROLL] = 42;
536  stabilization_cmd[COMMAND_PITCH] = 42;
537  stabilization_cmd[COMMAND_YAW] = 42;
538 }
539 
546 void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
547 {
548  /* attitude error */
549  struct FloatQuat att_err;
550  struct FloatQuat *att_quat = stateGetNedToBodyQuat_f();
551  struct FloatQuat quat_sp_f;
552 
553  QUAT_FLOAT_OF_BFP(quat_sp_f, quat_sp);
554  float_quat_inv_comp_norm_shortest(&att_err, att_quat, &quat_sp_f);
555 
556  struct FloatVect3 att_fb;
557 
558 #if TILT_TWIST_CTRL
559  struct FloatQuat tilt;
560  struct FloatQuat twist;
561  float_quat_tilt_twist(&tilt, &twist, &att_err);
562  att_fb.x = tilt.qx;
563  att_fb.y = tilt.qy;
564  att_fb.z = twist.qz;
565 #else
566  att_fb.x = att_err.qx;
567  att_fb.y = att_err.qy;
568  att_fb.z = att_err.qz;
569 #endif
570 
571  // local variable to compute rate setpoints based on attitude error
572  struct FloatRates rate_sp;
573 
574  // calculate the virtual control (reference acceleration) based on a PD controller
575  rate_sp.p = indi_gains.att.p * att_fb.x / indi_gains.rate.p;
576  rate_sp.q = indi_gains.att.q * att_fb.y / indi_gains.rate.q;
577  rate_sp.r = indi_gains.att.r * att_fb.z / indi_gains.rate.r;
578 
579  // Possibly we can use some bounding here
580  /*BoundAbs(rate_sp.r, 5.0);*/
581 
582  /* compute the INDI command */
583  stabilization_indi_rate_run(rate_sp, in_flight);
584 
585  // Reset thrust increment boolean
587 }
588 
589 // This function reads rc commands
590 void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
591 {
592  struct FloatQuat q_sp;
593 #if USE_EARTH_BOUND_RC_SETPOINT
594  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
595 #else
596  stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
597 #endif
598 
600 }
601 
609 {
610 #if INDI_RPM_FEEDBACK
611  float_vect_copy(actuator_state, act_obs, INDI_NUM_ACT);
612 #else
613  //actuator dynamics
614  int8_t i;
615  float UNUSED prev_actuator_state;
616  for (i = 0; i < INDI_NUM_ACT; i++) {
617  prev_actuator_state = actuator_state[i];
618 
620  + act_dyn[i] * (indi_u[i] - actuator_state[i]);
621 
622 #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
623  if ((actuator_state[i] - prev_actuator_state) > act_rate_limit[i]) {
624  actuator_state[i] = prev_actuator_state + act_rate_limit[i];
625  } else if ((actuator_state[i] - prev_actuator_state) < -act_rate_limit[i]) {
626  actuator_state[i] = prev_actuator_state - act_rate_limit[i];
627  }
628 #endif
629  }
630 
631 #endif
632 }
633 
644 void calc_g1_element(float ddx_error, int8_t i, int8_t j, float mu)
645 {
646  g1_est[i][j] = g1_est[i][j] - du_estimation[j] * mu * ddx_error;
647 }
648 
658 void calc_g2_element(float ddx_error, int8_t j, float mu)
659 {
660  g2_est[j] = g2_est[j] - ddu_estimation[j] * mu * ddx_error;
661 }
662 
668 void lms_estimation(void)
669 {
670 
671  // Get the acceleration in body axes
672  struct Int32Vect3 *body_accel_i;
673  body_accel_i = stateGetAccelBody_i();
674  ACCELS_FLOAT_OF_BFP(body_accel_f, *body_accel_i);
675 
676  // Filter the acceleration in z axis
678 
679  // Calculate the derivative of the acceleration via finite difference
680  float indi_accel_d = (acceleration_lowpass_filter.o[0]
681  - acceleration_lowpass_filter.o[1]) * PERIODIC_FREQUENCY;
682 
683  // Use xml setting for adaptive mu for lms
684  // Set default value if not defined
685  #ifndef STABILIZATION_INDI_ADAPTIVE_MU
686  float adaptive_mu_lr = 0.001;
687  #else
688  float adaptive_mu_lr = STABILIZATION_INDI_ADAPTIVE_MU;
689  #endif
690 
691  // scale the inputs to avoid numerical errors
692  float_vect_smul(du_estimation, actuator_state_filt_vectd, adaptive_mu_lr, INDI_NUM_ACT);
693  float_vect_smul(ddu_estimation, actuator_state_filt_vectdd, adaptive_mu_lr / PERIODIC_FREQUENCY, INDI_NUM_ACT);
694 
695  float ddx_estimation[INDI_OUTPUTS] = {estimation_rate_dd[0], estimation_rate_dd[1], estimation_rate_dd[2], indi_accel_d};
696 
697  //Estimation of G
698  // TODO: only estimate when du_norm2 is large enough (enough input)
699  /*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];*/
700  int8_t i;
701  for (i = 0; i < INDI_OUTPUTS; i++) {
702  // Calculate the error between prediction and measurement
703  float ddx_error = - ddx_estimation[i];
704  int8_t j;
705  for (j = 0; j < INDI_NUM_ACT; j++) {
706  ddx_error += g1_est[i][j] * du_estimation[j];
707  if (i == 2) {
708  // Changing the momentum of the rotors gives a counter torque
709  ddx_error += g2_est[j] * ddu_estimation[j];
710  }
711  }
712 
713  // when doing the yaw axis, also use G2
714  if (i == 2) {
715  for (j = 0; j < INDI_NUM_ACT; j++) {
716  calc_g2_element(ddx_error, j, mu2);
717  }
718  } else if (i == 3) {
719  // If the acceleration change is very large (rough landing), don't adapt
720  if (fabs(indi_accel_d) > 60.0) {
721  ddx_error = 0.0;
722  }
723  }
724 
725  // Calculate the row of the G1 matrix corresponding to this axis
726  for (j = 0; j < INDI_NUM_ACT; j++) {
727  calc_g1_element(ddx_error, i, j, mu1[i]);
728  }
729  }
730 
731  bound_g_mat();
732 
733  // Save the calculated matrix to G1 and G2
734  // until thrust is included, first part of the array
735  float_vect_copy(g1[0], g1_est[0], INDI_OUTPUTS * INDI_NUM_ACT);
736  float_vect_copy(g2, g2_est, INDI_NUM_ACT);
737 
738 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
739  // Calculate the inverse of (G1+G2)
741 #endif
742 }
743 
748 {
749 
750  //sum of G1 and G2
751  int8_t i;
752  int8_t j;
753  for (i = 0; i < INDI_OUTPUTS; i++) {
754  for (j = 0; j < INDI_NUM_ACT; j++) {
755  if (i != 2) {
756  g1g2[i][j] = g1[i][j] / INDI_G_SCALING;
757  } else {
758  g1g2[i][j] = (g1[i][j] + g2[j]) / INDI_G_SCALING;
759  }
760  }
761  }
762 
763  //G1G2*transpose(G1G2)
764  //calculate matrix multiplication of its transpose INDI_OUTPUTSxnum_act x num_actxINDI_OUTPUTS
765  float element = 0;
766  int8_t row;
767  int8_t col;
768  for (row = 0; row < INDI_OUTPUTS; row++) {
769  for (col = 0; col < INDI_OUTPUTS; col++) {
770  element = 0;
771  for (i = 0; i < INDI_NUM_ACT; i++) {
772  element = element + g1g2[row][i] * g1g2[col][i];
773  }
774  g1g2_trans_mult[row][col] = element;
775  }
776  }
777 
778  //there are numerical errors if the scaling is not right.
779  float_vect_scale(g1g2_trans_mult[0], 100.0, INDI_OUTPUTS * INDI_OUTPUTS);
780 
781  //inverse of 4x4 matrix
783 
784  //scale back
785  float_vect_scale(g1g2inv[0], 100.0, INDI_OUTPUTS * INDI_OUTPUTS);
786 
787  //G1G2'*G1G2inv
788  //calculate matrix multiplication INDI_NUM_ACTxINDI_OUTPUTS x INDI_OUTPUTSxINDI_OUTPUTS
789  for (row = 0; row < INDI_NUM_ACT; row++) {
790  for (col = 0; col < INDI_OUTPUTS; col++) {
791  element = 0;
792  for (i = 0; i < INDI_OUTPUTS; i++) {
793  element = element + g1g2[i][row] * g1g2inv[col][i];
794  }
795  g1g2_pseudo_inv[row][col] = element;
796  }
797  }
798 }
799 
800 static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t UNUSED *rpm, uint8_t UNUSED num_act)
801 {
802 #if INDI_RPM_FEEDBACK
803  int8_t i;
804  for (i = 0; i < num_act; i++) {
805  act_obs[i] = (rpm[i] - get_servo_min(i));
806  act_obs[i] *= (MAX_PPRZ / (float)(get_servo_max(i) - get_servo_min(i)));
807  Bound(act_obs[i], 0, MAX_PPRZ);
808  }
809 #endif
810 }
811 
815 static void thrust_cb(uint8_t UNUSED sender_id, float thrust_increment)
816 {
817  indi_thrust_increment = thrust_increment;
819 }
820 
821 static void bound_g_mat(void)
822 {
823  int8_t i;
824  int8_t j;
825  for (j = 0; j < INDI_NUM_ACT; j++) {
826  float max_limit;
827  float min_limit;
828 
829  // Limit the values of the estimated G1 matrix
830  for (i = 0; i < INDI_OUTPUTS; i++) {
831  if (g1_init[i][j] > 0.0) {
832  max_limit = g1_init[i][j] * INDI_ALLOWED_G_FACTOR;
833  min_limit = g1_init[i][j] / INDI_ALLOWED_G_FACTOR;
834  } else {
835  max_limit = g1_init[i][j] / INDI_ALLOWED_G_FACTOR;
836  min_limit = g1_init[i][j] * INDI_ALLOWED_G_FACTOR;
837  }
838 
839  if (g1_est[i][j] > max_limit) {
840  g1_est[i][j] = max_limit;
841  }
842  if (g1_est[i][j] < min_limit) {
843  g1_est[i][j] = min_limit;
844  }
845  }
846 
847  // Do the same for the G2 matrix
848  if (g2_init[j] > 0.0) {
849  max_limit = g2_init[j] * INDI_ALLOWED_G_FACTOR;
850  min_limit = g2_init[j] / INDI_ALLOWED_G_FACTOR;
851  } else {
852  max_limit = g2_init[j] / INDI_ALLOWED_G_FACTOR;
853  min_limit = g2_init[j] * INDI_ALLOWED_G_FACTOR;
854  }
855 
856  if (g2_est[j] > max_limit) {
857  g2_est[j] = max_limit;
858  }
859  if (g2_est[j] < min_limit) {
860  g2_est[j] = min_limit;
861  }
862  }
863 }
int32_quat_of_eulers
void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e)
Quaternion from Euler angles.
Definition: pprz_algebra_int.c:375
Int32Eulers::theta
int32_t theta
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:148
FloatQuat::qz
float qz
Definition: pprz_algebra_float.h:67
autopilot_get_motors_on
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:244
send_indi_g
static void send_indi_g(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_indi.c:198
radio_control.h
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
mu
static float mu
Physical model parameters.
Definition: nps_fdm_rover.c:60
send_ahrs_ref_quat
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_indi.c:207
update_butterworth_2_low_pass
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
Definition: low_pass_filter.h:291
int8_t
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
g1g2
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]
Definition: stabilization_indi.c:179
indi_du
float indi_du[INDI_NUM_ACT]
Definition: stabilization_indi.c:136
indi_gains
struct Indi_gains indi_gains
Definition: stabilization_indi.c:84
stabilization_attitude_read_rc_setpoint_quat_earth_bound_f
void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree, bool coordinated_turn)
Definition: stabilization_attitude_rc_setpoint.c:427
stabilization_indi_init
void stabilization_indi_init(void)
Function that initializes important values upon engaging INDI.
Definition: stabilization_indi.c:225
du_pref
float du_pref[INDI_NUM_ACT]
Definition: stabilization_indi.c:71
indi_thrust_increment_set
bool indi_thrust_increment_set
Definition: stabilization_indi.c:172
stabilization_attitude.h
quat_from_earth_cmd_i
void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading)
Definition: stabilization_attitude_quat_transformations.c:48
THRUST_INCREMENT_ID
#define THRUST_INCREMENT_ID
Definition: abi_sender_ids.h:459
act_obs
float act_obs[INDI_NUM_ACT]
Definition: stabilization_indi.c:158
acceleration_lowpass_filter
Butterworth2LowPass acceleration_lowpass_filter
Definition: stabilization_indi.c:189
bound_g_mat
static void bound_g_mat(void)
Definition: stabilization_indi.c:821
estimation_rate_dd
float estimation_rate_dd[INDI_NUM_ACT]
Definition: stabilization_indi.c:148
abi.h
Bwls
float * Bwls[INDI_OUTPUTS]
Definition: stabilization_indi.c:73
STABILIZATION_INDI_FILT_CUTOFF_Q
#define STABILIZATION_INDI_FILT_CUTOFF_Q
Definition: stabilization_indi.c:60
du_min
float du_min[INDI_NUM_ACT]
Definition: stabilization_indi.c:69
thrust_ev
abi_event thrust_ev
Definition: stabilization_indi.c:169
INDI_ALLOWED_G_FACTOR
#define INDI_ALLOWED_G_FACTOR
Definition: stabilization_indi.c:49
Int32Quat
Rotation quaternion.
Definition: pprz_algebra_int.h:99
Int32Vect2::y
int32_t y
Definition: pprz_algebra_int.h:85
stateGetNedToBodyQuat_i
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1113
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
indi_thrust_increment
float indi_thrust_increment
Definition: stabilization_indi.c:171
init_first_order_low_pass
static void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, float sample_time, float value)
Init first order low pass filter.
Definition: low_pass_filter.h:57
update_first_order_low_pass
static float update_first_order_low_pass(struct FirstOrderLowPass *filter, float value)
Update first order low pass filter state with a new value.
Definition: low_pass_filter.h:71
stateGetBodyRates_f
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
ddu_estimation
float ddu_estimation[INDI_NUM_ACT]
Definition: stabilization_indi.c:150
PPRZ_ITRIG_COS
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:112
float_mat_inv_4d
bool float_mat_inv_4d(float invOut[16], float mat_in[16])
4x4 Matrix inverse
Definition: pprz_algebra_float.c:863
actuator_lowpass_filters
Butterworth2LowPass actuator_lowpass_filters[INDI_NUM_ACT]
Definition: stabilization_indi.c:185
UNUSED
uint8_t last_wp UNUSED
Definition: navigation.c:101
SecondOrderLowPass
Second order low pass filter structure.
Definition: low_pass_filter.h:136
g1_est
float g1_est[INDI_OUTPUTS][INDI_NUM_ACT]
Definition: stabilization_indi.c:180
estimation_rate_d
float estimation_rate_d[INDI_NUM_ACT]
Definition: stabilization_indi.c:147
g2_est
float g2_est[INDI_NUM_ACT]
Definition: stabilization_indi.c:181
actuator_state_filt_vectdd
float actuator_state_filt_vectdd[INDI_NUM_ACT]
Definition: stabilization_indi.c:146
pprz_algebra_float.h
Paparazzi floating point algebra.
calc_g1g2_pseudo_inv
static void calc_g1g2_pseudo_inv(void)
Function that calculates the pseudo-inverse of (G1+G2).
Definition: stabilization_indi.c:747
stab_att_sp_quat
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
Definition: stabilization_indi.c:164
indi_v
float indi_v[INDI_OUTPUTS]
Definition: stabilization_indi.c:72
stabilization_attitude_get_heading_i
int32_t stabilization_attitude_get_heading_i(void)
Definition: stabilization_attitude_rc_setpoint.c:130
stabilization_indi_set_failsafe_setpoint
void stabilization_indi_set_failsafe_setpoint(void)
Function that calculates the failsafe setpoint.
Definition: stabilization_indi.c:320
rpm
uint16_t rpm
Definition: rpm_sensor.c:33
calc_g1_element
static void calc_g1_element(float dx_error, int8_t i, int8_t j, float mu_extra)
Definition: stabilization_indi.c:644
STABILIZATION_INDI_FILT_CUTOFF_P
#define STABILIZATION_INDI_FILT_CUTOFF_P
Definition: stabilization_indi.c:54
STABILIZATION_INDI_FILT_CUTOFF_R
#define STABILIZATION_INDI_FILT_CUTOFF_R
Definition: stabilization_indi.c:66
wls_alloc
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)
active set algorithm for control allocation
Definition: wls_alloc.c:112
stabilization_indi_rate_run
void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
Definition: stabilization_indi.c:373
stabilization_indi.h
stabilization_indi_set_rpy_setpoint_i
void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
Definition: stabilization_indi.c:335
SecondOrderLowPass::o
float o[2]
output history
Definition: low_pass_filter.h:140
q_filt
float q_filt
Definition: stabilization_indi.c:139
FloatVect3
Definition: pprz_algebra_float.h:54
telemetry.h
int16_t
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
stabilization_indi_enter
void stabilization_indi_enter(void)
Function that resets important values upon engaging INDI.
Definition: stabilization_indi.c:275
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
estimation_input_lowpass_filters
Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT]
Definition: stabilization_indi.c:186
Int32Vect2::x
int32_t x
Definition: pprz_algebra_int.h:84
Int32Eulers::psi
int32_t psi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:149
stabilization_att_indi_cmd
int32_t stabilization_att_indi_cmd[COMMANDS_NB]
Definition: stabilization_indi.c:83
RPM_SENSOR_ID
#define RPM_SENSOR_ID
Definition: abi_sender_ids.h:452
Int32Eulers::phi
int32_t phi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:147
GUIDANCE_H_MODE_HOVER
#define GUIDANCE_H_MODE_HOVER
Definition: guidance_h.h:59
g2_times_du
float g2_times_du
Definition: stabilization_indi.c:137
stateGetNedToBodyQuat_f
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
g2
float g2[INDI_NUM_ACT]
Definition: stabilization_indi.c:175
HorizontalGuidance::mode
uint8_t mode
Definition: guidance_h.h:96
Indi_gains::rate
struct FloatRates rate
Definition: stabilization_indi.h:43
act_pref
float act_pref[INDI_NUM_ACT]
Definition: stabilization_indi.c:118
stateGetAirspeed_f
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
float_vect_zero
static void float_vect_zero(float *a, const int n)
a = 0
Definition: pprz_algebra_float.h:542
actuator_state_filt_vectd
float actuator_state_filt_vectd[INDI_NUM_ACT]
Definition: stabilization_indi.c:145
angular_acceleration
float angular_acceleration[3]
Definition: stabilization_indi.c:133
Int32Quat::qz
int32_t qz
Definition: pprz_algebra_int.h:103
Int32Vect2
Definition: pprz_algebra_int.h:83
Int32Vect3
Definition: pprz_algebra_int.h:88
stabilization_indi_attitude_run
void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
Definition: stabilization_indi.c:546
stabilization_attitude_rc_setpoint.h
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
num_iter
int num_iter
Definition: stabilization_indi.c:74
estimation_output_lowpass_filters
Butterworth2LowPass estimation_output_lowpass_filters[3]
Definition: stabilization_indi.c:188
r_filt
float r_filt
Definition: stabilization_indi.c:140
rpm_ev
abi_event rpm_ev
Definition: stabilization_indi.c:166
Wv
static float Wv[INDI_OUTPUTS]
Definition: stabilization_indi.c:127
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
QUAT_BFP_OF_REAL
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:752
du_estimation
float du_estimation[INDI_NUM_ACT]
Definition: stabilization_indi.c:149
g1g2_trans_mult
float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS]
Definition: stabilization_indi.c:143
Int32Quat::qi
int32_t qi
Definition: pprz_algebra_int.h:100
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatQuat::qx
float qx
Definition: pprz_algebra_float.h:65
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
float_vect_smul
static void float_vect_smul(float *o, const float *a, const float s, const int n)
o = a * s
Definition: pprz_algebra_float.h:591
body_accel_f
struct FloatVect3 body_accel_f
Definition: stabilization_indi.c:192
wls_alloc.h
stabilization_indi_set_earth_cmd_i
void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
Definition: stabilization_indi.c:349
mu2
float mu2
Definition: stabilization_indi.c:155
stabilization_attitude_read_rc_setpoint_quat_f
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.
Definition: stabilization_attitude_rc_setpoint.c:374
stabilization_attitude_quat_transformations.h
STABILIZATION_INDI_FILT_CUTOFF
#define STABILIZATION_INDI_FILT_CUTOFF
Definition: stabilization_indi_simple.c:51
Int32Quat::qy
int32_t qy
Definition: pprz_algebra_int.h:102
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
g2_init
float g2_init[INDI_NUM_ACT]
Definition: stabilization_indi.c:183
g1g2inv
float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS]
Definition: stabilization_indi.c:144
act_is_servo
bool act_is_servo[INDI_NUM_ACT]
Definition: stabilization_indi.c:110
float_quat_tilt_twist
void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, struct FloatQuat *quat)
Tilt twist decomposition of a quaternion (z axis)
Definition: pprz_algebra_float.c:634
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
init_filters
void init_filters(void)
Function that resets the filters to zeros.
Definition: stabilization_indi.c:287
actuators.h
stateGetAccelBody_i
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
Definition: state.h:953
stabilization_indi_read_rc
void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
Definition: stabilization_indi.c:590
actuator_state_filt_vect
float actuator_state_filt_vect[INDI_NUM_ACT]
Definition: stabilization_indi.c:131
actuator_state
float actuator_state[INDI_NUM_ACT]
Definition: stabilization_indi.c:134
indi_use_adaptive
bool indi_use_adaptive
Definition: stabilization_indi.c:100
act_dyn
float act_dyn[INDI_NUM_ACT]
Definition: stabilization_indi.c:121
g1g2_pseudo_inv
float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS]
Definition: stabilization_indi.c:174
indi_u
float indi_u[INDI_NUM_ACT]
Definition: stabilization_indi.c:135
stabilization_cmd
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
float_vect_scale
static void float_vect_scale(float *a, const float s, const int n)
a *= s
Definition: pprz_algebra_float.h:616
g1_init
float g1_init[INDI_OUTPUTS][INDI_NUM_ACT]
Definition: stabilization_indi.c:182
QUAT_FLOAT_OF_BFP
#define QUAT_FLOAT_OF_BFP(_qf, _qi)
Definition: pprz_algebra.h:745
guidance_h
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:90
g1
float g1[INDI_OUTPUTS][INDI_NUM_ACT]
Definition: stabilization_indi.c:176
du_max
float du_max[INDI_NUM_ACT]
Definition: stabilization_indi.c:70
float_vect_copy
static void float_vect_copy(float *a, const float *b, const int n)
a = b
Definition: pprz_algebra_float.h:549
Indi_gains
Definition: stabilization_indi.h:41
float_quat_inv_comp_norm_shortest
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
Definition: pprz_algebra_float.c:358
ACCELS_FLOAT_OF_BFP
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
init_butterworth_2_low_pass
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
Definition: low_pass_filter.h:280
measurement_lowpass_filters
Butterworth2LowPass measurement_lowpass_filters[3]
Definition: stabilization_indi.c:187
INDI_G_SCALING
#define INDI_G_SCALING
Definition: stabilization_indi.h:30
stateGetNedToBodyEulers_i
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
stab_att_sp_euler
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
Definition: stabilization_indi.c:163
rates_filt_fo
static struct FirstOrderLowPass rates_filt_fo[3]
Definition: stabilization_indi.c:190
num_thrusters
int32_t num_thrusters
Definition: stabilization_indi.c:161
FloatQuat::qy
float qy
Definition: pprz_algebra_float.h:66
state.h
INT32_TRIG_FRAC
#define INT32_TRIG_FRAC
Definition: pprz_algebra_int.h:154
GUIDANCE_H_MODE_NAV
#define GUIDANCE_H_MODE_NAV
Definition: guidance_h.h:60
rpm_cb
static void rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act)
Indi_gains::att
struct FloatRates att
Definition: stabilization_indi.h:42
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
Int32Quat::qx
int32_t qx
Definition: pprz_algebra_int.h:101
mu1
float mu1[INDI_OUTPUTS]
Definition: stabilization_indi.c:153
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
thrust_cb
static void thrust_cb(uint8_t sender_id, float thrust_increment)
FirstOrderLowPass
First order low pass filter structure.
Definition: low_pass_filter.h:39
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
p
static float p[2][2]
Definition: ins_alt_float.c:257
angular_accel_ref
struct FloatRates angular_accel_ref
Definition: stabilization_indi.c:132
calc_g2_element
static void calc_g2_element(float dx_error, int8_t j, float mu_extra)
Definition: stabilization_indi.c:658
get_actuator_state
static void get_actuator_state(void)
Function that tries to get actuator feedback.
Definition: stabilization_indi.c:608
STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF
#define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF
Definition: stabilization_indi_simple.c:72
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
float_vect_sum
static void float_vect_sum(float *o, const float *a, const float *b, const int n)
o = a + b
Definition: pprz_algebra_float.h:556
low_pass_filter.h
Simple first order low pass filter with bilinear transform.
lms_estimation
static void lms_estimation(void)
Function that estimates the control effectiveness of each actuator online.
Definition: stabilization_indi.c:668
heading
float heading
Definition: wedgebug.c:258
PPRZ_ITRIG_SIN
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:111