Paparazzi UAS  v6.3_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 
84 struct Indi_gains indi_gains = {
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 
190 static struct FirstOrderLowPass rates_filt_fo[3];
191 
192 struct FloatVect3 body_accel_f;
193 
194 void init_filters(void);
195 void sum_g1_g2(void);
196 
197 #if PERIODIC_TELEMETRY
199 static void send_indi_g(struct transport_tx *trans, struct link_device *dev)
200 {
201  pprz_msg_send_INDI_G(trans, dev, AC_ID, INDI_NUM_ACT, g1_est[0],
202  INDI_NUM_ACT, g1_est[1],
203  INDI_NUM_ACT, g1_est[2],
204  INDI_NUM_ACT, g1_est[3],
205  INDI_NUM_ACT, g2_est);
206 }
207 
208 static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
209 {
210  struct Int32Quat *quat = stateGetNedToBodyQuat_i();
211  pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
216  &(quat->qi),
217  &(quat->qx),
218  &(quat->qy),
219  &(quat->qz));
220 }
221 #endif
222 
227 {
228  // Initialize filters
229  init_filters();
230 
231  AbiBindMsgRPM(RPM_SENSOR_ID, &rpm_ev, rpm_cb);
232  AbiBindMsgTHRUST(THRUST_INCREMENT_ID, &thrust_ev, thrust_cb);
233 
236  float_vect_zero(estimation_rate_d, INDI_NUM_ACT);
237  float_vect_zero(estimation_rate_dd, INDI_NUM_ACT);
239 
240  //Calculate G1G2_PSEUDO_INVERSE
241  sum_g1_g2();
243 
244  // Initialize the array of pointers to the rows of g1g2
245  uint8_t i;
246  for (i = 0; i < INDI_OUTPUTS; i++) {
247  Bwls[i] = g1g2[i];
248  }
249 
250  // Initialize the estimator matrices
251  float_vect_copy(g1_est[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
252  float_vect_copy(g2_est, g2, INDI_NUM_ACT);
253  // Remember the initial matrices
254  float_vect_copy(g1_init[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
255  float_vect_copy(g2_init, g2, INDI_NUM_ACT);
256 
257  // Assume all non-servos are delivering thrust
258  num_thrusters = INDI_NUM_ACT;
259  for (i = 0; i < INDI_NUM_ACT; i++) {
261  }
262 
263 #if PERIODIC_TELEMETRY
266 #endif
267 }
268 
278 {
279  /* reset psi setpoint to current psi angle */
281 
282  float_vect_zero(du_estimation, INDI_NUM_ACT);
283  float_vect_zero(ddu_estimation, INDI_NUM_ACT);
284 }
285 
289 void init_filters(void)
290 {
291  // tau = 1/(2*pi*Fc)
292  float tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF);
293  float tau_est = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF);
294  float sample_time = 1.0 / PERIODIC_FREQUENCY;
295  // Filtering of the gyroscope
296  int8_t i;
297  for (i = 0; i < 3; i++) {
298  init_butterworth_2_low_pass(&measurement_lowpass_filters[i], tau, sample_time, 0.0);
299  init_butterworth_2_low_pass(&estimation_output_lowpass_filters[i], tau_est, sample_time, 0.0);
300  }
301 
302  // Filtering of the actuators
303  for (i = 0; i < INDI_NUM_ACT; i++) {
304  init_butterworth_2_low_pass(&actuator_lowpass_filters[i], tau, sample_time, 0.0);
305  init_butterworth_2_low_pass(&estimation_input_lowpass_filters[i], tau_est, sample_time, 0.0);
306  }
307 
308  // Filtering of the accel body z
309  init_butterworth_2_low_pass(&acceleration_lowpass_filter, tau_est, sample_time, 0.0);
310 
311  // Init rate filter for feedback
312  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)};
313 
314  init_first_order_low_pass(&rates_filt_fo[0], time_constants[0], sample_time, stateGetBodyRates_f()->p);
315  init_first_order_low_pass(&rates_filt_fo[1], time_constants[1], sample_time, stateGetBodyRates_f()->q);
316  init_first_order_low_pass(&rates_filt_fo[2], time_constants[2], sample_time, stateGetBodyRates_f()->r);
317 }
318 
323 {
324  /* set failsafe to zero roll/pitch and current heading */
327  stab_att_sp_quat.qx = 0;
328  stab_att_sp_quat.qy = 0;
330 }
331 
338 {
339  // stab_att_sp_euler.psi still used in ref..
340  stab_att_sp_euler = *rpy;
341 
343 }
344 
352 {
353  // stab_att_sp_euler.psi still used in ref..
355 
356  // compute sp_euler phi/theta for debugging/telemetry
357  /* Rotate horizontal commands to body frame by psi */
359  int32_t s_psi, c_psi;
360  PPRZ_ITRIG_SIN(s_psi, psi);
361  PPRZ_ITRIG_COS(c_psi, psi);
362  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
363  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
364 
366 }
367 
375 void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
376 {
377  /* Propagate the filter on the gyroscopes */
378  struct FloatRates *body_rates = stateGetBodyRates_f();
379  float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r};
380  int8_t i;
381  for (i = 0; i < 3; i++) {
384 
385  //Calculate the angular acceleration via finite difference
387  - measurement_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
388 
389  // Calculate derivatives for estimation
390  float estimation_rate_d_prev = estimation_rate_d[i];
392  PERIODIC_FREQUENCY;
393  estimation_rate_dd[i] = (estimation_rate_d[i] - estimation_rate_d_prev) * PERIODIC_FREQUENCY;
394  }
395 
396  //The rates used for feedback are by default the measured rates.
397  //If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.
398  //Note that due to the delay, the PD controller may need relaxed gains.
399  struct FloatRates rates_filt;
400 #if STABILIZATION_INDI_FILTER_ROLL_RATE
401  rates_filt.p = update_first_order_low_pass(&rates_filt_fo[0], body_rates->p);
402 #else
403  rates_filt.p = body_rates->p;
404 #endif
405 #if STABILIZATION_INDI_FILTER_PITCH_RATE
406  rates_filt.q = update_first_order_low_pass(&rates_filt_fo[1], body_rates->q);
407 #else
408  rates_filt.q = body_rates->q;
409 #endif
410 #if STABILIZATION_INDI_FILTER_YAW_RATE
411  rates_filt.r = update_first_order_low_pass(&rates_filt_fo[2], body_rates->r);
412 #else
413  rates_filt.r = body_rates->r;
414 #endif
415 
416  //calculate the virtual control (reference acceleration) based on a PD controller
417  angular_accel_ref.p = (rate_sp.p - rates_filt.p) * indi_gains.rate.p;
418  angular_accel_ref.q = (rate_sp.q - rates_filt.q) * indi_gains.rate.q;
419  angular_accel_ref.r = (rate_sp.r - rates_filt.r) * indi_gains.rate.r;
420 
421  g2_times_du = 0.0;
422  for (i = 0; i < INDI_NUM_ACT; i++) {
423  g2_times_du += g2[i] * indi_du[i];
424  }
425  //G2 is scaled by INDI_G_SCALING to make it readable
427 
428  float v_thrust = 0.0;
429  if (indi_thrust_increment_set && in_flight) {
430  v_thrust = indi_thrust_increment;
431 
432  //update thrust command such that the current is correctly estimated
433  stabilization_cmd[COMMAND_THRUST] = 0;
434  for (i = 0; i < INDI_NUM_ACT; i++) {
435  stabilization_cmd[COMMAND_THRUST] += actuator_state[i] * -((int32_t) act_is_servo[i] - 1);
436  }
437  stabilization_cmd[COMMAND_THRUST] /= num_thrusters;
438 
439  } else {
440  // incremental thrust
441  for (i = 0; i < INDI_NUM_ACT; i++) {
442  v_thrust +=
443  (stabilization_cmd[COMMAND_THRUST] - actuator_state_filt_vect[i]) * Bwls[3][i];
444  }
445  }
446 
447  // The control objective in array format
451  indi_v[3] = v_thrust;
452 
453  if (in_flight) {
454 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
455  // Calculate the increment for each actuator
456  for (i = 0; i < INDI_NUM_ACT; i++) {
457  indi_du[i] = (g1g2_pseudo_inv[i][0] * indi_v[0])
458  + (g1g2_pseudo_inv[i][1] * indi_v[1])
459  + (g1g2_pseudo_inv[i][2] * indi_v[2])
460  + (g1g2_pseudo_inv[i][3] * indi_v[3]);
461  }
462 #else
463  // Calculate the min and max increments
464  for (i = 0; i < INDI_NUM_ACT; i++) {
468 
469 #ifdef GUIDANCE_INDI_MIN_THROTTLE
470  float airspeed = stateGetAirspeed_f();
471  //limit minimum thrust ap can give
472  if (!act_is_servo[i]) {
474  if (airspeed < 8.0) {
475  du_min[i] = GUIDANCE_INDI_MIN_THROTTLE - actuator_state_filt_vect[i];
476  } else {
477  du_min[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD - actuator_state_filt_vect[i];
478  }
479  }
480  }
481 #endif
482  }
483 
484  // WLS Control Allocator
485  num_iter =
486  wls_alloc(indi_du, indi_v, du_min, du_max, Bwls, 0, 0, Wv, 0, du_pref, 10000, 10);
487 #endif
488 
489  // Add the increments to the actuators
491 
492  // Bound the inputs to the actuators
493  for (i = 0; i < INDI_NUM_ACT; i++) {
494  if (act_is_servo[i]) {
495  BoundAbs(indi_u[i], MAX_PPRZ);
496  } else {
497  if (autopilot_get_motors_on()) {
498  Bound(indi_u[i], 0, MAX_PPRZ);
499  }
500  else {
501  indi_u[i] = -MAX_PPRZ;
502  }
503  }
504  }
505 
506  } else {
507  //Don't increment if not flying (not armed)
508  float_vect_zero(indi_u, INDI_NUM_ACT);
509  float_vect_zero(indi_du, INDI_NUM_ACT);
510  }
511 
512  // Propagate actuator filters
514  for (i = 0; i < INDI_NUM_ACT; i++) {
518 
519  // calculate derivatives for estimation
520  float actuator_state_filt_vectd_prev = actuator_state_filt_vectd[i];
522  PERIODIC_FREQUENCY;
523  actuator_state_filt_vectdd[i] = (actuator_state_filt_vectd[i] - actuator_state_filt_vectd_prev) * PERIODIC_FREQUENCY;
524  }
525 
526  // Use online effectiveness estimation only when flying
527  if (in_flight && indi_use_adaptive) {
528  lms_estimation();
529  }
530 
531  /*Commit the actuator command*/
532  for (i = 0; i < INDI_NUM_ACT; i++) {
533  actuators_pprz[i] = (int16_t) indi_u[i];
534  }
535 
536  // Set the stab_cmd to 42 to indicate that it is not used
537  stabilization_cmd[COMMAND_ROLL] = 42;
538  stabilization_cmd[COMMAND_PITCH] = 42;
539  stabilization_cmd[COMMAND_YAW] = 42;
540 }
541 
548 void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
549 {
550  /* attitude error */
551  struct FloatQuat att_err;
552  struct FloatQuat *att_quat = stateGetNedToBodyQuat_f();
553  struct FloatQuat quat_sp_f;
554 
555  QUAT_FLOAT_OF_BFP(quat_sp_f, quat_sp);
556  float_quat_inv_comp_norm_shortest(&att_err, att_quat, &quat_sp_f);
557 
558  struct FloatVect3 att_fb;
559 
560 #if TILT_TWIST_CTRL
561  struct FloatQuat tilt;
562  struct FloatQuat twist;
563  float_quat_tilt_twist(&tilt, &twist, &att_err);
564  att_fb.x = tilt.qx;
565  att_fb.y = tilt.qy;
566  att_fb.z = twist.qz;
567 #else
568  att_fb.x = att_err.qx;
569  att_fb.y = att_err.qy;
570  att_fb.z = att_err.qz;
571 #endif
572 
573  // local variable to compute rate setpoints based on attitude error
574  struct FloatRates rate_sp;
575 
576  // calculate the virtual control (reference acceleration) based on a PD controller
577  rate_sp.p = indi_gains.att.p * att_fb.x / indi_gains.rate.p;
578  rate_sp.q = indi_gains.att.q * att_fb.y / indi_gains.rate.q;
579  rate_sp.r = indi_gains.att.r * att_fb.z / indi_gains.rate.r;
580 
581  // Possibly we can use some bounding here
582  /*BoundAbs(rate_sp.r, 5.0);*/
583 
584  /* compute the INDI command */
585  stabilization_indi_rate_run(rate_sp, in_flight);
586 
587  // Reset thrust increment boolean
589 }
590 
591 // This function reads rc commands
592 void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
593 {
594  struct FloatQuat q_sp;
595 #if USE_EARTH_BOUND_RC_SETPOINT
596  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
597 #else
598  stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
599 #endif
600 
602 }
603 
611 {
612 #if INDI_RPM_FEEDBACK
613  float_vect_copy(actuator_state, act_obs, INDI_NUM_ACT);
614 #else
615  //actuator dynamics
616  int8_t i;
617  float UNUSED prev_actuator_state;
618  for (i = 0; i < INDI_NUM_ACT; i++) {
619  prev_actuator_state = actuator_state[i];
620 
622  + act_dyn[i] * (indi_u[i] - actuator_state[i]);
623 
624 #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
625  if ((actuator_state[i] - prev_actuator_state) > act_rate_limit[i]) {
626  actuator_state[i] = prev_actuator_state + act_rate_limit[i];
627  } else if ((actuator_state[i] - prev_actuator_state) < -act_rate_limit[i]) {
628  actuator_state[i] = prev_actuator_state - act_rate_limit[i];
629  }
630 #endif
631  }
632 
633 #endif
634 }
635 
646 void calc_g1_element(float ddx_error, int8_t i, int8_t j, float mu)
647 {
648  g1_est[i][j] = g1_est[i][j] - du_estimation[j] * mu * ddx_error;
649 }
650 
660 void calc_g2_element(float ddx_error, int8_t j, float mu)
661 {
662  g2_est[j] = g2_est[j] - ddu_estimation[j] * mu * ddx_error;
663 }
664 
670 void lms_estimation(void)
671 {
672 
673  // Get the acceleration in body axes
674  struct Int32Vect3 *body_accel_i;
675  body_accel_i = stateGetAccelBody_i();
676  ACCELS_FLOAT_OF_BFP(body_accel_f, *body_accel_i);
677 
678  // Filter the acceleration in z axis
680 
681  // Calculate the derivative of the acceleration via finite difference
682  float indi_accel_d = (acceleration_lowpass_filter.o[0]
683  - acceleration_lowpass_filter.o[1]) * PERIODIC_FREQUENCY;
684 
685  // Use xml setting for adaptive mu for lms
686  // Set default value if not defined
687  #ifndef STABILIZATION_INDI_ADAPTIVE_MU
688  float adaptive_mu_lr = 0.001;
689  #else
690  float adaptive_mu_lr = STABILIZATION_INDI_ADAPTIVE_MU;
691  #endif
692 
693  // scale the inputs to avoid numerical errors
694  float_vect_smul(du_estimation, actuator_state_filt_vectd, adaptive_mu_lr, INDI_NUM_ACT);
695  float_vect_smul(ddu_estimation, actuator_state_filt_vectdd, adaptive_mu_lr / PERIODIC_FREQUENCY, INDI_NUM_ACT);
696 
697  float ddx_estimation[INDI_OUTPUTS] = {estimation_rate_dd[0], estimation_rate_dd[1], estimation_rate_dd[2], indi_accel_d};
698 
699  //Estimation of G
700  // TODO: only estimate when du_norm2 is large enough (enough input)
701  /*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];*/
702  int8_t i;
703  for (i = 0; i < INDI_OUTPUTS; i++) {
704  // Calculate the error between prediction and measurement
705  float ddx_error = - ddx_estimation[i];
706  int8_t j;
707  for (j = 0; j < INDI_NUM_ACT; j++) {
708  ddx_error += g1_est[i][j] * du_estimation[j];
709  if (i == 2) {
710  // Changing the momentum of the rotors gives a counter torque
711  ddx_error += g2_est[j] * ddu_estimation[j];
712  }
713  }
714 
715  // when doing the yaw axis, also use G2
716  if (i == 2) {
717  for (j = 0; j < INDI_NUM_ACT; j++) {
718  calc_g2_element(ddx_error, j, mu2);
719  }
720  } else if (i == 3) {
721  // If the acceleration change is very large (rough landing), don't adapt
722  if (fabs(indi_accel_d) > 60.0) {
723  ddx_error = 0.0;
724  }
725  }
726 
727  // Calculate the row of the G1 matrix corresponding to this axis
728  for (j = 0; j < INDI_NUM_ACT; j++) {
729  calc_g1_element(ddx_error, i, j, mu1[i]);
730  }
731  }
732 
733  bound_g_mat();
734 
735  // Save the calculated matrix to G1 and G2
736  // until thrust is included, first part of the array
737  float_vect_copy(g1[0], g1_est[0], INDI_OUTPUTS * INDI_NUM_ACT);
738  float_vect_copy(g2, g2_est, INDI_NUM_ACT);
739 
740  // Calculate sum of G1 and G2 for Bwls
741  sum_g1_g2();
742 
743 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
744  // Calculate the inverse of (G1+G2)
746 #endif
747 }
748 
753 void sum_g1_g2(void) {
754  int8_t i;
755  int8_t j;
756  for (i = 0; i < INDI_OUTPUTS; i++) {
757  for (j = 0; j < INDI_NUM_ACT; j++) {
758  if (i != 2) {
759  g1g2[i][j] = g1[i][j] / INDI_G_SCALING;
760  } else {
761  g1g2[i][j] = (g1[i][j] + g2[j]) / INDI_G_SCALING;
762  }
763  }
764  }
765 }
766 
772 {
773  //G1G2*transpose(G1G2)
774  //calculate matrix multiplication of its transpose INDI_OUTPUTSxnum_act x num_actxINDI_OUTPUTS
775  float element = 0;
776  int8_t row;
777  int8_t col;
778  int8_t i;
779  for (row = 0; row < INDI_OUTPUTS; row++) {
780  for (col = 0; col < INDI_OUTPUTS; col++) {
781  element = 0;
782  for (i = 0; i < INDI_NUM_ACT; i++) {
783  element = element + g1g2[row][i] * g1g2[col][i];
784  }
785  g1g2_trans_mult[row][col] = element;
786  }
787  }
788 
789  //there are numerical errors if the scaling is not right.
790  float_vect_scale(g1g2_trans_mult[0], 1000.0, INDI_OUTPUTS * INDI_OUTPUTS);
791 
792  //inverse of 4x4 matrix
794 
795  //scale back
796  float_vect_scale(g1g2inv[0], 1000.0, INDI_OUTPUTS * INDI_OUTPUTS);
797 
798  //G1G2'*G1G2inv
799  //calculate matrix multiplication INDI_NUM_ACTxINDI_OUTPUTS x INDI_OUTPUTSxINDI_OUTPUTS
800  for (row = 0; row < INDI_NUM_ACT; row++) {
801  for (col = 0; col < INDI_OUTPUTS; col++) {
802  element = 0;
803  for (i = 0; i < INDI_OUTPUTS; i++) {
804  element = element + g1g2[i][row] * g1g2inv[col][i];
805  }
806  g1g2_pseudo_inv[row][col] = element;
807  }
808  }
809 }
810 
811 static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t UNUSED *rpm, uint8_t UNUSED num_act)
812 {
813 #if INDI_RPM_FEEDBACK
814  int8_t i;
815  for (i = 0; i < num_act; i++) {
816  act_obs[i] = (rpm[i] - get_servo_min(i));
817  act_obs[i] *= (MAX_PPRZ / (float)(get_servo_max(i) - get_servo_min(i)));
818  Bound(act_obs[i], 0, MAX_PPRZ);
819  }
820 #endif
821 }
822 
826 static void thrust_cb(uint8_t UNUSED sender_id, float thrust_increment)
827 {
828  indi_thrust_increment = thrust_increment;
830 }
831 
832 static void bound_g_mat(void)
833 {
834  int8_t i;
835  int8_t j;
836  for (j = 0; j < INDI_NUM_ACT; j++) {
837  float max_limit;
838  float min_limit;
839 
840  // Limit the values of the estimated G1 matrix
841  for (i = 0; i < INDI_OUTPUTS; i++) {
842  if (g1_init[i][j] > 0.0) {
843  max_limit = g1_init[i][j] * INDI_ALLOWED_G_FACTOR;
844  min_limit = g1_init[i][j] / INDI_ALLOWED_G_FACTOR;
845  } else {
846  max_limit = g1_init[i][j] / INDI_ALLOWED_G_FACTOR;
847  min_limit = g1_init[i][j] * INDI_ALLOWED_G_FACTOR;
848  }
849 
850  if (g1_est[i][j] > max_limit) {
851  g1_est[i][j] = max_limit;
852  }
853  if (g1_est[i][j] < min_limit) {
854  g1_est[i][j] = min_limit;
855  }
856  }
857 
858  // Do the same for the G2 matrix
859  if (g2_init[j] > 0.0) {
860  max_limit = g2_init[j] * INDI_ALLOWED_G_FACTOR;
861  min_limit = g2_init[j] / INDI_ALLOWED_G_FACTOR;
862  } else {
863  max_limit = g2_init[j] / INDI_ALLOWED_G_FACTOR;
864  min_limit = g2_init[j] * INDI_ALLOWED_G_FACTOR;
865  }
866 
867  if (g2_est[j] > max_limit) {
868  g2_est[j] = max_limit;
869  }
870  if (g2_est[j] < min_limit) {
871  g2_est[j] = min_limit;
872  }
873  }
874 }
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
#define RPM_SENSOR_ID
#define THRUST_INCREMENT_ID
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:244
float q
in rad/s
float p
in rad/s
float r
in rad/s
static void float_vect_sum(float *o, const float *a, const float *b, const int n)
o = a + b
static void float_vect_zero(float *a, const int n)
a = 0
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
static void float_vect_smul(float *o, const float *a, const float s, const int n)
o = a * s
static void float_vect_scale(float *a, const float s, const int n)
a *= s
bool float_mat_inv_4d(float invOut[16], float mat_in[16])
4x4 Matrix inverse
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)
Roation quaternion.
angular rates
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:752
#define QUAT_FLOAT_OF_BFP(_qf, _qi)
Definition: pprz_algebra.h:745
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
#define INT32_TRIG_FRAC
void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e)
Quaternion from Euler angles.
euler angles
Rotation quaternion.
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1113
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
Definition: state.h:953
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
static float p[2][2]
Simple first order low pass filter with bilinear transform.
float o[2]
output history
static float update_first_order_low_pass(struct FirstOrderLowPass *filter, float value)
Update first order low pass filter state with a new value.
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
static void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, float sample_time, float value)
Init first order low pass filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
First order low pass filter structure.
Second order low pass filter structure.
Hardware independent API for actuators (servos, motor controllers).
static float mu
Physical model parameters.
Definition: nps_fdm_rover.c:60
#define MAX_PPRZ
Definition: paparazzi.h:8
Paparazzi floating point algebra.
#define PPRZ_ITRIG_SIN(_s, _a)
#define PPRZ_ITRIG_COS(_c, _a)
Generic interface for radio control modules.
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:90
#define GUIDANCE_H_MODE_NAV
Definition: guidance_h.h:60
#define GUIDANCE_H_MODE_HOVER
Definition: guidance_h.h:59
uint8_t last_wp UNUSED
Definition: navigation.c:101
General attitude stabilization interface for rotorcrafts.
uint16_t rpm
Definition: rpm_sensor.c:33
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading)
Quaternion transformation functions.
int32_t stabilization_attitude_get_heading_i(void)
void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree, bool coordinated_turn)
void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree, bool coordinated_turn)
Read attitude setpoint from RC as quaternion Interprets the stick positions as axes.
Read an attitude setpoint from the RC.
static void calc_g1g2_pseudo_inv(void)
Function that calculates the pseudo-inverse of (G1+G2).
static void calc_g1_element(float dx_error, int8_t i, int8_t j, float mu_extra)
float du_max[INDI_NUM_ACT]
Butterworth2LowPass acceleration_lowpass_filter
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
void stabilization_indi_enter(void)
Function that resets important values upon engaging INDI.
static float Wv[INDI_OUTPUTS]
float act_pref[INDI_NUM_ACT]
float * Bwls[INDI_OUTPUTS]
int32_t stabilization_att_indi_cmd[COMMANDS_NB]
struct Indi_gains indi_gains
void stabilization_indi_set_failsafe_setpoint(void)
Function that calculates the failsafe setpoint.
bool act_is_servo[INDI_NUM_ACT]
void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
static struct FirstOrderLowPass rates_filt_fo[3]
float angular_acceleration[3]
int32_t num_thrusters
void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
float actuator_state_filt_vectd[INDI_NUM_ACT]
abi_event rpm_ev
float r_filt
#define STABILIZATION_INDI_FILT_CUTOFF_P
static void bound_g_mat(void)
float mu1[INDI_OUTPUTS]
#define STABILIZATION_INDI_FILT_CUTOFF_Q
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]
static void calc_g2_element(float dx_error, int8_t j, float mu_extra)
float q_filt
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
float estimation_rate_d[INDI_NUM_ACT]
float indi_thrust_increment
float du_estimation[INDI_NUM_ACT]
float actuator_state_filt_vectdd[INDI_NUM_ACT]
abi_event thrust_ev
Butterworth2LowPass measurement_lowpass_filters[3]
bool indi_thrust_increment_set
void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
float g2[INDI_NUM_ACT]
struct FloatVect3 body_accel_f
float ddu_estimation[INDI_NUM_ACT]
float estimation_rate_dd[INDI_NUM_ACT]
float mu2
float act_obs[INDI_NUM_ACT]
float g1[INDI_OUTPUTS][INDI_NUM_ACT]
void stabilization_indi_init(void)
Function that initializes important values upon engaging INDI.
static void send_indi_g(struct transport_tx *trans, struct link_device *dev)
float act_dyn[INDI_NUM_ACT]
float du_min[INDI_NUM_ACT]
float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS]
void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
float actuator_state_filt_vect[INDI_NUM_ACT]
struct FloatRates angular_accel_ref
float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS]
static void rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act)
#define STABILIZATION_INDI_FILT_CUTOFF_R
bool indi_use_adaptive
float g2_init[INDI_NUM_ACT]
float du_pref[INDI_NUM_ACT]
float g2_times_du
Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT]
void sum_g1_g2(void)
Function that sums g1 and g2 to obtain the g1g2 matrix It also undoes the scaling that was done to ma...
float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS]
float g2_est[INDI_NUM_ACT]
float g1_init[INDI_OUTPUTS][INDI_NUM_ACT]
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
float actuator_state[INDI_NUM_ACT]
static void thrust_cb(uint8_t sender_id, float thrust_increment)
static void lms_estimation(void)
Function that estimates the control effectiveness of each actuator online.
#define INDI_ALLOWED_G_FACTOR
float g1_est[INDI_OUTPUTS][INDI_NUM_ACT]
float indi_v[INDI_OUTPUTS]
int num_iter
float indi_u[INDI_NUM_ACT]
static void get_actuator_state(void)
Function that tries to get actuator feedback.
Butterworth2LowPass actuator_lowpass_filters[INDI_NUM_ACT]
void init_filters(void)
Function that resets the filters to zeros.
Butterworth2LowPass estimation_output_lowpass_filters[3]
float indi_du[INDI_NUM_ACT]
#define INDI_G_SCALING
#define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF
#define STABILIZATION_INDI_FILT_CUTOFF
struct FloatRates rate
struct FloatRates att
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
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
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
float heading
Definition: wedgebug.c:258
int wls_alloc(float *u, float *v, float *umin, float *umax, float **B, float *u_guess, float *W_init, float *Wv, float *Wu, float *up, float gamma_sq, int imax)
active set algorithm for control allocation
Definition: wls_alloc.c:112