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