Paparazzi UAS  v7.0_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 #warning STABILIZATION_INDI_ACT_DYN is deprecated, use STABILIZATION_INDI_ACT_FREQ instead.
46 #warning You now have to define the continuous time corner frequency in rad/s of the actuators.
47 #warning "Use -ln(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old values."
48 #else
49 #if !defined(STABILIZATION_INDI_ACT_FREQ_P) && !defined(STABILIZATION_INDI_ACT_FREQ_Q) && !defined(STABILIZATION_INDI_ACT_FREQ_R)
50 #warning You have to define the corner frequency of the first order actuator dynamics model in rad/s!
51 #endif
52 #endif
53 
54 // these parameters are used in the filtering of the angular acceleration
55 // define them in the airframe file if different values are required
56 #ifndef STABILIZATION_INDI_FILT_CUTOFF
57 #define STABILIZATION_INDI_FILT_CUTOFF 8.0
58 #endif
59 
60 // the yaw sometimes requires more filtering
61 #ifndef STABILIZATION_INDI_FILT_CUTOFF_RDOT
62 #define STABILIZATION_INDI_FILT_CUTOFF_RDOT STABILIZATION_INDI_FILT_CUTOFF
63 #endif
64 
65 #ifndef STABILIZATION_INDI_MAX_RATE
66 #define STABILIZATION_INDI_MAX_RATE 6.0
67 #endif
68 
69 #if STABILIZATION_INDI_USE_ADAPTIVE
70 #warning "Use caution with adaptive indi. See the wiki for more info"
71 #endif
72 
73 #ifndef STABILIZATION_INDI_MAX_R
74 #define STABILIZATION_INDI_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R
75 #endif
76 
77 #ifndef STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF
78 #define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF 4.0
79 #endif
80 
81 #ifdef STABILIZATION_INDI_FILT_CUTOFF_P
82 #define STABILIZATION_INDI_FILTER_ROLL_RATE TRUE
83 #else
84 #define STABILIZATION_INDI_FILT_CUTOFF_P 20.0
85 #endif
86 
87 #ifdef STABILIZATION_INDI_FILT_CUTOFF_Q
88 #define STABILIZATION_INDI_FILTER_PITCH_RATE TRUE
89 #else
90 #define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0
91 #endif
92 
93 #ifdef STABILIZATION_INDI_FILT_CUTOFF_R
94 #define STABILIZATION_INDI_FILTER_YAW_RATE TRUE
95 #else
96 #define STABILIZATION_INDI_FILT_CUTOFF_R 20.0
97 #endif
98 
102 
103 static struct FirstOrderLowPass rates_filt_fo[3];
104 
105 static inline void lms_estimation(void);
106 static void indi_init_filters(void);
107 
108 //The G values are scaled to avoid numerical problems during the estimation
109 #define INDI_EST_SCALE 0.001
110 
111 struct IndiVariables indi = {
113  .max_rate = STABILIZATION_INDI_MAX_RATE,
114  .attitude_max_yaw_rate = STABILIZATION_INDI_MAX_R,
115 
116  .g1 = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R},
117  .g2 = STABILIZATION_INDI_G2_R,
118  .gains = {
119  .att = {
120  STABILIZATION_INDI_REF_ERR_P,
121  STABILIZATION_INDI_REF_ERR_Q,
122  STABILIZATION_INDI_REF_ERR_R
123  },
124  .rate = {
125  STABILIZATION_INDI_REF_RATE_P,
126  STABILIZATION_INDI_REF_RATE_Q,
127  STABILIZATION_INDI_REF_RATE_R
128  },
129  },
130 
131  /* Estimation parameters for adaptive INDI */
132  .est = {
133  .g1 = {
134  STABILIZATION_INDI_G1_P / INDI_EST_SCALE,
135  STABILIZATION_INDI_G1_Q / INDI_EST_SCALE,
136  STABILIZATION_INDI_G1_R / INDI_EST_SCALE
137  },
138  .g2 = STABILIZATION_INDI_G2_R / INDI_EST_SCALE,
139  .mu = STABILIZATION_INDI_ADAPTIVE_MU,
140  },
141 
142 #if STABILIZATION_INDI_USE_ADAPTIVE
143  .adaptive = TRUE,
144 #else
145  .adaptive = FALSE,
146 #endif
147 };
148 
149 #if PERIODIC_TELEMETRY
151 
152 static void send_att_indi(struct transport_tx *trans, struct link_device *dev)
153 {
154  float zero = 0.0;
155  pprz_msg_send_STAB_ATTITUDE(trans, dev, AC_ID,
156  &zero, &zero, &zero, // att
157  &zero, &zero, &zero, // att.ref
158  &indi.rate[0].o[0], // rate
159  &indi.rate[1].o[0],
160  &indi.rate[2].o[0],
161  &zero, &zero, &zero, // rate.ref
162  &indi.rate_d[0], // ang.acc = rate.diff
163  &indi.rate_d[1],
164  &indi.rate_d[2],
165  &indi.angular_accel_ref.p, // ang.acc.ref
168  1, &zero, // inputs
169  1, &zero); // outputs
170 }
171 static void send_eff_mat_g_indi_simple(struct transport_tx *trans, struct link_device *dev)
172 {
173  //The estimated G values are scaled, so scale them back before sending
174  struct FloatRates g1_disp;
175  RATES_SMUL(g1_disp, indi.est.g1, INDI_EST_SCALE);
176  float g2_disp = indi.est.g2 * INDI_EST_SCALE;
177  float zero = 0.0;
178  pprz_msg_send_EFF_MAT_G(trans, dev, AC_ID,
179  1, &zero,
180  1, &zero,
181  1, &zero,
182  1, &g1_disp.p,
183  1, &g1_disp.q,
184  1, &g1_disp.r,
185  1, &g2_disp,
186  1, &zero);
187 }
188 
189 static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
190 {
191  struct Int32Quat *quat = stateGetNedToBodyQuat_i();
192  pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
197  &(quat->qi),
198  &(quat->qx),
199  &(quat->qy),
200  &(quat->qz));
201 }
202 #endif
203 
205 {
206  // Initialize filters
208 
209 #ifdef STABILIZATION_INDI_ACT_FREQ_P
210  indi.act_dyn.p = 1-exp(-STABILIZATION_INDI_ACT_FREQ_P/PERIODIC_FREQUENCY);
211  indi.act_dyn.q = 1-exp(-STABILIZATION_INDI_ACT_FREQ_Q/PERIODIC_FREQUENCY);
212  indi.act_dyn.r = 1-exp(-STABILIZATION_INDI_ACT_FREQ_R/PERIODIC_FREQUENCY);
213 #else
214  indi.act_dyn.p = STABILIZATION_INDI_ACT_DYN_P;
215  indi.act_dyn.q = STABILIZATION_INDI_ACT_DYN_Q;
216  indi.act_dyn.r = STABILIZATION_INDI_ACT_DYN_R;
217 #endif
218 
219 #if PERIODIC_TELEMETRY
220  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE, send_att_indi);
223 #endif
224 }
225 
227 {
228  // tau = 1/(2*pi*Fc)
229  float tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF);
230  float tau_rdot = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_RDOT);
231  float tau_axis[3] = {tau, tau, tau_rdot};
232  float tau_est = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF);
233  float sample_time = 1.0 / PERIODIC_FREQUENCY;
234  // Filtering of gyroscope and actuators
235  for (int8_t i = 0; i < 3; i++) {
236  init_butterworth_2_low_pass(&indi.u[i], tau_axis[i], sample_time, 0.0);
237  init_butterworth_2_low_pass(&indi.rate[i], tau_axis[i], sample_time, 0.0);
238  init_butterworth_2_low_pass(&indi.est.u[i], tau_est, sample_time, 0.0);
239  init_butterworth_2_low_pass(&indi.est.rate[i], tau_est, sample_time, 0.0);
240  }
241 
242  // Init rate filter for feedback
243  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)};
244 
245  init_first_order_low_pass(&rates_filt_fo[0], time_constants[0], sample_time, stateGetBodyRates_f()->p);
246  init_first_order_low_pass(&rates_filt_fo[1], time_constants[1], sample_time, stateGetBodyRates_f()->q);
247  init_first_order_low_pass(&rates_filt_fo[2], time_constants[2], sample_time, stateGetBodyRates_f()->r);
248 }
249 
250 // Callback function for setting cutoff frequency for r
252  float sample_time = 1.0 / PERIODIC_FREQUENCY;
253  indi.cutoff_r = new_cutoff;
254  float time_constant = 1.0/(2.0 * M_PI * indi.cutoff_r);
255  init_first_order_low_pass(&rates_filt_fo[2], time_constant, sample_time, stateGetBodyRates_f()->r);
256 }
257 
259 {
260  /* reset psi setpoint to current psi angle */
262 
266 
267  // Re-initialize filters
269 }
270 
272 {
273  /* set failsafe to zero roll/pitch and current heading */
276  stab_att_sp_quat.qx = 0;
277  stab_att_sp_quat.qy = 0;
280 }
281 
288 {
289  // stab_att_sp_euler.psi still used in ref..
290  stab_att_sp_euler = *rpy;
291 
294 }
295 
300 {
301  stab_att_sp_quat = *quat;
304 }
305 
313 {
314  // stab_att_sp_euler.psi still used in ref..
316 
317  // compute sp_euler phi/theta for debugging/telemetry
318  /* Rotate horizontal commands to body frame by psi */
320  int32_t s_psi, c_psi;
321  PPRZ_ITRIG_SIN(s_psi, psi);
322  PPRZ_ITRIG_COS(c_psi, psi);
323  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
324  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
325 
328 }
329 
336 {
340 }
341 
348 static inline void filter_pqr(Butterworth2LowPass *filter, struct FloatRates *new_values)
349 {
350  update_butterworth_2_low_pass(&filter[0], new_values->p);
351  update_butterworth_2_low_pass(&filter[1], new_values->q);
352  update_butterworth_2_low_pass(&filter[2], new_values->r);
353 }
354 
362 static inline void finite_difference_from_filter(float *output, Butterworth2LowPass *filter)
363 {
364  for (int8_t i = 0; i < 3; i++) {
365  output[i] = (filter[i].o[0] - filter[i].o[1]) * PERIODIC_FREQUENCY;
366  }
367 }
368 
376 static inline void finite_difference(float output[3], float new[3], float old[3])
377 {
378  for (int8_t i = 0; i < 3; i++) {
379  output[i] = (new[i] - old[i])*PERIODIC_FREQUENCY;
380  }
381 }
382 
389 void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight __attribute__((unused)))
390 {
391  //Propagate input filters
392  //first order actuator dynamics
396 
397  // Propagate the filter on the gyroscopes and actuators
398  struct FloatRates *body_rates = stateGetBodyRates_f();
400  filter_pqr(indi.rate, body_rates);
401 
402  // Calculate the derivative of the rates
404 
405  //The rates used for feedback are by default the measured rates.
406  //If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.
407  //Note that due to the delay, the PD controller may need relaxed gains.
408  struct FloatRates rates_filt;
409 #if STABILIZATION_INDI_FILTER_ROLL_RATE
410  rates_filt.p = update_first_order_low_pass(&rates_filt_fo[0], body_rates->p);
411 #else
412  rates_filt.p = body_rates->p;
413 #endif
414 #if STABILIZATION_INDI_FILTER_PITCH_RATE
415  rates_filt.q = update_first_order_low_pass(&rates_filt_fo[1], body_rates->q);
416 #else
417  rates_filt.q = body_rates->q;
418 #endif
419 #if STABILIZATION_INDI_FILTER_YAW_RATE
420  rates_filt.r = update_first_order_low_pass(&rates_filt_fo[2], body_rates->r);
421 #else
422  rates_filt.r = body_rates->r;
423 #endif
424 
425  //This lets you impose a maximum yaw rate.
426  BoundAbs(rate_sp.r, indi.attitude_max_yaw_rate);
427 
428  // Compute reference angular acceleration:
429  indi.angular_accel_ref.p = (rate_sp.p - rates_filt.p) * indi.gains.rate.p;
430  indi.angular_accel_ref.q = (rate_sp.q - rates_filt.q) * indi.gains.rate.q;
431  indi.angular_accel_ref.r = (rate_sp.r - rates_filt.r) * indi.gains.rate.r;
432 
433  //Increment in angular acceleration requires increment in control input
434  //G1 is the control effectiveness. In the yaw axis, we need something additional: G2.
435  //It takes care of the angular acceleration caused by the change in rotation rate of the propellers
436  //(they have significant inertia, see the paper mentioned in the header for more explanation)
437  indi.du.p = 1.0 / indi.g1.p * (indi.angular_accel_ref.p - indi.rate_d[0]);
438  indi.du.q = 1.0 / indi.g1.q * (indi.angular_accel_ref.q - indi.rate_d[1]);
439  indi.du.r = 1.0 / (indi.g1.r + indi.g2) * (indi.angular_accel_ref.r - indi.rate_d[2] + indi.g2 * indi.du.r);
440 
441  //Don't increment if thrust is off and on the ground
442  //without this the inputs will increment to the maximum before even getting in the air.
443  if (stabilization_cmd[COMMAND_THRUST] < 300 && !in_flight) {
445 
446  // If on the gournd, no increments, just proportional control
447  indi.u_in.p = indi.du.p;
448  indi.u_in.q = indi.du.q;
449  indi.u_in.r = indi.du.r;
450  } else {
451  //add the increment to the total control input
452  indi.u_in.p = indi.u[0].o[0] + indi.du.p;
453  indi.u_in.q = indi.u[1].o[0] + indi.du.q;
454  indi.u_in.r = indi.u[2].o[0] + indi.du.r;
455 
456  // only run the estimation if the commands are not zero.
457  lms_estimation();
458  }
459 
460  //bound the total control input
461 #if STABILIZATION_INDI_FULL_AUTHORITY
462  Bound(indi.u_in.p, -9600, 9600);
463  Bound(indi.u_in.q, -9600, 9600);
464  float rlim = 9600 - fabs(indi.u_in.q);
465  Bound(indi.u_in.r, -rlim, rlim);
466  Bound(indi.u_in.r, -9600, 9600);
467 #else
468  Bound(indi.u_in.p, -4500, 4500);
469  Bound(indi.u_in.q, -4500, 4500);
470  Bound(indi.u_in.r, -4500, 4500);
471 #endif
472 
473  /* INDI feedback */
474  stabilization_cmd[COMMAND_ROLL] = indi.u_in.p;
475  stabilization_cmd[COMMAND_PITCH] = indi.u_in.q;
476  stabilization_cmd[COMMAND_YAW] = indi.u_in.r;
477 
478  /* bound the result */
479  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
480  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
481  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
482 }
483 
490 void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight __attribute__((unused)))
491 {
492  /* attitude error */
493  struct FloatQuat att_err;
494  struct FloatQuat *att_quat = stateGetNedToBodyQuat_f();
495  struct FloatQuat quat_sp_f;
496 
497  QUAT_FLOAT_OF_BFP(quat_sp_f, quat_sp);
498  float_quat_inv_comp_norm_shortest(&att_err, att_quat, &quat_sp_f);
499 
500  struct FloatVect3 att_fb;
501 
502 #if TILT_TWIST_CTRL
503  struct FloatQuat tilt;
504  struct FloatQuat twist;
505  float_quat_tilt_twist(&tilt, &twist, &att_err);
506  att_fb.x = tilt.qx;
507  att_fb.y = tilt.qy;
508  att_fb.z = twist.qz;
509 #else
510  att_fb.x = att_err.qx;
511  att_fb.y = att_err.qy;
512  att_fb.z = att_err.qz;
513 #endif
514 
515  struct FloatRates rate_sp;
516  // Divide by rate gain to make it equivalent to a parallel structure
517  rate_sp.p = indi.gains.att.p * att_fb.x / indi.gains.rate.p;
518  rate_sp.q = indi.gains.att.q * att_fb.y / indi.gains.rate.q;
519  rate_sp.r = indi.gains.att.r * att_fb.z / indi.gains.rate.r;
520 
521  // Add feed-forward rates to the attitude feedback part
522  RATES_ADD(rate_sp, stab_att_ff_rates);
523 
524  /* compute the INDI command */
525  stabilization_indi_rate_run(rate_sp, in_flight);
526 }
527 
533 void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
534 {
535  struct FloatQuat q_sp;
536 #if USE_EARTH_BOUND_RC_SETPOINT
537  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
538 #else
539  stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
540 #endif
542 }
543 
550 static inline void lms_estimation(void)
551 {
552  static struct IndiEstimation *est = &indi.est;
553  // Only pass really low frequencies so you don't adapt to noise
554  struct FloatRates *body_rates = stateGetBodyRates_f();
555  filter_pqr(est->u, &indi.u_act_dyn);
556  filter_pqr(est->rate, body_rates);
557 
558  // Calculate the first and second derivatives of the rates and actuators
559  float rate_d_prev[3];
560  float u_d_prev[3];
561  float_vect_copy(rate_d_prev, est->rate_d, 3);
562  float_vect_copy(u_d_prev, est->u_d, 3);
564  finite_difference_from_filter(est->u_d, est->u);
565  finite_difference(est->rate_dd, est->rate_d, rate_d_prev);
566  finite_difference(est->u_dd, est->u_d, u_d_prev);
567 
568  // The inputs are scaled in order to avoid overflows
569  float du[3];
570  float_vect_copy(du, est->u_d, 3);
572  est->g1.p = est->g1.p - (est->g1.p * du[0] - est->rate_dd[0]) * du[0] * est->mu;
573  est->g1.q = est->g1.q - (est->g1.q * du[1] - est->rate_dd[1]) * du[1] * est->mu;
574  float ddu = est->u_dd[2] * INDI_EST_SCALE / PERIODIC_FREQUENCY;
575  float error = (est->g1.r * du[2] + est->g2 * ddu - est->rate_dd[2]);
576  est->g1.r = est->g1.r - error * du[2] * est->mu / 3;
577  est->g2 = est->g2 - error * 1000 * ddu * est->mu / 3;
578 
579  //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...
580  if (est->g1.p < 0.01) { est->g1.p = 0.01; }
581  if (est->g1.q < 0.01) { est->g1.q = 0.01; }
582  if (est->g1.r < 0.01) { est->g1.r = 0.01; }
583  if (est->g2 < 0.01) { est->g2 = 0.01; }
584 
585  if (indi.adaptive) {
586  //Commit the estimated G values and apply the scaling
587  indi.g1.p = est->g1.p * INDI_EST_SCALE;
588  indi.g1.q = est->g1.q * INDI_EST_SCALE;
589  indi.g1.r = est->g1.r * INDI_EST_SCALE;
590  indi.g2 = est->g2 * INDI_EST_SCALE;
591  }
592 }
float q
in rad/s
float p
in rad/s
float r
in rad/s
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_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)
#define FLOAT_RATES_ZERO(_r)
Roation quaternion.
angular rates
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:379
#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
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 float p[2][2]
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.
#define MAX_PPRZ
Definition: paparazzi.h:8
#define PPRZ_ITRIG_SIN(_s, _a)
#define PPRZ_ITRIG_COS(_c, _a)
Generic interface for radio control modules.
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 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)
This function reads rc commands.
void stabilization_indi_enter(void)
Function that resets important values upon engaging INDI.
void stabilization_indi_set_failsafe_setpoint(void)
Function that calculates the failsafe setpoint.
static void finite_difference(float output[3], float new[3], float old[3])
Calculate derivative of an array via finite difference.
void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
runs stabilization indi
static struct FirstOrderLowPass rates_filt_fo[3]
#define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF
static void lms_estimation(void)
This is a Least Mean Squares adaptive filter It estimates the actuator effectiveness online,...
void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
Set attitude setpoint from command in earth axes.
#define STABILIZATION_INDI_FILT_CUTOFF_P
#define STABILIZATION_INDI_FILT_CUTOFF_Q
static void send_att_indi(struct transport_tx *trans, struct link_device *dev)
void stabilization_indi_simple_reset_r_filter_cutoff(float new_cutoff)
static void finite_difference_from_filter(float *output, Butterworth2LowPass *filter)
Caclulate finite difference form a filter array The filter already contains the previous values.
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
#define STABILIZATION_INDI_MAX_RATE
static void send_eff_mat_g_indi_simple(struct transport_tx *trans, struct link_device *dev)
void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
Does the INDI calculations.
void stabilization_indi_init(void)
Function that initializes important values upon engaging INDI.
#define STABILIZATION_INDI_FILT_CUTOFF
void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp)
Set attitude setpoint from stabilization setpoint struct.
#define INDI_EST_SCALE
void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
Set attitude quaternion setpoint from rpy.
struct IndiVariables indi
#define STABILIZATION_INDI_MAX_R
#define STABILIZATION_INDI_FILT_CUTOFF_R
static void filter_pqr(Butterworth2LowPass *filter, struct FloatRates *new_values)
Update butterworth filter for p, q and r of a FloatRates struct.
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
#define STABILIZATION_INDI_FILT_CUTOFF_RDOT
void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat)
static void indi_init_filters(void)
struct FloatRates stab_att_ff_rates
struct Indi_gains gains
Butterworth2LowPass rate[3]
struct FloatRates act_dyn
struct IndiEstimation est
Estimation parameters for adaptive INDI.
Butterworth2LowPass u[3]
struct FloatRates du
Butterworth2LowPass u[3]
struct FloatRates u_act_dyn
struct FloatRates u_in
struct FloatRates rate
struct FloatRates angular_accel_ref
struct FloatRates att
struct FloatRates g1
bool adaptive
Enable adataptive estimation.
struct FloatRates g1
float attitude_max_yaw_rate
Maximum yaw rate in atttiude control in rad/s.
Butterworth2LowPass rate[3]
API to get/set the generic vehicle states.
#define TRUE
Definition: std.h:4
#define FALSE
Definition: std.h:5
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
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
float heading
Definition: wedgebug.c:258