Paparazzi UAS  v6.0_unstable-20-gfdbd467-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 float du_min[INDI_NUM_ACT];
52 float du_max[INDI_NUM_ACT];
53 float du_pref[INDI_NUM_ACT];
54 float indi_v[INDI_OUTPUTS];
55 float *Bwls[INDI_OUTPUTS];
56 int num_iter = 0;
57 
58 static void lms_estimation(void);
59 static void get_actuator_state(void);
60 static void calc_g1_element(float dx_error, int8_t i, int8_t j, float mu_extra);
61 static void calc_g2_element(float dx_error, int8_t j, float mu_extra);
62 static void calc_g1g2_pseudo_inv(void);
63 static void bound_g_mat(void);
64 
67  .att = {
68  STABILIZATION_INDI_REF_ERR_P,
69  STABILIZATION_INDI_REF_ERR_Q,
70  STABILIZATION_INDI_REF_ERR_R
71  },
72  .rate = {
73  STABILIZATION_INDI_REF_RATE_P,
74  STABILIZATION_INDI_REF_RATE_Q,
75  STABILIZATION_INDI_REF_RATE_R
76  },
77 };
78 
79 #if STABILIZATION_INDI_USE_ADAPTIVE
80 bool indi_use_adaptive = true;
81 #else
82 bool indi_use_adaptive = false;
83 #endif
84 
85 #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
86 float act_rate_limit[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_RATE_LIMIT;
87 #endif
88 
89 #ifdef STABILIZATION_INDI_ACT_IS_SERVO
90 bool act_is_servo[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO;
91 #else
92 bool act_is_servo[INDI_NUM_ACT] = {0};
93 #endif
94 
95 #ifdef STABILIZATION_INDI_ACT_PREF
96 // Preferred (neutral, least energy) actuator value
97 float act_pref[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_PREF;
98 #else
99 // Assume 0 is neutral
100 float act_pref[INDI_NUM_ACT] = {0.0};
101 #endif
102 
103 float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
104 
105 #ifdef STABILIZATION_INDI_WLS_PRIORITIES
106 static float Wv[INDI_OUTPUTS] = STABILIZATION_INDI_WLS_PRIORITIES;
107 #else
108 //State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
109 static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100};
110 #endif
111 
112 // variables needed for control
113 float actuator_state_filt_vect[INDI_NUM_ACT];
114 struct FloatRates angular_accel_ref = {0., 0., 0.};
115 float angular_acceleration[3] = {0., 0., 0.};
116 float actuator_state[INDI_NUM_ACT];
117 float indi_u[INDI_NUM_ACT];
118 float indi_du[INDI_NUM_ACT];
120 
121 float q_filt = 0.0;
122 float r_filt = 0.0;
123 
124 // variables needed for estimation
125 float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS];
126 float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS];
127 float actuator_state_filt_vectd[INDI_NUM_ACT];
128 float actuator_state_filt_vectdd[INDI_NUM_ACT];
129 float estimation_rate_d[INDI_NUM_ACT];
130 float estimation_rate_dd[INDI_NUM_ACT];
131 float du_estimation[INDI_NUM_ACT];
132 float ddu_estimation[INDI_NUM_ACT];
133 
134 // The learning rate per axis (roll, pitch, yaw, thrust)
135 float mu1[INDI_OUTPUTS] = {0.00001, 0.00001, 0.000003, 0.000002};
136 // The learning rate for the propeller inertia (scaled by 512 wrt mu1)
137 float mu2 = 0.002;
138 
139 // other variables
140 float act_obs[INDI_NUM_ACT];
141 
142 // Number of actuators used to provide thrust
144 
147 
149 static void rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act);
150 
152 static void thrust_cb(uint8_t sender_id, float thrust_increment);
155 
156 float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS];
157 float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
158 float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
159  STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
160  };
161 float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
162 float g1_est[INDI_OUTPUTS][INDI_NUM_ACT];
163 float g2_est[INDI_NUM_ACT];
164 float g1_init[INDI_OUTPUTS][INDI_NUM_ACT];
165 float g2_init[INDI_NUM_ACT];
166 
172 
174 
175 void init_filters(void);
176 
177 #if PERIODIC_TELEMETRY
179 static void send_indi_g(struct transport_tx *trans, struct link_device *dev)
180 {
181  pprz_msg_send_INDI_G(trans, dev, AC_ID, INDI_NUM_ACT, g1_est[0],
182  INDI_NUM_ACT, g1_est[1],
183  INDI_NUM_ACT, g1_est[2],
184  INDI_NUM_ACT, g1_est[3],
185  INDI_NUM_ACT, g2_est);
186 }
187 
188 static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
189 {
190  struct Int32Quat *quat = stateGetNedToBodyQuat_i();
191  pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
196  &(quat->qi),
197  &(quat->qx),
198  &(quat->qy),
199  &(quat->qz));
200 }
201 #endif
202 
207 {
208  // Initialize filters
209  init_filters();
210 
211  AbiBindMsgRPM(RPM_SENSOR_ID, &rpm_ev, rpm_cb);
212  AbiBindMsgTHRUST(THRUST_INCREMENT_ID, &thrust_ev, thrust_cb);
213 
216  float_vect_zero(estimation_rate_d, INDI_NUM_ACT);
217  float_vect_zero(estimation_rate_dd, INDI_NUM_ACT);
219 
220  //Calculate G1G2_PSEUDO_INVERSE
222 
223  // Initialize the array of pointers to the rows of g1g2
224  uint8_t i;
225  for (i = 0; i < INDI_OUTPUTS; i++) {
226  Bwls[i] = g1g2[i];
227  }
228 
229  // Initialize the estimator matrices
230  float_vect_copy(g1_est[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
231  float_vect_copy(g2_est, g2, INDI_NUM_ACT);
232  // Remember the initial matrices
233  float_vect_copy(g1_init[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
234  float_vect_copy(g2_init, g2, INDI_NUM_ACT);
235 
236  // Assume all non-servos are delivering thrust
237  num_thrusters = INDI_NUM_ACT;
238  for (i = 0; i < INDI_NUM_ACT; i++) {
240  }
241 
242 #if PERIODIC_TELEMETRY
245 #endif
246 }
247 
257 {
258  /* reset psi setpoint to current psi angle */
260 
261  float_vect_zero(du_estimation, INDI_NUM_ACT);
262  float_vect_zero(ddu_estimation, INDI_NUM_ACT);
263 }
264 
268 void init_filters(void)
269 {
270  // tau = 1/(2*pi*Fc)
271  float tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF);
272  float tau_est = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF);
273  float sample_time = 1.0 / PERIODIC_FREQUENCY;
274  // Filtering of the gyroscope
275  int8_t i;
276  for (i = 0; i < 3; i++) {
277  init_butterworth_2_low_pass(&measurement_lowpass_filters[i], tau, sample_time, 0.0);
278  init_butterworth_2_low_pass(&estimation_output_lowpass_filters[i], tau_est, sample_time, 0.0);
279  }
280 
281  // Filtering of the actuators
282  for (i = 0; i < INDI_NUM_ACT; i++) {
283  init_butterworth_2_low_pass(&actuator_lowpass_filters[i], tau, sample_time, 0.0);
284  init_butterworth_2_low_pass(&estimation_input_lowpass_filters[i], tau_est, sample_time, 0.0);
285  }
286 
287  // Filtering of the accel body z
288  init_butterworth_2_low_pass(&acceleration_lowpass_filter, tau_est, sample_time, 0.0);
289 }
290 
295 {
296  /* set failsafe to zero roll/pitch and current heading */
299  stab_att_sp_quat.qx = 0;
300  stab_att_sp_quat.qy = 0;
302 }
303 
310 {
311  // stab_att_sp_euler.psi still used in ref..
312  stab_att_sp_euler = *rpy;
313 
315 }
316 
324 {
325  // stab_att_sp_euler.psi still used in ref..
327 
328  // compute sp_euler phi/theta for debugging/telemetry
329  /* Rotate horizontal commands to body frame by psi */
331  int32_t s_psi, c_psi;
332  PPRZ_ITRIG_SIN(s_psi, psi);
333  PPRZ_ITRIG_COS(c_psi, psi);
334  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
335  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
336 
337  quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
338 }
339 
347 void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
348 {
349  /* Propagate the filter on the gyroscopes */
350  struct FloatRates *body_rates = stateGetBodyRates_f();
351  float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r};
352  int8_t i;
353  for (i = 0; i < 3; i++) {
354  update_butterworth_2_low_pass(&measurement_lowpass_filters[i], rate_vect[i]);
355  update_butterworth_2_low_pass(&estimation_output_lowpass_filters[i], rate_vect[i]);
356 
357  //Calculate the angular acceleration via finite difference
358  angular_acceleration[i] = (measurement_lowpass_filters[i].o[0]
359  - measurement_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
360 
361  // Calculate derivatives for estimation
362  float estimation_rate_d_prev = estimation_rate_d[i];
363  estimation_rate_d[i] = (estimation_output_lowpass_filters[i].o[0] - estimation_output_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
364  estimation_rate_dd[i] = (estimation_rate_d[i] - estimation_rate_d_prev) * PERIODIC_FREQUENCY;
365  }
366 
367  // FIXME: this should be configurable!
368  q_filt = (q_filt*3+body_rates->q)/4;
369  r_filt = (r_filt*3+body_rates->r)/4;
370 
371  //calculate the virtual control (reference acceleration) based on a PD controller
372  angular_accel_ref.p = (rate_sp.p - body_rates->p) * indi_gains.rate.p;
373  angular_accel_ref.q = (rate_sp.q - q_filt) * indi_gains.rate.q;
374  angular_accel_ref.r = (rate_sp.r - r_filt) * indi_gains.rate.r;
375 
376  g2_times_du = 0.0;
377  for (i = 0; i < INDI_NUM_ACT; i++) {
378  g2_times_du += g2[i] * indi_du[i];
379  }
380  //G2 is scaled by INDI_G_SCALING to make it readable
382 
383  float v_thrust = 0.0;
384  if (indi_thrust_increment_set && in_flight) {
385  v_thrust = indi_thrust_increment;
386 
387  //update thrust command such that the current is correctly estimated
388  stabilization_cmd[COMMAND_THRUST] = 0;
389  for (i = 0; i < INDI_NUM_ACT; i++) {
390  stabilization_cmd[COMMAND_THRUST] += actuator_state[i] * -((int32_t) act_is_servo[i] - 1);
391  }
392  stabilization_cmd[COMMAND_THRUST] /= num_thrusters;
393 
394  } else {
395  // incremental thrust
396  for (i = 0; i < INDI_NUM_ACT; i++) {
397  v_thrust +=
398  (stabilization_cmd[COMMAND_THRUST] - actuator_state_filt_vect[i]) * Bwls[3][i];
399  }
400  }
401 
402  // The control objective in array format
403  indi_v[0] = (angular_accel_ref.p - angular_acceleration[0]);
404  indi_v[1] = (angular_accel_ref.q - angular_acceleration[1]);
405  indi_v[2] = (angular_accel_ref.r - angular_acceleration[2] + g2_times_du);
406  indi_v[3] = v_thrust;
407 
408 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
409  // Calculate the increment for each actuator
410  for (i = 0; i < INDI_NUM_ACT; i++) {
411  indi_du[i] = (g1g2_pseudo_inv[i][0] * indi_v[0])
412  + (g1g2_pseudo_inv[i][1] * indi_v[1])
413  + (g1g2_pseudo_inv[i][2] * indi_v[2])
414  + (g1g2_pseudo_inv[i][3] * indi_v[3]);
415  }
416 #else
417  // Calculate the min and max increments
418  for (i = 0; i < INDI_NUM_ACT; i++) {
420  du_max[i] = MAX_PPRZ - actuator_state_filt_vect[i];
421  du_pref[i] = act_pref[i] - actuator_state_filt_vect[i];
422 
423 #ifdef GUIDANCE_INDI_MIN_THROTTLE
424  float airspeed = stateGetAirspeed_f();
425  //limit minimum thrust ap can give
426  if(!act_is_servo[i]) {
428  if(airspeed < 8.0) {
429  du_min[i] = GUIDANCE_INDI_MIN_THROTTLE - actuator_state_filt_vect[i];
430  } else {
431  du_min[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD - actuator_state_filt_vect[i];
432  }
433  }
434  }
435 #endif
436  }
437 
438  // WLS Control Allocator
439  num_iter =
440  wls_alloc(indi_du, indi_v, du_min, du_max, Bwls, 0, 0, Wv, 0, du_pref, 10000, 10);
441 #endif
442 
443  // Add the increments to the actuators
445 
446  // Bound the inputs to the actuators
447  for (i = 0; i < INDI_NUM_ACT; i++) {
448  if (act_is_servo[i]) {
449  BoundAbs(indi_u[i], MAX_PPRZ);
450  } else {
451  Bound(indi_u[i], 0, MAX_PPRZ);
452  }
453  }
454 
455  //Don't increment if not flying (not armed)
456  if (!in_flight) {
457  float_vect_zero(indi_u, INDI_NUM_ACT);
458  float_vect_zero(indi_du, INDI_NUM_ACT);
459  }
460 
461  // Propagate actuator filters
463  for (i = 0; i < INDI_NUM_ACT; i++) {
464  update_butterworth_2_low_pass(&actuator_lowpass_filters[i], actuator_state[i]);
465  update_butterworth_2_low_pass(&estimation_input_lowpass_filters[i], actuator_state[i]);
466  actuator_state_filt_vect[i] = actuator_lowpass_filters[i].o[0];
467 
468  // calculate derivatives for estimation
469  float actuator_state_filt_vectd_prev = actuator_state_filt_vectd[i];
470  actuator_state_filt_vectd[i] = (estimation_input_lowpass_filters[i].o[0] - estimation_input_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
471  actuator_state_filt_vectdd[i] = (actuator_state_filt_vectd[i] - actuator_state_filt_vectd_prev) * PERIODIC_FREQUENCY;
472  }
473 
474  // Use online effectiveness estimation only when flying
475  if (in_flight && indi_use_adaptive) {
476  lms_estimation();
477  }
478 
479  /*Commit the actuator command*/
480  for (i = 0; i < INDI_NUM_ACT; i++) {
481  actuators_pprz[i] = (int16_t) indi_u[i];
482  }
483 
484  // Set the stab_cmd to 42 to indicate that it is not used
485  stabilization_cmd[COMMAND_ROLL] = 42;
486  stabilization_cmd[COMMAND_PITCH] = 42;
487  stabilization_cmd[COMMAND_YAW] = 42;
488 }
489 
496 void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
497 {
498  /* attitude error */
499  struct Int32Quat att_err;
500  struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
501  int32_quat_inv_comp(&att_err, att_quat, &quat_sp);
502  /* wrap it in the shortest direction */
503  int32_quat_wrap_shortest(&att_err);
504  int32_quat_normalize(&att_err);
505 
506  // local variable to compute rate setpoints based on attitude error
507  struct FloatRates rate_sp;
508 
509  // calculate the virtual control (reference acceleration) based on a PD controller
510  rate_sp.p = indi_gains.att.p * QUAT1_FLOAT_OF_BFP(att_err.qx) / indi_gains.rate.p;
511  rate_sp.q = indi_gains.att.q * QUAT1_FLOAT_OF_BFP(att_err.qy) / indi_gains.rate.q;
512  rate_sp.r = indi_gains.att.r * QUAT1_FLOAT_OF_BFP(att_err.qz) / indi_gains.rate.r;
513 
514  // Possibly we can use some bounding here
515  /*BoundAbs(rate_sp.r, 5.0);*/
516 
517  /* compute the INDI command */
518  stabilization_indi_rate_run(rate_sp, in_flight);
519 
520  // Reset thrust increment boolean
522 }
523 
524 // This function reads rc commands
525 void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
526 {
527  struct FloatQuat q_sp;
528 #if USE_EARTH_BOUND_RC_SETPOINT
529  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
530 #else
531  stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
532 #endif
533 
535 }
536 
544 {
545 #if INDI_RPM_FEEDBACK
546  float_vect_copy(actuator_state, act_obs, INDI_NUM_ACT);
547 #else
548  //actuator dynamics
549  int8_t i;
550  float UNUSED prev_actuator_state;
551  for (i = 0; i < INDI_NUM_ACT; i++) {
552  prev_actuator_state = actuator_state[i];
553 
555  + act_dyn[i] * (indi_u[i] - actuator_state[i]);
556 
557 #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
558  if ((actuator_state[i] - prev_actuator_state) > act_rate_limit[i]) {
559  actuator_state[i] = prev_actuator_state + act_rate_limit[i];
560  } else if ((actuator_state[i] - prev_actuator_state) < -act_rate_limit[i]) {
561  actuator_state[i] = prev_actuator_state - act_rate_limit[i];
562  }
563 #endif
564  }
565 
566 #endif
567 }
568 
579 void calc_g1_element(float ddx_error, int8_t i, int8_t j, float mu)
580 {
581  g1_est[i][j] = g1_est[i][j] - du_estimation[j] * mu * ddx_error;
582 }
583 
593 void calc_g2_element(float ddx_error, int8_t j, float mu)
594 {
595  g2_est[j] = g2_est[j] - ddu_estimation[j] * mu * ddx_error;
596 }
597 
603 void lms_estimation(void)
604 {
605 
606  // Get the acceleration in body axes
607  struct Int32Vect3 *body_accel_i;
608  body_accel_i = stateGetAccelBody_i();
609  ACCELS_FLOAT_OF_BFP(body_accel_f, *body_accel_i);
610 
611  // Filter the acceleration in z axis
612  update_butterworth_2_low_pass(&acceleration_lowpass_filter, body_accel_f.z);
613 
614  // Calculate the derivative of the acceleration via finite difference
615  float indi_accel_d = (acceleration_lowpass_filter.o[0]
616  - acceleration_lowpass_filter.o[1]) * PERIODIC_FREQUENCY;
617 
618  // scale the inputs to avoid numerical errors
620  float_vect_smul(ddu_estimation, actuator_state_filt_vectdd, 0.001 / PERIODIC_FREQUENCY, INDI_NUM_ACT);
621 
622  float ddx_estimation[INDI_OUTPUTS] = {estimation_rate_dd[0], estimation_rate_dd[1], estimation_rate_dd[2], indi_accel_d};
623 
624  //Estimation of G
625  // TODO: only estimate when du_norm2 is large enough (enough input)
626  /*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];*/
627  int8_t i;
628  for (i = 0; i < INDI_OUTPUTS; i++) {
629  // Calculate the error between prediction and measurement
630  float ddx_error = - ddx_estimation[i];
631  int8_t j;
632  for (j = 0; j < INDI_NUM_ACT; j++) {
633  ddx_error += g1_est[i][j] * du_estimation[j];
634  if (i == 2) {
635  // Changing the momentum of the rotors gives a counter torque
636  ddx_error += g2_est[j] * ddu_estimation[j];
637  }
638  }
639 
640  // when doing the yaw axis, also use G2
641  if (i == 2) {
642  for (j = 0; j < INDI_NUM_ACT; j++) {
643  calc_g2_element(ddx_error, j, mu2);
644  }
645  } else if (i == 3) {
646  // If the acceleration change is very large (rough landing), don't adapt
647  if (fabs(indi_accel_d) > 60.0) {
648  ddx_error = 0.0;
649  }
650  }
651 
652  // Calculate the row of the G1 matrix corresponding to this axis
653  for (j = 0; j < INDI_NUM_ACT; j++) {
654  calc_g1_element(ddx_error, i, j, mu1[i]);
655  }
656  }
657 
658  bound_g_mat();
659 
660  // Save the calculated matrix to G1 and G2
661  // until thrust is included, first part of the array
662  float_vect_copy(g1[0], g1_est[0], INDI_OUTPUTS * INDI_NUM_ACT);
663  float_vect_copy(g2, g2_est, INDI_NUM_ACT);
664 
665 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
666  // Calculate the inverse of (G1+G2)
668 #endif
669 }
670 
675 {
676 
677  //sum of G1 and G2
678  int8_t i;
679  int8_t j;
680  for (i = 0; i < INDI_OUTPUTS; i++) {
681  for (j = 0; j < INDI_NUM_ACT; j++) {
682  if (i != 2) {
683  g1g2[i][j] = g1[i][j] / INDI_G_SCALING;
684  } else {
685  g1g2[i][j] = (g1[i][j] + g2[j]) / INDI_G_SCALING;
686  }
687  }
688  }
689 
690  //G1G2*transpose(G1G2)
691  //calculate matrix multiplication of its transpose INDI_OUTPUTSxnum_act x num_actxINDI_OUTPUTS
692  float element = 0;
693  int8_t row;
694  int8_t col;
695  for (row = 0; row < INDI_OUTPUTS; row++) {
696  for (col = 0; col < INDI_OUTPUTS; col++) {
697  element = 0;
698  for (i = 0; i < INDI_NUM_ACT; i++) {
699  element = element + g1g2[row][i] * g1g2[col][i];
700  }
701  g1g2_trans_mult[row][col] = element;
702  }
703  }
704 
705  //there are numerical errors if the scaling is not right.
706  float_vect_scale(g1g2_trans_mult[0], 100.0, INDI_OUTPUTS * INDI_OUTPUTS);
707 
708  //inverse of 4x4 matrix
710 
711  //scale back
712  float_vect_scale(g1g2inv[0], 100.0, INDI_OUTPUTS * INDI_OUTPUTS);
713 
714  //G1G2'*G1G2inv
715  //calculate matrix multiplication INDI_NUM_ACTxINDI_OUTPUTS x INDI_OUTPUTSxINDI_OUTPUTS
716  for (row = 0; row < INDI_NUM_ACT; row++) {
717  for (col = 0; col < INDI_OUTPUTS; col++) {
718  element = 0;
719  for (i = 0; i < INDI_OUTPUTS; i++) {
720  element = element + g1g2[i][row] * g1g2inv[col][i];
721  }
722  g1g2_pseudo_inv[row][col] = element;
723  }
724  }
725 }
726 
727 static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t UNUSED *rpm, uint8_t UNUSED num_act)
728 {
729 #if INDI_RPM_FEEDBACK
730  int8_t i;
731  for (i = 0; i < num_act; i++) {
732  act_obs[i] = (rpm[i] - get_servo_min(i));
733  act_obs[i] *= (MAX_PPRZ / (float)(get_servo_max(i) - get_servo_min(i)));
734  Bound(act_obs[i], 0, MAX_PPRZ);
735  }
736 #endif
737 }
738 
742 static void thrust_cb(uint8_t UNUSED sender_id, float thrust_increment)
743 {
744  indi_thrust_increment = thrust_increment;
746 }
747 
748 static void bound_g_mat(void)
749 {
750  int8_t i;
751  int8_t j;
752  for (j = 0; j < INDI_NUM_ACT; j++) {
753  float max_limit;
754  float min_limit;
755 
756  // Limit the values of the estimated G1 matrix
757  for (i = 0; i < INDI_OUTPUTS; i++) {
758  if (g1_init[i][j] > 0.0) {
759  max_limit = g1_init[i][j] * INDI_ALLOWED_G_FACTOR;
760  min_limit = g1_init[i][j] / INDI_ALLOWED_G_FACTOR;
761  } else {
762  max_limit = g1_init[i][j] / INDI_ALLOWED_G_FACTOR;
763  min_limit = g1_init[i][j] * INDI_ALLOWED_G_FACTOR;
764  }
765 
766  if (g1_est[i][j] > max_limit) {
767  g1_est[i][j] = max_limit;
768  }
769  if (g1_est[i][j] < min_limit) {
770  g1_est[i][j] = min_limit;
771  }
772  }
773 
774  // Do the same for the G2 matrix
775  if (g2_init[j] > 0.0) {
776  max_limit = g2_init[j] * INDI_ALLOWED_G_FACTOR;
777  min_limit = g2_init[j] / INDI_ALLOWED_G_FACTOR;
778  } else {
779  max_limit = g2_init[j] / INDI_ALLOWED_G_FACTOR;
780  min_limit = g2_init[j] * INDI_ALLOWED_G_FACTOR;
781  }
782 
783  if (g2_est[j] > max_limit) {
784  g2_est[j] = max_limit;
785  }
786  if (g2_est[j] < min_limit) {
787  g2_est[j] = min_limit;
788  }
789  }
790 }
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
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 void int32_quat_normalize(struct Int32Quat *q)
normalize a quaternion inplace
void stabilization_indi_set_failsafe_setpoint(void)
Function that calculates the failsafe setpoint.
bool indi_thrust_increment_set
void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions.
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)
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)
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]
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 QUAT1_FLOAT_OF_BFP(_qi)
#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.
bool act_is_servo[INDI_NUM_ACT]
float indi_du[INDI_NUM_ACT]
#define GUIDANCE_H_MODE_NAV
Definition: guidance_h.h:60
static void float_vect_scale(float *a, const float s, const int n)
a *= s
struct FloatRates rate
static void int32_quat_wrap_shortest(struct Int32Quat *q)
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)
float mu1[INDI_OUTPUTS]
float g2_init[INDI_NUM_ACT]
float g2[INDI_NUM_ACT]
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]
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