Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
stabilization_indi_simple.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 
38 #include "state.h"
39 #include "generated/airframe.h"
40 #include "paparazzi.h"
43 
44 #if !defined(STABILIZATION_INDI_ACT_DYN_P) && !defined(STABILIZATION_INDI_ACT_DYN_Q) && !defined(STABILIZATION_INDI_ACT_DYN_R)
45 #error You have to define the first order time constant of the actuator dynamics!
46 #endif
47 
48 // these parameters are used in the filtering of the angular acceleration
49 // define them in the airframe file if different values are required
50 #ifndef STABILIZATION_INDI_FILT_CUTOFF
51 #define STABILIZATION_INDI_FILT_CUTOFF 8.0
52 #endif
53 
54 // the yaw sometimes requires more filtering
55 #ifndef STABILIZATION_INDI_FILT_CUTOFF_RDOT
56 #define STABILIZATION_INDI_FILT_CUTOFF_RDOT STABILIZATION_INDI_FILT_CUTOFF
57 #endif
58 
59 #ifndef STABILIZATION_INDI_MAX_RATE
60 #define STABILIZATION_INDI_MAX_RATE 6.0
61 #endif
62 
63 #if STABILIZATION_INDI_USE_ADAPTIVE
64 #warning "Use caution with adaptive indi. See the wiki for more info"
65 #endif
66 
67 #ifndef STABILIZATION_INDI_MAX_R
68 #define STABILIZATION_INDI_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R
69 #endif
70 
71 #ifndef STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF
72 #define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF 4.0
73 #endif
74 
75 #ifdef STABILIZATION_INDI_FILT_CUTOFF_P
76 #define STABILIZATION_INDI_FILTER_ROLL_RATE TRUE
77 #else
78 #define STABILIZATION_INDI_FILT_CUTOFF_P 20.0
79 #endif
80 
81 #ifdef STABILIZATION_INDI_FILT_CUTOFF_Q
82 #define STABILIZATION_INDI_FILTER_PITCH_RATE TRUE
83 #else
84 #define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0
85 #endif
86 
87 #ifdef STABILIZATION_INDI_FILT_CUTOFF_R
88 #define STABILIZATION_INDI_FILTER_YAW_RATE TRUE
89 #else
90 #define STABILIZATION_INDI_FILT_CUTOFF_R 20.0
91 #endif
92 
95 
97 
98 static inline void lms_estimation(void);
99 static void indi_init_filters(void);
100 
101 //The G values are scaled to avoid numerical problems during the estimation
102 #define INDI_EST_SCALE 0.001
103 
104 struct IndiVariables indi = {
106  .max_rate = STABILIZATION_INDI_MAX_RATE,
107  .attitude_max_yaw_rate = STABILIZATION_INDI_MAX_R,
108 
109  .g1 = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R},
110  .g2 = STABILIZATION_INDI_G2_R,
111  .gains = {
112  .att = {
113  STABILIZATION_INDI_REF_ERR_P,
114  STABILIZATION_INDI_REF_ERR_Q,
115  STABILIZATION_INDI_REF_ERR_R
116  },
117  .rate = {
118  STABILIZATION_INDI_REF_RATE_P,
119  STABILIZATION_INDI_REF_RATE_Q,
120  STABILIZATION_INDI_REF_RATE_R
121  },
122  },
123 
124  /* Estimation parameters for adaptive INDI */
125  .est = {
126  .g1 = {
127  STABILIZATION_INDI_G1_P / INDI_EST_SCALE,
128  STABILIZATION_INDI_G1_Q / INDI_EST_SCALE,
129  STABILIZATION_INDI_G1_R / INDI_EST_SCALE
130  },
131  .g2 = STABILIZATION_INDI_G2_R / INDI_EST_SCALE,
132  .mu = STABILIZATION_INDI_ADAPTIVE_MU,
133  },
134 
135 #if STABILIZATION_INDI_USE_ADAPTIVE
136  .adaptive = TRUE,
137 #else
138  .adaptive = FALSE,
139 #endif
140 };
141 
142 #if PERIODIC_TELEMETRY
144 
145 static void send_att_indi(struct transport_tx *trans, struct link_device *dev)
146 {
147  //The estimated G values are scaled, so scale them back before sending
148  struct FloatRates g1_disp;
149  RATES_SMUL(g1_disp, indi.est.g1, INDI_EST_SCALE);
150  float g2_disp = indi.est.g2 * INDI_EST_SCALE;
151 
152  pprz_msg_send_STAB_ATTITUDE_INDI(trans, dev, AC_ID,
153  &indi.rate_d[0],
154  &indi.rate_d[1],
155  &indi.rate_d[2],
159  &g1_disp.p,
160  &g1_disp.q,
161  &g1_disp.r,
162  &g2_disp);
163 }
164 
165 static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
166 {
167  struct Int32Quat *quat = stateGetNedToBodyQuat_i();
168  pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
173  &(quat->qi),
174  &(quat->qx),
175  &(quat->qy),
176  &(quat->qz));
177 }
178 #endif
179 
181 {
182  // Initialize filters
184 
185 #if PERIODIC_TELEMETRY
186  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_INDI, send_att_indi);
188 #endif
189 }
190 
192 {
193  // tau = 1/(2*pi*Fc)
194  float tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF);
195  float tau_rdot = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_RDOT);
196  float tau_axis[3] = {tau, tau, tau_rdot};
197  float tau_est = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF);
198  float sample_time = 1.0 / PERIODIC_FREQUENCY;
199  // Filtering of gyroscope and actuators
200  for (int8_t i = 0; i < 3; i++) {
201  init_butterworth_2_low_pass(&indi.u[i], tau_axis[i], sample_time, 0.0);
202  init_butterworth_2_low_pass(&indi.rate[i], tau_axis[i], sample_time, 0.0);
203  init_butterworth_2_low_pass(&indi.est.u[i], tau_est, sample_time, 0.0);
204  init_butterworth_2_low_pass(&indi.est.rate[i], tau_est, sample_time, 0.0);
205  }
206 
207  // Init rate filter for feedback
208  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)};
209 
210  init_first_order_low_pass(&rates_filt_fo[0], time_constants[0], sample_time, stateGetBodyRates_f()->p);
211  init_first_order_low_pass(&rates_filt_fo[1], time_constants[1], sample_time, stateGetBodyRates_f()->q);
212  init_first_order_low_pass(&rates_filt_fo[2], time_constants[2], sample_time, stateGetBodyRates_f()->r);
213 }
214 
215 // Callback function for setting cutoff frequency for r
217  float sample_time = 1.0 / PERIODIC_FREQUENCY;
218  indi.cutoff_r = new_cutoff;
219  float time_constant = 1.0/(2.0 * M_PI * indi.cutoff_r);
220  init_first_order_low_pass(&rates_filt_fo[2], time_constant, sample_time, stateGetBodyRates_f()->r);
221 }
222 
224 {
225  /* reset psi setpoint to current psi angle */
227 
231 
232  // Re-initialize filters
234 }
235 
237 {
238  /* set failsafe to zero roll/pitch and current heading */
241  stab_att_sp_quat.qx = 0;
242  stab_att_sp_quat.qy = 0;
244 }
245 
252 {
253  // stab_att_sp_euler.psi still used in ref..
254  stab_att_sp_euler = *rpy;
255 
257 }
258 
266 {
267  // stab_att_sp_euler.psi still used in ref..
269 
270  // compute sp_euler phi/theta for debugging/telemetry
271  /* Rotate horizontal commands to body frame by psi */
273  int32_t s_psi, c_psi;
274  PPRZ_ITRIG_SIN(s_psi, psi);
275  PPRZ_ITRIG_COS(c_psi, psi);
276  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
277  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
278 
280 }
281 
288 static inline void filter_pqr(Butterworth2LowPass *filter, struct FloatRates *new_values)
289 {
290  update_butterworth_2_low_pass(&filter[0], new_values->p);
291  update_butterworth_2_low_pass(&filter[1], new_values->q);
292  update_butterworth_2_low_pass(&filter[2], new_values->r);
293 }
294 
302 static inline void finite_difference_from_filter(float *output, Butterworth2LowPass *filter)
303 {
304  for (int8_t i = 0; i < 3; i++) {
305  output[i] = (filter[i].o[0] - filter[i].o[1]) * PERIODIC_FREQUENCY;
306  }
307 }
308 
316 static inline void finite_difference(float output[3], float new[3], float old[3])
317 {
318  for (int8_t i = 0; i < 3; i++) {
319  output[i] = (new[i] - old[i])*PERIODIC_FREQUENCY;
320  }
321 }
322 
329 void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight __attribute__((unused)))
330 {
331  //Propagate input filters
332  //first order actuator dynamics
333  indi.u_act_dyn.p = indi.u_act_dyn.p + STABILIZATION_INDI_ACT_DYN_P * (indi.u_in.p - indi.u_act_dyn.p);
334  indi.u_act_dyn.q = indi.u_act_dyn.q + STABILIZATION_INDI_ACT_DYN_Q * (indi.u_in.q - indi.u_act_dyn.q);
335  indi.u_act_dyn.r = indi.u_act_dyn.r + STABILIZATION_INDI_ACT_DYN_R * (indi.u_in.r - indi.u_act_dyn.r);
336 
337  // Propagate the filter on the gyroscopes and actuators
338  struct FloatRates *body_rates = stateGetBodyRates_f();
340  filter_pqr(indi.rate, body_rates);
341 
342  // Calculate the derivative of the rates
344 
345  //The rates used for feedback are by default the measured rates.
346  //If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.
347  //Note that due to the delay, the PD controller may need relaxed gains.
348  struct FloatRates rates_filt;
349 #if STABILIZATION_INDI_FILTER_ROLL_RATE
350  rates_filt.p = update_first_order_low_pass(&rates_filt_fo[0], body_rates->p);
351 #else
352  rates_filt.p = body_rates->p;
353 #endif
354 #if STABILIZATION_INDI_FILTER_PITCH_RATE
355  rates_filt.q = update_first_order_low_pass(&rates_filt_fo[1], body_rates->q);
356 #else
357  rates_filt.q = body_rates->q;
358 #endif
359 #if STABILIZATION_INDI_FILTER_YAW_RATE
360  rates_filt.r = update_first_order_low_pass(&rates_filt_fo[2], body_rates->r);
361 #else
362  rates_filt.r = body_rates->r;
363 #endif
364 
365  //This lets you impose a maximum yaw rate.
366  BoundAbs(rate_sp.r, indi.attitude_max_yaw_rate);
367 
368  // Compute reference angular acceleration:
369  indi.angular_accel_ref.p = (rate_sp.p - rates_filt.p) * indi.gains.rate.p;
370  indi.angular_accel_ref.q = (rate_sp.q - rates_filt.q) * indi.gains.rate.q;
371  indi.angular_accel_ref.r = (rate_sp.r - rates_filt.r) * indi.gains.rate.r;
372 
373  //Increment in angular acceleration requires increment in control input
374  //G1 is the control effectiveness. In the yaw axis, we need something additional: G2.
375  //It takes care of the angular acceleration caused by the change in rotation rate of the propellers
376  //(they have significant inertia, see the paper mentioned in the header for more explanation)
377  indi.du.p = 1.0 / indi.g1.p * (indi.angular_accel_ref.p - indi.rate_d[0]);
378  indi.du.q = 1.0 / indi.g1.q * (indi.angular_accel_ref.q - indi.rate_d[1]);
379  indi.du.r = 1.0 / (indi.g1.r + indi.g2) * (indi.angular_accel_ref.r - indi.rate_d[2] + indi.g2 * indi.du.r);
380 
381  //Don't increment if thrust is off and on the ground
382  //without this the inputs will increment to the maximum before even getting in the air.
383  if (stabilization_cmd[COMMAND_THRUST] < 300 && !in_flight) {
385 
386  // If on the gournd, no increments, just proportional control
387  indi.u_in.p = indi.du.p;
388  indi.u_in.q = indi.du.q;
389  indi.u_in.r = indi.du.r;
390  } else {
391  //add the increment to the total control input
392  indi.u_in.p = indi.u[0].o[0] + indi.du.p;
393  indi.u_in.q = indi.u[1].o[0] + indi.du.q;
394  indi.u_in.r = indi.u[2].o[0] + indi.du.r;
395 
396  // only run the estimation if the commands are not zero.
397  lms_estimation();
398  }
399 
400  //bound the total control input
401 #if STABILIZATION_INDI_FULL_AUTHORITY
402  Bound(indi.u_in.p, -9600, 9600);
403  Bound(indi.u_in.q, -9600, 9600);
404  float rlim = 9600 - fabs(indi.u_in.q);
405  Bound(indi.u_in.r, -rlim, rlim);
406  Bound(indi.u_in.r, -9600, 9600);
407 #else
408  Bound(indi.u_in.p, -4500, 4500);
409  Bound(indi.u_in.q, -4500, 4500);
410  Bound(indi.u_in.r, -4500, 4500);
411 #endif
412 
413  /* INDI feedback */
414  stabilization_cmd[COMMAND_ROLL] = indi.u_in.p;
415  stabilization_cmd[COMMAND_PITCH] = indi.u_in.q;
416  stabilization_cmd[COMMAND_YAW] = indi.u_in.r;
417 
418  /* bound the result */
419  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
420  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
421  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
422 }
423 
430 void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight __attribute__((unused)))
431 {
432  /* attitude error */
433  struct FloatQuat att_err;
434  struct FloatQuat *att_quat = stateGetNedToBodyQuat_f();
435  struct FloatQuat quat_sp_f;
436 
437  QUAT_FLOAT_OF_BFP(quat_sp_f, quat_sp);
438  float_quat_inv_comp_norm_shortest(&att_err, att_quat, &quat_sp_f);
439 
440  struct FloatVect3 att_fb;
441 
442 #if TILT_TWIST_CTRL
443  struct FloatQuat tilt;
444  struct FloatQuat twist;
445  float_quat_tilt_twist(&tilt, &twist, &att_err);
446  att_fb.x = tilt.qx;
447  att_fb.y = tilt.qy;
448  att_fb.z = twist.qz;
449 #else
450  att_fb.x = att_err.qx;
451  att_fb.y = att_err.qy;
452  att_fb.z = att_err.qz;
453 #endif
454 
455  struct FloatRates rate_sp;
456  // Divide by rate gain to make it equivalent to a parallel structure
457  rate_sp.p = indi.gains.att.p * att_fb.x / indi.gains.rate.p;
458  rate_sp.q = indi.gains.att.q * att_fb.y / indi.gains.rate.q;
459  rate_sp.r = indi.gains.att.r * att_fb.z / indi.gains.rate.r;
460 
461  /* compute the INDI command */
462  stabilization_indi_rate_run(rate_sp, in_flight);
463 }
464 
470 void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
471 {
472  struct FloatQuat q_sp;
473 #if USE_EARTH_BOUND_RC_SETPOINT
474  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
475 #else
476  stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
477 #endif
479 }
480 
487 static inline void lms_estimation(void)
488 {
489  static struct IndiEstimation *est = &indi.est;
490  // Only pass really low frequencies so you don't adapt to noise
491  struct FloatRates *body_rates = stateGetBodyRates_f();
492  filter_pqr(est->u, &indi.u_act_dyn);
493  filter_pqr(est->rate, body_rates);
494 
495  // Calculate the first and second derivatives of the rates and actuators
496  float rate_d_prev[3];
497  float u_d_prev[3];
498  float_vect_copy(rate_d_prev, est->rate_d, 3);
499  float_vect_copy(u_d_prev, est->u_d, 3);
501  finite_difference_from_filter(est->u_d, est->u);
502  finite_difference(est->rate_dd, est->rate_d, rate_d_prev);
503  finite_difference(est->u_dd, est->u_d, u_d_prev);
504 
505  // The inputs are scaled in order to avoid overflows
506  float du[3];
507  float_vect_copy(du, est->u_d, 3);
509  est->g1.p = est->g1.p - (est->g1.p * du[0] - est->rate_dd[0]) * du[0] * est->mu;
510  est->g1.q = est->g1.q - (est->g1.q * du[1] - est->rate_dd[1]) * du[1] * est->mu;
511  float ddu = est->u_dd[2] * INDI_EST_SCALE / PERIODIC_FREQUENCY;
512  float error = (est->g1.r * du[2] + est->g2 * ddu - est->rate_dd[2]);
513  est->g1.r = est->g1.r - error * du[2] * est->mu / 3;
514  est->g2 = est->g2 - error * 1000 * ddu * est->mu / 3;
515 
516  //the g values should be larger than zero, otherwise there is positive feedback, the command will go to max and there is nothing to learn anymore...
517  if (est->g1.p < 0.01) { est->g1.p = 0.01; }
518  if (est->g1.q < 0.01) { est->g1.q = 0.01; }
519  if (est->g1.r < 0.01) { est->g1.r = 0.01; }
520  if (est->g2 < 0.01) { est->g2 = 0.01; }
521 
522  if (indi.adaptive) {
523  //Commit the estimated G values and apply the scaling
524  indi.g1.p = est->g1.p * INDI_EST_SCALE;
525  indi.g1.q = est->g1.q * INDI_EST_SCALE;
526  indi.g1.r = est->g1.r * INDI_EST_SCALE;
527  indi.g2 = est->g2 * INDI_EST_SCALE;
528  }
529 }
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
radio_control.h
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
stab_att_sp_euler
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
Definition: stabilization_indi_simple.c:93
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
STABILIZATION_INDI_FILT_CUTOFF_P
#define STABILIZATION_INDI_FILT_CUTOFF_P
Definition: stabilization_indi_simple.c:78
stabilization_indi_simple_reset_r_filter_cutoff
void stabilization_indi_simple_reset_r_filter_cutoff(float new_cutoff)
Definition: stabilization_indi_simple.c:216
indi_init_filters
static void indi_init_filters(void)
Definition: stabilization_indi_simple.c:191
IndiVariables::u_act_dyn
struct FloatRates u_act_dyn
Definition: stabilization_indi_simple.h:65
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_set_earth_cmd_i
void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
Set attitude setpoint from command in earth axes.
Definition: stabilization_indi_simple.c:265
STABILIZATION_INDI_FILT_CUTOFF_R
#define STABILIZATION_INDI_FILT_CUTOFF_R
Definition: stabilization_indi_simple.c:90
INDI_EST_SCALE
#define INDI_EST_SCALE
Definition: stabilization_indi_simple.c:102
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
STABILIZATION_INDI_MAX_RATE
#define STABILIZATION_INDI_MAX_RATE
Definition: stabilization_indi_simple.c:60
rates_filt_fo
static struct FirstOrderLowPass rates_filt_fo[3]
Definition: stabilization_indi_simple.c:96
STABILIZATION_INDI_FILT_CUTOFF_Q
#define STABILIZATION_INDI_FILT_CUTOFF_Q
Definition: stabilization_indi_simple.c:84
IndiVariables::rate_d
float rate_d[3]
Definition: stabilization_indi_simple.h:66
FLOAT_RATES_ZERO
#define FLOAT_RATES_ZERO(_r)
Definition: pprz_algebra_float.h:191
IndiEstimation::u_dd
float u_dd[3]
Definition: stabilization_indi_simple.h:54
IndiVariables::u
Butterworth2LowPass u[3]
Definition: stabilization_indi_simple.h:68
IndiEstimation::g1
struct FloatRates g1
Definition: stabilization_indi_simple.h:55
stabilization_indi_set_rpy_setpoint_i
void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
Set attitude quaternion setpoint from rpy.
Definition: stabilization_indi_simple.c:251
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
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
RATES_SMUL
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:379
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
stabilization_indi_init
void stabilization_indi_init(void)
Function that initializes important values upon engaging INDI.
Definition: stabilization_indi_simple.c:180
PPRZ_ITRIG_COS
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:112
IndiVariables::u_in
struct FloatRates u_in
Definition: stabilization_indi_simple.h:64
SecondOrderLowPass
Second order low pass filter structure.
Definition: low_pass_filter.h:136
indi
struct IndiVariables indi
Definition: stabilization_indi_simple.c:104
stabilization_indi_enter
void stabilization_indi_enter(void)
Function that resets important values upon engaging INDI.
Definition: stabilization_indi_simple.c:223
stabilization_attitude_get_heading_i
int32_t stabilization_attitude_get_heading_i(void)
Definition: stabilization_attitude_rc_setpoint.c:130
IndiVariables
Definition: stabilization_indi_simple.h:60
paparazzi.h
stabilization_indi_attitude_run
void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
runs stabilization indi
Definition: stabilization_indi_simple.c:430
IndiEstimation::rate
Butterworth2LowPass rate[3]
Definition: stabilization_indi_simple.h:50
SecondOrderLowPass::o
float o[2]
output history
Definition: low_pass_filter.h:140
IndiVariables::adaptive
bool adaptive
Enable adataptive estimation.
Definition: stabilization_indi_simple.h:75
FloatVect3
Definition: pprz_algebra_float.h:54
IndiVariables::angular_accel_ref
struct FloatRates angular_accel_ref
Definition: stabilization_indi_simple.h:62
telemetry.h
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
STABILIZATION_INDI_FILT_CUTOFF_RDOT
#define STABILIZATION_INDI_FILT_CUTOFF_RDOT
Definition: stabilization_indi_simple.c:56
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
Int32Eulers::phi
int32_t phi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:147
stab_att_sp_quat
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
Definition: stabilization_indi_simple.c:94
stateGetNedToBodyQuat_f
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
STABILIZATION_INDI_MAX_R
#define STABILIZATION_INDI_MAX_R
Definition: stabilization_indi_simple.c:68
IndiVariables::cutoff_r
float cutoff_r
Definition: stabilization_indi_simple.h:61
stabilization_indi_simple.h
Indi_gains::rate
struct FloatRates rate
Definition: stabilization_indi.h:43
IndiVariables::g1
struct FloatRates g1
Definition: stabilization_indi_simple.h:70
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
IndiVariables::du
struct FloatRates du
Definition: stabilization_indi_simple.h:63
Int32Quat::qz
int32_t qz
Definition: pprz_algebra_int.h:103
IndiEstimation::mu
float mu
Definition: stabilization_indi_simple.h:57
Int32Vect2
Definition: pprz_algebra_int.h:83
stabilization_indi_read_rc
void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
This function reads rc commands.
Definition: stabilization_indi_simple.c:470
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
send_ahrs_ref_quat
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_indi_simple.c:165
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
IndiEstimation::g2
float g2
Definition: stabilization_indi_simple.h:56
QUAT_BFP_OF_REAL
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:752
filter_pqr
static void filter_pqr(Butterworth2LowPass *filter, struct FloatRates *new_values)
Update butterworth filter for p, q and r of a FloatRates struct.
Definition: stabilization_indi_simple.c:288
send_att_indi
static void send_att_indi(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_indi_simple.c:145
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
IndiVariables::est
struct IndiEstimation est
Estimation parameters for adaptive INDI.
Definition: stabilization_indi_simple.h:78
IndiEstimation::u_d
float u_d[3]
Definition: stabilization_indi_simple.h:53
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
IndiEstimation::u
Butterworth2LowPass u[3]
Definition: stabilization_indi_simple.h:49
IndiEstimation::rate_d
float rate_d[3]
Definition: stabilization_indi_simple.h:51
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
IndiEstimation::rate_dd
float rate_dd[3]
Definition: stabilization_indi_simple.h:52
stabilization_indi_set_failsafe_setpoint
void stabilization_indi_set_failsafe_setpoint(void)
Function that calculates the failsafe setpoint.
Definition: stabilization_indi_simple.c:236
lms_estimation
static void lms_estimation(void)
This is a Least Mean Squares adaptive filter It estimates the actuator effectiveness online,...
Definition: stabilization_indi_simple.c:487
IndiVariables::g2
float g2
Definition: stabilization_indi_simple.h:71
IndiVariables::rate
Butterworth2LowPass rate[3]
Definition: stabilization_indi_simple.h:69
IndiVariables::attitude_max_yaw_rate
float attitude_max_yaw_rate
Maximum yaw rate in atttiude control in rad/s.
Definition: stabilization_indi_simple.h:77
IndiVariables::gains
struct Indi_gains gains
Definition: stabilization_indi_simple.h:73
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
QUAT_FLOAT_OF_BFP
#define QUAT_FLOAT_OF_BFP(_qf, _qi)
Definition: pprz_algebra.h:745
float_vect_copy
static void float_vect_copy(float *a, const float *b, const int n)
a = b
Definition: pprz_algebra_float.h:549
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
IndiEstimation
Definition: stabilization_indi_simple.h:48
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
stateGetNedToBodyEulers_i
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
finite_difference
static void finite_difference(float output[3], float new[3], float old[3])
Calculate derivative of an array via finite difference.
Definition: stabilization_indi_simple.c:316
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
FALSE
#define FALSE
Definition: std.h:5
TRUE
#define TRUE
Definition: std.h:4
stabilization_indi_rate_run
void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
Does the INDI calculations.
Definition: stabilization_indi_simple.c:329
Indi_gains::att
struct FloatRates att
Definition: stabilization_indi.h:42
Int32Quat::qx
int32_t qx
Definition: pprz_algebra_int.h:101
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
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
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
low_pass_filter.h
Simple first order low pass filter with bilinear transform.
finite_difference_from_filter
static void finite_difference_from_filter(float *output, Butterworth2LowPass *filter)
Caclulate finite difference form a filter array The filter already contains the previous values.
Definition: stabilization_indi_simple.c:302
heading
float heading
Definition: wedgebug.c:258
PPRZ_ITRIG_SIN
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:111