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 
36 
37 #include "state.h"
38 #include "generated/airframe.h"
39 #include "paparazzi.h"
42 
43 #if defined(STABILIZATION_INDI_ACT_DYN_P) && !defined(STABILIZATION_INDI_ACT_DYN_Q) && !defined(STABILIZATION_INDI_ACT_DYN_R)
44 #warning STABILIZATION_INDI_ACT_DYN is deprecated, use STABILIZATION_INDI_ACT_FREQ instead.
45 #warning You now have to define the continuous time corner frequency in rad/s of the actuators.
46 #warning "Use -ln(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old values."
47 #else
48 #if !defined(STABILIZATION_INDI_ACT_FREQ_P) && !defined(STABILIZATION_INDI_ACT_FREQ_Q) && !defined(STABILIZATION_INDI_ACT_FREQ_R)
49 #warning You have to define the corner frequency of the first order actuator dynamics model in rad/s!
50 #endif
51 #endif
52 
53 // these parameters are used in the filtering of the angular acceleration
54 // define them in the airframe file if different values are required
55 #ifndef STABILIZATION_INDI_FILT_CUTOFF
56 #define STABILIZATION_INDI_FILT_CUTOFF 8.0
57 #endif
58 
59 // the yaw sometimes requires more filtering
60 #ifndef STABILIZATION_INDI_FILT_CUTOFF_RDOT
61 #define STABILIZATION_INDI_FILT_CUTOFF_RDOT STABILIZATION_INDI_FILT_CUTOFF
62 #endif
63 
64 #ifndef STABILIZATION_INDI_MAX_RATE
65 #define STABILIZATION_INDI_MAX_RATE 6.0
66 #endif
67 
68 #if STABILIZATION_INDI_USE_ADAPTIVE
69 #warning "Use caution with adaptive indi. See the wiki for more info"
70 #endif
71 
72 #ifndef STABILIZATION_INDI_MAX_R
73 #define STABILIZATION_INDI_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R
74 #endif
75 
76 #ifndef STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF
77 #define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF 4.0
78 #endif
79 
80 #ifdef STABILIZATION_INDI_FILT_CUTOFF_P
81 #define STABILIZATION_INDI_FILTER_ROLL_RATE TRUE
82 #else
83 #define STABILIZATION_INDI_FILT_CUTOFF_P 20.0
84 #endif
85 
86 #ifdef STABILIZATION_INDI_FILT_CUTOFF_Q
87 #define STABILIZATION_INDI_FILTER_PITCH_RATE TRUE
88 #else
89 #define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0
90 #endif
91 
92 #ifdef STABILIZATION_INDI_FILT_CUTOFF_R
93 #define STABILIZATION_INDI_FILTER_YAW_RATE TRUE
94 #else
95 #define STABILIZATION_INDI_FILT_CUTOFF_R 20.0
96 #endif
97 
100 
101 static struct FirstOrderLowPass rates_filt_fo[3];
102 
103 static inline void lms_estimation(void);
104 static void indi_init_filters(void);
105 
106 //The G values are scaled to avoid numerical problems during the estimation
107 #define INDI_EST_SCALE 0.001
108 
109 struct IndiVariables indi = {
111  .max_rate = STABILIZATION_INDI_MAX_RATE,
112  .attitude_max_yaw_rate = STABILIZATION_INDI_MAX_R,
113 
114  .g1 = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R},
115  .g2 = STABILIZATION_INDI_G2_R,
116  .gains = {
117  .att = {
118  STABILIZATION_INDI_REF_ERR_P,
119  STABILIZATION_INDI_REF_ERR_Q,
120  STABILIZATION_INDI_REF_ERR_R
121  },
122  .rate = {
123  STABILIZATION_INDI_REF_RATE_P,
124  STABILIZATION_INDI_REF_RATE_Q,
125  STABILIZATION_INDI_REF_RATE_R
126  },
127  },
128 
129  /* Estimation parameters for adaptive INDI */
130  .est = {
131  .g1 = {
132  STABILIZATION_INDI_G1_P / INDI_EST_SCALE,
133  STABILIZATION_INDI_G1_Q / INDI_EST_SCALE,
134  STABILIZATION_INDI_G1_R / INDI_EST_SCALE
135  },
136  .g2 = STABILIZATION_INDI_G2_R / INDI_EST_SCALE,
137  .mu = STABILIZATION_INDI_ADAPTIVE_MU,
138  },
139 
140 #if STABILIZATION_INDI_USE_ADAPTIVE
141  .adaptive = TRUE,
142 #else
143  .adaptive = FALSE,
144 #endif
145 };
146 
147 #if PERIODIC_TELEMETRY
149 
150 static void send_att_indi(struct transport_tx *trans, struct link_device *dev)
151 {
152  float zero = 0.0;
153  pprz_msg_send_STAB_ATTITUDE(trans, dev, AC_ID,
154  &zero, &zero, &zero, // att
155  &zero, &zero, &zero, // att.ref
156  &indi.rate[0].o[0], // rate
157  &indi.rate[1].o[0],
158  &indi.rate[2].o[0],
159  &zero, &zero, &zero, // rate.ref
160  &indi.rate_d[0], // ang.acc = rate.diff
161  &indi.rate_d[1],
162  &indi.rate_d[2],
163  &indi.angular_accel_ref.p, // ang.acc.ref
166  1, &zero, // inputs
167  1, &zero); // outputs
168 }
169 static void send_eff_mat_g_indi_simple(struct transport_tx *trans, struct link_device *dev)
170 {
171  //The estimated G values are scaled, so scale them back before sending
172  struct FloatRates g1_disp;
173  RATES_SMUL(g1_disp, indi.est.g1, INDI_EST_SCALE);
174  float g2_disp = indi.est.g2 * INDI_EST_SCALE;
175  float zero = 0.0;
176  pprz_msg_send_EFF_MAT_G(trans, dev, AC_ID,
177  1, &zero,
178  1, &zero,
179  1, &zero,
180  1, &g1_disp.p,
181  1, &g1_disp.q,
182  1, &g1_disp.r,
183  1, &g2_disp,
184  1, &zero);
185 }
186 
187 static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
188 {
189  struct Int32Quat *quat = stateGetNedToBodyQuat_i();
190  pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
195  &(quat->qi),
196  &(quat->qx),
197  &(quat->qy),
198  &(quat->qz));
199 }
200 #endif
201 
203 {
204  // Initialize filters
206 
207 #ifdef STABILIZATION_INDI_ACT_FREQ_P
208  indi.act_dyn.p = 1-exp(-STABILIZATION_INDI_ACT_FREQ_P/PERIODIC_FREQUENCY);
209  indi.act_dyn.q = 1-exp(-STABILIZATION_INDI_ACT_FREQ_Q/PERIODIC_FREQUENCY);
210  indi.act_dyn.r = 1-exp(-STABILIZATION_INDI_ACT_FREQ_R/PERIODIC_FREQUENCY);
211 #else
212  indi.act_dyn.p = STABILIZATION_INDI_ACT_DYN_P;
213  indi.act_dyn.q = STABILIZATION_INDI_ACT_DYN_Q;
214  indi.act_dyn.r = STABILIZATION_INDI_ACT_DYN_R;
215 #endif
216 
217 #if PERIODIC_TELEMETRY
218  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE, send_att_indi);
221 #endif
222 }
223 
225 {
226  // tau = 1/(2*pi*Fc)
227  float tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF);
228  float tau_rdot = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_RDOT);
229  float tau_axis[3] = {tau, tau, tau_rdot};
230  float tau_est = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF);
231  float sample_time = 1.0 / PERIODIC_FREQUENCY;
232  // Filtering of gyroscope and actuators
233  for (int8_t i = 0; i < 3; i++) {
234  init_butterworth_2_low_pass(&indi.u[i], tau_axis[i], sample_time, 0.0);
235  init_butterworth_2_low_pass(&indi.rate[i], tau_axis[i], sample_time, 0.0);
236  init_butterworth_2_low_pass(&indi.est.u[i], tau_est, sample_time, 0.0);
237  init_butterworth_2_low_pass(&indi.est.rate[i], tau_est, sample_time, 0.0);
238  }
239 
240  // Init rate filter for feedback
241  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)};
242 
243  init_first_order_low_pass(&rates_filt_fo[0], time_constants[0], sample_time, stateGetBodyRates_f()->p);
244  init_first_order_low_pass(&rates_filt_fo[1], time_constants[1], sample_time, stateGetBodyRates_f()->q);
245  init_first_order_low_pass(&rates_filt_fo[2], time_constants[2], sample_time, stateGetBodyRates_f()->r);
246 }
247 
248 // Callback function for setting cutoff frequency for r
250  float sample_time = 1.0 / PERIODIC_FREQUENCY;
251  indi.cutoff_r = new_cutoff;
252  float time_constant = 1.0/(2.0 * M_PI * indi.cutoff_r);
253  init_first_order_low_pass(&rates_filt_fo[2], time_constant, sample_time, stateGetBodyRates_f()->r);
254 }
255 
257 {
258  /* reset psi setpoint to current psi angle */
260 
264 
265  // Re-initialize filters
267 }
268 
275 static inline void filter_pqr(Butterworth2LowPass *filter, struct FloatRates *new_values)
276 {
277  update_butterworth_2_low_pass(&filter[0], new_values->p);
278  update_butterworth_2_low_pass(&filter[1], new_values->q);
279  update_butterworth_2_low_pass(&filter[2], new_values->r);
280 }
281 
289 static inline void finite_difference_from_filter(float *output, Butterworth2LowPass *filter)
290 {
291  for (int8_t i = 0; i < 3; i++) {
292  output[i] = (filter[i].o[0] - filter[i].o[1]) * PERIODIC_FREQUENCY;
293  }
294 }
295 
303 static inline void finite_difference(float output[3], float new[3], float old[3])
304 {
305  for (int8_t i = 0; i < 3; i++) {
306  output[i] = (new[i] - old[i])*PERIODIC_FREQUENCY;
307  }
308 }
309 
318 void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
319 {
320 
321  //Propagate input filters
322  //first order actuator dynamics
326 
327  // Propagate the filter on the gyroscopes and actuators
328  struct FloatRates *body_rates = stateGetBodyRates_f();
330  filter_pqr(indi.rate, body_rates);
331 
332  // Calculate the derivative of the rates
334 
335  //The rates used for feedback are by default the measured rates.
336  //If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.
337  //Note that due to the delay, the PD controller may need relaxed gains.
338  struct FloatRates rates_filt;
339 #if STABILIZATION_INDI_FILTER_ROLL_RATE
340  rates_filt.p = update_first_order_low_pass(&rates_filt_fo[0], body_rates->p);
341 #else
342  rates_filt.p = body_rates->p;
343 #endif
344 #if STABILIZATION_INDI_FILTER_PITCH_RATE
345  rates_filt.q = update_first_order_low_pass(&rates_filt_fo[1], body_rates->q);
346 #else
347  rates_filt.q = body_rates->q;
348 #endif
349 #if STABILIZATION_INDI_FILTER_YAW_RATE
350  rates_filt.r = update_first_order_low_pass(&rates_filt_fo[2], body_rates->r);
351 #else
352  rates_filt.r = body_rates->r;
353 #endif
354 
355  //This lets you impose a maximum yaw rate.
356  struct FloatRates rate_sp = stab_sp_to_rates_f(sp);
357  BoundAbs(rate_sp.r, indi.attitude_max_yaw_rate);
358 
359  // Compute reference angular acceleration:
360  indi.angular_accel_ref.p = (rate_sp.p - rates_filt.p) * indi.gains.rate.p;
361  indi.angular_accel_ref.q = (rate_sp.q - rates_filt.q) * indi.gains.rate.q;
362  indi.angular_accel_ref.r = (rate_sp.r - rates_filt.r) * indi.gains.rate.r;
363 
364  //Increment in angular acceleration requires increment in control input
365  //G1 is the control effectiveness. In the yaw axis, we need something additional: G2.
366  //It takes care of the angular acceleration caused by the change in rotation rate of the propellers
367  //(they have significant inertia, see the paper mentioned in the header for more explanation)
368  indi.du.p = 1.0 / indi.g1.p * (indi.angular_accel_ref.p - indi.rate_d[0]);
369  indi.du.q = 1.0 / indi.g1.q * (indi.angular_accel_ref.q - indi.rate_d[1]);
370  indi.du.r = 1.0 / (indi.g1.r + indi.g2) * (indi.angular_accel_ref.r - indi.rate_d[2] + indi.g2 * indi.du.r);
371 
372  //Don't increment if thrust is off and on the ground
373  //without this the inputs will increment to the maximum before even getting in the air.
374  if (th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z) < 300 && !in_flight) {
376 
377  // If on the gournd, no increments, just proportional control
378  indi.u_in.p = indi.du.p;
379  indi.u_in.q = indi.du.q;
380  indi.u_in.r = indi.du.r;
381  } else {
382  //add the increment to the total control input
383  indi.u_in.p = indi.u[0].o[0] + indi.du.p;
384  indi.u_in.q = indi.u[1].o[0] + indi.du.q;
385  indi.u_in.r = indi.u[2].o[0] + indi.du.r;
386 
387  // only run the estimation if the commands are not zero.
388  lms_estimation();
389  }
390 
391  //bound the total control input
392 #if STABILIZATION_INDI_FULL_AUTHORITY
393  Bound(indi.u_in.p, -9600, 9600);
394  Bound(indi.u_in.q, -9600, 9600);
395  float rlim = 9600 - fabs(indi.u_in.q);
396  Bound(indi.u_in.r, -rlim, rlim);
397  Bound(indi.u_in.r, -9600, 9600);
398 #else
399  Bound(indi.u_in.p, -4500, 4500);
400  Bound(indi.u_in.q, -4500, 4500);
401  Bound(indi.u_in.r, -4500, 4500);
402 #endif
403 
404  /* INDI feedback */
405  cmd[COMMAND_ROLL] = indi.u_in.p;
406  cmd[COMMAND_PITCH] = indi.u_in.q;
407  cmd[COMMAND_YAW] = indi.u_in.r;
408  cmd[COMMAND_THRUST] = th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z);
409 
410  /* bound the result */
411  BoundAbs(cmd[COMMAND_ROLL], MAX_PPRZ);
412  BoundAbs(cmd[COMMAND_PITCH], MAX_PPRZ);
413  BoundAbs(cmd[COMMAND_YAW], MAX_PPRZ);
414  BoundAbs(cmd[COMMAND_THRUST], MAX_PPRZ);
415 }
416 
423 void stabilization_indi_attitude_run(bool in_flight, struct StabilizationSetpoint *att_sp, struct ThrustSetpoint *thrust, int32_t *cmd)
424 {
425  stab_att_sp_euler = stab_sp_to_eulers_i(att_sp); // stab_att_sp_euler.psi still used in ref..
426  stab_att_sp_quat = stab_sp_to_quat_i(att_sp); // quat attitude setpoint
427 
428  /* attitude error in float */
429  struct FloatQuat att_err;
430  struct FloatQuat *att_quat = stateGetNedToBodyQuat_f();
431  struct FloatQuat quat_sp = stab_sp_to_quat_f(att_sp);
432 
433  float_quat_inv_comp_norm_shortest(&att_err, att_quat, &quat_sp);
434 
435  struct FloatVect3 att_fb;
436 #if TILT_TWIST_CTRL
437  struct FloatQuat tilt;
438  struct FloatQuat twist;
439  float_quat_tilt_twist(&tilt, &twist, &att_err);
440  att_fb.x = tilt.qx;
441  att_fb.y = tilt.qy;
442  att_fb.z = twist.qz;
443 #else
444  att_fb.x = att_err.qx;
445  att_fb.y = att_err.qy;
446  att_fb.z = att_err.qz;
447 #endif
448 
449  struct FloatRates rate_sp;
450  // Divide by rate gain to make it equivalent to a parallel structure
451  rate_sp.p = indi.gains.att.p * att_fb.x / indi.gains.rate.p;
452  rate_sp.q = indi.gains.att.q * att_fb.y / indi.gains.rate.q;
453  rate_sp.r = indi.gains.att.r * att_fb.z / indi.gains.rate.r;
454 
455  // Add feed-forward rates to the attitude feedback part
456  struct FloatRates ff_rates = stab_sp_to_rates_f(att_sp);
457  RATES_ADD(rate_sp, ff_rates);
458 
459  /* compute the INDI command */
460  struct StabilizationSetpoint sp = stab_sp_from_rates_f(&rate_sp);
461  stabilization_indi_rate_run(in_flight, &sp, thrust, cmd);
462 }
463 
470 static inline void lms_estimation(void)
471 {
472  static struct IndiEstimation *est = &indi.est;
473  // Only pass really low frequencies so you don't adapt to noise
474  struct FloatRates *body_rates = stateGetBodyRates_f();
475  filter_pqr(est->u, &indi.u_act_dyn);
476  filter_pqr(est->rate, body_rates);
477 
478  // Calculate the first and second derivatives of the rates and actuators
479  float rate_d_prev[3];
480  float u_d_prev[3];
481  float_vect_copy(rate_d_prev, est->rate_d, 3);
482  float_vect_copy(u_d_prev, est->u_d, 3);
484  finite_difference_from_filter(est->u_d, est->u);
485  finite_difference(est->rate_dd, est->rate_d, rate_d_prev);
486  finite_difference(est->u_dd, est->u_d, u_d_prev);
487 
488  // The inputs are scaled in order to avoid overflows
489  float du[3];
490  float_vect_copy(du, est->u_d, 3);
492  est->g1.p = est->g1.p - (est->g1.p * du[0] - est->rate_dd[0]) * du[0] * est->mu;
493  est->g1.q = est->g1.q - (est->g1.q * du[1] - est->rate_dd[1]) * du[1] * est->mu;
494  float ddu = est->u_dd[2] * INDI_EST_SCALE / PERIODIC_FREQUENCY;
495  float error = (est->g1.r * du[2] + est->g2 * ddu - est->rate_dd[2]);
496  est->g1.r = est->g1.r - error * du[2] * est->mu / 3;
497  est->g2 = est->g2 - error * 1000 * ddu * est->mu / 3;
498 
499  //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...
500  if (est->g1.p < 0.01) { est->g1.p = 0.01; }
501  if (est->g1.q < 0.01) { est->g1.q = 0.01; }
502  if (est->g1.r < 0.01) { est->g1.r = 0.01; }
503  if (est->g2 < 0.01) { est->g2 = 0.01; }
504 
505  if (indi.adaptive) {
506  //Commit the estimated G values and apply the scaling
507  indi.g1.p = est->g1.p * INDI_EST_SCALE;
508  indi.g1.q = est->g1.q * INDI_EST_SCALE;
509  indi.g1.r = est->g1.r * INDI_EST_SCALE;
510  indi.g2 = est->g2 * INDI_EST_SCALE;
511  }
512 }
513 
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 RATES_ADD(_a, _b)
Definition: pprz_algebra.h:344
int32_t psi
in rad with INT32_ANGLE_FRAC
euler angles
Rotation quaternion.
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1113
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
Generic interface for radio control modules.
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)
struct StabilizationSetpoint stab_sp_from_rates_f(struct FloatRates *rates)
struct FloatQuat stab_sp_to_quat_f(struct StabilizationSetpoint *sp)
int32_t th_sp_to_thrust_i(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
#define THRUST_AXIS_Z
Quaternion transformation functions.
int32_t stabilization_attitude_get_heading_i(void)
Get attitude heading as int (avoiding jumps)
Read an attitude setpoint from the RC.
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
void stabilization_indi_enter(void)
Function that resets important values upon engaging INDI.
static void finite_difference(float output[3], float new[3], float old[3])
Calculate derivative of an array via finite difference.
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,...
#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
#define STABILIZATION_INDI_MAX_RATE
static void send_eff_mat_g_indi_simple(struct transport_tx *trans, struct link_device *dev)
void stabilization_indi_init(void)
Function that initializes important values upon engaging INDI.
#define STABILIZATION_INDI_FILT_CUTOFF
#define INDI_EST_SCALE
void stabilization_indi_attitude_run(bool in_flight, struct StabilizationSetpoint *att_sp, struct ThrustSetpoint *thrust, int32_t *cmd)
runs stabilization indi
void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Does the INDI calculations.
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
#define STABILIZATION_INDI_FILT_CUTOFF_RDOT
static void indi_init_filters(void)
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:53
union StabilizationSetpoint::@278 sp
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
Definition: stabilization.h:82
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
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