Paparazzi UAS  v5.17_devel-24-g2ae834f
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  STABILIZATION_INDI_REF_ERR_P,
68  STABILIZATION_INDI_REF_ERR_Q,
69  STABILIZATION_INDI_REF_ERR_R,
70  STABILIZATION_INDI_REF_RATE_P,
71  STABILIZATION_INDI_REF_RATE_Q,
72  STABILIZATION_INDI_REF_RATE_R,
73 };
74 
75 #if STABILIZATION_INDI_USE_ADAPTIVE
76 bool indi_use_adaptive = true;
77 #else
78 bool indi_use_adaptive = false;
79 #endif
80 
81 #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
82 float act_rate_limit[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_RATE_LIMIT;
83 #endif
84 
85 #ifdef STABILIZATION_INDI_ACT_IS_SERVO
86 bool act_is_servo[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO;
87 #else
88 bool act_is_servo[INDI_NUM_ACT] = {0};
89 #endif
90 
91 #ifdef STABILIZATION_INDI_ACT_PREF
92 // Preferred (neutral, least energy) actuator value
93 float act_pref[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_PREF;
94 #else
95 // Assume 0 is neutral
96 float act_pref[INDI_NUM_ACT] = {0.0};
97 #endif
98 
99 float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
100 
102 #ifndef STABILIZATION_INDI_MAX_RATE
103 #define STABILIZATION_INDI_MAX_RATE 6.0
104 #endif
105 
106 #ifdef STABILIZATION_INDI_WLS_PRIORITIES
107 static float Wv[INDI_OUTPUTS] = STABILIZATION_INDI_WLS_PRIORITIES;
108 #else
109 //State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
110 static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100};
111 #endif
112 
113 // variables needed for control
114 float actuator_state_filt_vect[INDI_NUM_ACT];
115 struct FloatRates angular_accel_ref = {0., 0., 0.};
116 float angular_acceleration[3] = {0., 0., 0.};
117 float actuator_state[INDI_NUM_ACT];
118 float indi_u[INDI_NUM_ACT];
119 float indi_du[INDI_NUM_ACT];
121 
122 float q_filt = 0.0;
123 float r_filt = 0.0;
124 
125 // variables needed for estimation
126 float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS];
127 float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS];
128 float actuator_state_filt_vectd[INDI_NUM_ACT];
129 float actuator_state_filt_vectdd[INDI_NUM_ACT];
130 float estimation_rate_d[INDI_NUM_ACT];
131 float estimation_rate_dd[INDI_NUM_ACT];
132 float du_estimation[INDI_NUM_ACT];
133 float ddu_estimation[INDI_NUM_ACT];
134 
135 // The learning rate per axis (roll, pitch, yaw, thrust)
136 float mu1[INDI_OUTPUTS] = {0.00001, 0.00001, 0.000003, 0.000002};
137 // The learning rate for the propeller inertia (scaled by 512 wrt mu1)
138 float mu2 = 0.002;
139 
140 // other variables
141 float act_obs[INDI_NUM_ACT];
142 
143 // Number of actuators used to provide thrust
145 
148 
150 static void rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act);
151 
153 static void thrust_cb(uint8_t sender_id, float thrust_increment);
156 
157 float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS];
158 float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
159 float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
160  STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
161  };
162 float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
163 float g1_est[INDI_OUTPUTS][INDI_NUM_ACT];
164 float g2_est[INDI_NUM_ACT];
165 float g1_init[INDI_OUTPUTS][INDI_NUM_ACT];
166 float g2_init[INDI_NUM_ACT];
167 
173 
175 
176 void init_filters(void);
177 
178 #if PERIODIC_TELEMETRY
180 static void send_indi_g(struct transport_tx *trans, struct link_device *dev)
181 {
182  pprz_msg_send_INDI_G(trans, dev, AC_ID, INDI_NUM_ACT, g1_est[0],
183  INDI_NUM_ACT, g1_est[1],
184  INDI_NUM_ACT, g1_est[2],
185  INDI_NUM_ACT, g1_est[3],
186  INDI_NUM_ACT, g2_est);
187 }
188 
189 static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
190 {
191  struct Int32Quat *quat = stateGetNedToBodyQuat_i();
192  pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
197  &(quat->qi),
198  &(quat->qx),
199  &(quat->qy),
200  &(quat->qz));
201 }
202 #endif
203 
208 {
209  // Initialize filters
210  init_filters();
211 
212  AbiBindMsgRPM(RPM_SENSOR_ID, &rpm_ev, rpm_cb);
213  AbiBindMsgTHRUST(THRUST_INCREMENT_ID, &thrust_ev, thrust_cb);
214 
217  float_vect_zero(estimation_rate_d, INDI_NUM_ACT);
218  float_vect_zero(estimation_rate_dd, INDI_NUM_ACT);
220 
221  //Calculate G1G2_PSEUDO_INVERSE
223 
224  // Initialize the array of pointers to the rows of g1g2
225  uint8_t i;
226  for (i = 0; i < INDI_OUTPUTS; i++) {
227  Bwls[i] = g1g2[i];
228  }
229 
230  // Initialize the estimator matrices
231  float_vect_copy(g1_est[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
232  float_vect_copy(g2_est, g2, INDI_NUM_ACT);
233  // Remember the initial matrices
234  float_vect_copy(g1_init[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
235  float_vect_copy(g2_init, g2, INDI_NUM_ACT);
236 
237  // Assume all non-servos are delivering thrust
238  num_thrusters = INDI_NUM_ACT;
239  for (i = 0; i < INDI_NUM_ACT; i++) {
241  }
242 
243 #if PERIODIC_TELEMETRY
246 #endif
247 }
248 
258 {
259  /* reset psi setpoint to current psi angle */
261 
262  float_vect_zero(du_estimation, INDI_NUM_ACT);
263  float_vect_zero(ddu_estimation, INDI_NUM_ACT);
264 }
265 
269 void init_filters(void)
270 {
271  // tau = 1/(2*pi*Fc)
272  float tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF);
273  float tau_est = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF);
274  float sample_time = 1.0 / PERIODIC_FREQUENCY;
275  // Filtering of the gyroscope
276  int8_t i;
277  for (i = 0; i < 3; i++) {
278  init_butterworth_2_low_pass(&measurement_lowpass_filters[i], tau, sample_time, 0.0);
279  init_butterworth_2_low_pass(&estimation_output_lowpass_filters[i], tau_est, sample_time, 0.0);
280  }
281 
282  // Filtering of the actuators
283  for (i = 0; i < INDI_NUM_ACT; i++) {
284  init_butterworth_2_low_pass(&actuator_lowpass_filters[i], tau, sample_time, 0.0);
285  init_butterworth_2_low_pass(&estimation_input_lowpass_filters[i], tau_est, sample_time, 0.0);
286  }
287 
288  // Filtering of the accel body z
289  init_butterworth_2_low_pass(&acceleration_lowpass_filter, tau_est, sample_time, 0.0);
290 }
291 
296 {
297  /* set failsafe to zero roll/pitch and current heading */
300  stab_att_sp_quat.qx = 0;
301  stab_att_sp_quat.qy = 0;
303 }
304 
311 {
312  // stab_att_sp_euler.psi still used in ref..
313  stab_att_sp_euler = *rpy;
314 
316 }
317 
325 {
326  // stab_att_sp_euler.psi still used in ref..
328 
329  // compute sp_euler phi/theta for debugging/telemetry
330  /* Rotate horizontal commands to body frame by psi */
332  int32_t s_psi, c_psi;
333  PPRZ_ITRIG_SIN(s_psi, psi);
334  PPRZ_ITRIG_COS(c_psi, psi);
335  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
336  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
337 
338  quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
339 }
340 
348 static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_control, bool in_flight)
349 {
350 
351  struct FloatRates rate_ref;
352  if (rate_control) { //Check if we are running the rate controller
355  rate_ref.r = (float)radio_control.values[RADIO_YAW] / MAX_PPRZ * STABILIZATION_INDI_MAX_RATE;
356  } else {
357  //calculate the virtual control (reference acceleration) based on a PD controller
358  rate_ref.p = reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
359  / reference_acceleration.rate_p;
360  rate_ref.q = reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
361  / reference_acceleration.rate_q;
362  rate_ref.r = reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
363  / reference_acceleration.rate_r;
364 
365  // Possibly we can use some bounding here
366  /*BoundAbs(rate_ref.r, 5.0);*/
367  }
368 
369  struct FloatRates *body_rates = stateGetBodyRates_f();
370 
371  q_filt = (q_filt*3+body_rates->q)/4;
372  r_filt = (r_filt*3+body_rates->r)/4;
373 
374  //calculate the virtual control (reference acceleration) based on a PD controller
375  angular_accel_ref.p = (rate_ref.p - body_rates->p) * reference_acceleration.rate_p;
376  angular_accel_ref.q = (rate_ref.q - q_filt) * reference_acceleration.rate_q;
377  angular_accel_ref.r = (rate_ref.r - r_filt) * reference_acceleration.rate_r;
378 
379  g2_times_du = 0.0;
380  int8_t i;
381  for (i = 0; i < INDI_NUM_ACT; i++) {
382  g2_times_du += g2[i] * indi_du[i];
383  }
384  //G2 is scaled by INDI_G_SCALING to make it readable
386 
387  float v_thrust = 0.0;
388  if (indi_thrust_increment_set && in_flight) {
389  v_thrust = indi_thrust_increment;
390 
391  //update thrust command such that the current is correctly estimated
392  stabilization_cmd[COMMAND_THRUST] = 0;
393  for (i = 0; i < INDI_NUM_ACT; i++) {
394  stabilization_cmd[COMMAND_THRUST] += actuator_state[i] * -((int32_t) act_is_servo[i] - 1);
395  }
396  stabilization_cmd[COMMAND_THRUST] /= num_thrusters;
397 
398  } else {
399  // incremental thrust
400  for (i = 0; i < INDI_NUM_ACT; i++) {
401  v_thrust +=
402  (stabilization_cmd[COMMAND_THRUST] - actuator_state_filt_vect[i]) * Bwls[3][i];
403  }
404  }
405 
406  // The control objective in array format
407  indi_v[0] = (angular_accel_ref.p - angular_acceleration[0]);
408  indi_v[1] = (angular_accel_ref.q - angular_acceleration[1]);
409  indi_v[2] = (angular_accel_ref.r - angular_acceleration[2] + g2_times_du);
410  indi_v[3] = v_thrust;
411 
412 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
413  // Calculate the increment for each actuator
414  for (i = 0; i < INDI_NUM_ACT; i++) {
415  indi_du[i] = (g1g2_pseudo_inv[i][0] * indi_v[0])
416  + (g1g2_pseudo_inv[i][1] * indi_v[1])
417  + (g1g2_pseudo_inv[i][2] * indi_v[2])
418  + (g1g2_pseudo_inv[i][3] * indi_v[3]);
419  }
420 #else
421  // Calculate the min and max increments
422  for (i = 0; i < INDI_NUM_ACT; i++) {
424  du_max[i] = MAX_PPRZ - actuator_state_filt_vect[i];
425  du_pref[i] = act_pref[i] - actuator_state_filt_vect[i];
426 
427 #ifdef GUIDANCE_INDI_MIN_THROTTLE
428  float airspeed = stateGetAirspeed_f();
429  //limit minimum thrust ap can give
430  if(!act_is_servo[i]) {
432  if(airspeed < 8.0) {
433  du_min[i] = GUIDANCE_INDI_MIN_THROTTLE - actuator_state_filt_vect[i];
434  } else {
435  du_min[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD - actuator_state_filt_vect[i];
436  }
437  }
438  }
439 #endif
440  }
441 
442  // WLS Control Allocator
443  num_iter =
444  wls_alloc(indi_du, indi_v, du_min, du_max, Bwls, 0, 0, Wv, 0, du_pref, 10000, 10);
445 #endif
446 
447  // Add the increments to the actuators
449 
450  // Bound the inputs to the actuators
451  for (i = 0; i < INDI_NUM_ACT; i++) {
452  if (act_is_servo[i]) {
453  BoundAbs(indi_u[i], MAX_PPRZ);
454  } else {
455  Bound(indi_u[i], 0, MAX_PPRZ);
456  }
457  }
458 
459  //Don't increment if not flying (not armed)
460  if (!in_flight) {
461  float_vect_zero(indi_u, INDI_NUM_ACT);
462  float_vect_zero(indi_du, INDI_NUM_ACT);
463  }
464 
465  // Propagate actuator filters
467  for (i = 0; i < INDI_NUM_ACT; i++) {
468  update_butterworth_2_low_pass(&actuator_lowpass_filters[i], actuator_state[i]);
469  update_butterworth_2_low_pass(&estimation_input_lowpass_filters[i], actuator_state[i]);
470  actuator_state_filt_vect[i] = actuator_lowpass_filters[i].o[0];
471 
472  // calculate derivatives for estimation
473  float actuator_state_filt_vectd_prev = actuator_state_filt_vectd[i];
474  actuator_state_filt_vectd[i] = (estimation_input_lowpass_filters[i].o[0] - estimation_input_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
475  actuator_state_filt_vectdd[i] = (actuator_state_filt_vectd[i] - actuator_state_filt_vectd_prev) * PERIODIC_FREQUENCY;
476  }
477 
478  // Use online effectiveness estimation only when flying
479  if (in_flight && indi_use_adaptive) {
480  lms_estimation();
481  }
482 
483  /*Commit the actuator command*/
484  for (i = 0; i < INDI_NUM_ACT; i++) {
485  actuators_pprz[i] = (int16_t) indi_u[i];
486  }
487 }
488 
495 void stabilization_indi_run(bool in_flight, bool rate_control)
496 {
497 
498  /* Propagate the filter on the gyroscopes */
499  struct FloatRates *body_rates = stateGetBodyRates_f();
500  float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r};
501  int8_t i;
502  for (i = 0; i < 3; i++) {
503  update_butterworth_2_low_pass(&measurement_lowpass_filters[i], rate_vect[i]);
504  update_butterworth_2_low_pass(&estimation_output_lowpass_filters[i], rate_vect[i]);
505 
506  //Calculate the angular acceleration via finite difference
507  angular_acceleration[i] = (measurement_lowpass_filters[i].o[0]
508  - measurement_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
509 
510  // Calculate derivatives for estimation
511  float estimation_rate_d_prev = estimation_rate_d[i];
512  estimation_rate_d[i] = (estimation_output_lowpass_filters[i].o[0] - estimation_output_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
513  estimation_rate_dd[i] = (estimation_rate_d[i] - estimation_rate_d_prev) * PERIODIC_FREQUENCY;
514  }
515 
516  /* attitude error */
517  struct Int32Quat att_err;
518  struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
519  int32_quat_inv_comp(&att_err, att_quat, &stab_att_sp_quat);
520  /* wrap it in the shortest direction */
521  int32_quat_wrap_shortest(&att_err);
522  int32_quat_normalize(&att_err);
523 
524  /* compute the INDI command */
525  stabilization_indi_calc_cmd(&att_err, rate_control, in_flight);
526 
527  // Set the stab_cmd to 42 to indicate that it is not used
528  stabilization_cmd[COMMAND_ROLL] = 42;
529  stabilization_cmd[COMMAND_PITCH] = 42;
530  stabilization_cmd[COMMAND_YAW] = 42;
531 
532  // Reset thrust increment boolean
534 }
535 
536 // This function reads rc commands
537 void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
538 {
539  struct FloatQuat q_sp;
540 #if USE_EARTH_BOUND_RC_SETPOINT
541  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
542 #else
543  stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
544 #endif
545 
547 }
548 
556 {
557 #if INDI_RPM_FEEDBACK
558  float_vect_copy(actuator_state, act_obs, INDI_NUM_ACT);
559 #else
560  //actuator dynamics
561  int8_t i;
562  float UNUSED prev_actuator_state;
563  for (i = 0; i < INDI_NUM_ACT; i++) {
564  prev_actuator_state = actuator_state[i];
565 
567  + act_dyn[i] * (indi_u[i] - actuator_state[i]);
568 
569 #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
570  if ((actuator_state[i] - prev_actuator_state) > act_rate_limit[i]) {
571  actuator_state[i] = prev_actuator_state + act_rate_limit[i];
572  } else if ((actuator_state[i] - prev_actuator_state) < -act_rate_limit[i]) {
573  actuator_state[i] = prev_actuator_state - act_rate_limit[i];
574  }
575 #endif
576  }
577 
578 #endif
579 }
580 
591 void calc_g1_element(float ddx_error, int8_t i, int8_t j, float mu)
592 {
593  g1_est[i][j] = g1_est[i][j] - du_estimation[j] * mu * ddx_error;
594 }
595 
605 void calc_g2_element(float ddx_error, int8_t j, float mu)
606 {
607  g2_est[j] = g2_est[j] - ddu_estimation[j] * mu * ddx_error;
608 }
609 
615 void lms_estimation(void)
616 {
617 
618  // Get the acceleration in body axes
619  struct Int32Vect3 *body_accel_i;
620  body_accel_i = stateGetAccelBody_i();
621  ACCELS_FLOAT_OF_BFP(body_accel_f, *body_accel_i);
622 
623  // Filter the acceleration in z axis
624  update_butterworth_2_low_pass(&acceleration_lowpass_filter, body_accel_f.z);
625 
626  // Calculate the derivative of the acceleration via finite difference
627  float indi_accel_d = (acceleration_lowpass_filter.o[0]
628  - acceleration_lowpass_filter.o[1]) * PERIODIC_FREQUENCY;
629 
630  // scale the inputs to avoid numerical errors
632  float_vect_smul(ddu_estimation, actuator_state_filt_vectdd, 0.001 / PERIODIC_FREQUENCY, INDI_NUM_ACT);
633 
634  float ddx_estimation[INDI_OUTPUTS] = {estimation_rate_dd[0], estimation_rate_dd[1], estimation_rate_dd[2], indi_accel_d};
635 
636  //Estimation of G
637  // TODO: only estimate when du_norm2 is large enough (enough input)
638  /*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];*/
639  int8_t i;
640  for (i = 0; i < INDI_OUTPUTS; i++) {
641  // Calculate the error between prediction and measurement
642  float ddx_error = - ddx_estimation[i];
643  int8_t j;
644  for (j = 0; j < INDI_NUM_ACT; j++) {
645  ddx_error += g1_est[i][j] * du_estimation[j];
646  if (i == 2) {
647  // Changing the momentum of the rotors gives a counter torque
648  ddx_error += g2_est[j] * ddu_estimation[j];
649  }
650  }
651 
652  // when doing the yaw axis, also use G2
653  if (i == 2) {
654  for (j = 0; j < INDI_NUM_ACT; j++) {
655  calc_g2_element(ddx_error, j, mu2);
656  }
657  } else if (i == 3) {
658  // If the acceleration change is very large (rough landing), don't adapt
659  if (fabs(indi_accel_d) > 60.0) {
660  ddx_error = 0.0;
661  }
662  }
663 
664  // Calculate the row of the G1 matrix corresponding to this axis
665  for (j = 0; j < INDI_NUM_ACT; j++) {
666  calc_g1_element(ddx_error, i, j, mu1[i]);
667  }
668  }
669 
670  bound_g_mat();
671 
672  // Save the calculated matrix to G1 and G2
673  // until thrust is included, first part of the array
674  float_vect_copy(g1[0], g1_est[0], INDI_OUTPUTS * INDI_NUM_ACT);
675  float_vect_copy(g2, g2_est, INDI_NUM_ACT);
676 
677 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
678  // Calculate the inverse of (G1+G2)
680 #endif
681 }
682 
687 {
688 
689  //sum of G1 and G2
690  int8_t i;
691  int8_t j;
692  for (i = 0; i < INDI_OUTPUTS; i++) {
693  for (j = 0; j < INDI_NUM_ACT; j++) {
694  if (i != 2) {
695  g1g2[i][j] = g1[i][j] / INDI_G_SCALING;
696  } else {
697  g1g2[i][j] = (g1[i][j] + g2[j]) / INDI_G_SCALING;
698  }
699  }
700  }
701 
702  //G1G2*transpose(G1G2)
703  //calculate matrix multiplication of its transpose INDI_OUTPUTSxnum_act x num_actxINDI_OUTPUTS
704  float element = 0;
705  int8_t row;
706  int8_t col;
707  for (row = 0; row < INDI_OUTPUTS; row++) {
708  for (col = 0; col < INDI_OUTPUTS; col++) {
709  element = 0;
710  for (i = 0; i < INDI_NUM_ACT; i++) {
711  element = element + g1g2[row][i] * g1g2[col][i];
712  }
713  g1g2_trans_mult[row][col] = element;
714  }
715  }
716 
717  //there are numerical errors if the scaling is not right.
718  float_vect_scale(g1g2_trans_mult[0], 100.0, INDI_OUTPUTS * INDI_OUTPUTS);
719 
720  //inverse of 4x4 matrix
722 
723  //scale back
724  float_vect_scale(g1g2inv[0], 100.0, INDI_OUTPUTS * INDI_OUTPUTS);
725 
726  //G1G2'*G1G2inv
727  //calculate matrix multiplication INDI_NUM_ACTxINDI_OUTPUTS x INDI_OUTPUTSxINDI_OUTPUTS
728  for (row = 0; row < INDI_NUM_ACT; row++) {
729  for (col = 0; col < INDI_OUTPUTS; col++) {
730  element = 0;
731  for (i = 0; i < INDI_OUTPUTS; i++) {
732  element = element + g1g2[i][row] * g1g2inv[col][i];
733  }
734  g1g2_pseudo_inv[row][col] = element;
735  }
736  }
737 }
738 
739 static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t UNUSED *rpm, uint8_t UNUSED num_act)
740 {
741 #if INDI_RPM_FEEDBACK
742  int8_t i;
743  for (i = 0; i < num_act; i++) {
744  act_obs[i] = (rpm[i] - get_servo_min(i));
745  act_obs[i] *= (MAX_PPRZ / (float)(get_servo_max(i) - get_servo_min(i)));
746  Bound(act_obs[i], 0, MAX_PPRZ);
747  }
748 #endif
749 }
750 
754 static void thrust_cb(uint8_t UNUSED sender_id, float thrust_increment)
755 {
756  indi_thrust_increment = thrust_increment;
758 }
759 
760 static void bound_g_mat(void)
761 {
762  int8_t i;
763  int8_t j;
764  for (j = 0; j < INDI_NUM_ACT; j++) {
765  float max_limit;
766  float min_limit;
767 
768  // Limit the values of the estimated G1 matrix
769  for (i = 0; i < INDI_OUTPUTS; i++) {
770  if (g1_init[i][j] > 0.0) {
771  max_limit = g1_init[i][j] * INDI_ALLOWED_G_FACTOR;
772  min_limit = g1_init[i][j] / INDI_ALLOWED_G_FACTOR;
773  } else {
774  max_limit = g1_init[i][j] / INDI_ALLOWED_G_FACTOR;
775  min_limit = g1_init[i][j] * INDI_ALLOWED_G_FACTOR;
776  }
777 
778  if (g1_est[i][j] > max_limit) {
779  g1_est[i][j] = max_limit;
780  }
781  if (g1_est[i][j] < min_limit) {
782  g1_est[i][j] = min_limit;
783  }
784  }
785 
786  // Do the same for the G2 matrix
787  if (g2_init[j] > 0.0) {
788  max_limit = g2_init[j] * INDI_ALLOWED_G_FACTOR;
789  min_limit = g2_init[j] / INDI_ALLOWED_G_FACTOR;
790  } else {
791  max_limit = g2_init[j] / INDI_ALLOWED_G_FACTOR;
792  min_limit = g2_init[j] * INDI_ALLOWED_G_FACTOR;
793  }
794 
795  if (g2_est[j] > max_limit) {
796  g2_est[j] = max_limit;
797  }
798  if (g2_est[j] < min_limit) {
799  g2_est[j] = min_limit;
800  }
801  }
802 }
int32_t psi
in rad with INT32_ANGLE_FRAC
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
unsigned short uint16_t
Definition: types.h:16
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 RADIO_ROLL
Definition: intermcu_ap.h:41
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.
struct ReferenceSystem reference_acceleration
bool indi_thrust_increment_set
void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions.
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]
#define STABILIZATION_INDI_MAX_RATE
Maximum rate you can request in RC rate mode (rad/s)
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
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
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_control, bool in_flight)
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)
void stabilization_indi_run(bool in_flight, bool rate_control)
Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT]
float estimation_rate_dd[INDI_NUM_ACT]
float o[2]
output history
Roation quaternion.
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).
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
signed short int16_t
Definition: types.h:17
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 RadioControl radio_control
Definition: radio_control.c:30
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
signed long int32_t
Definition: types.h:19
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)
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
#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
unsigned char uint8_t
Definition: types.h:14
float actuator_state_filt_vectd[INDI_NUM_ACT]
API to get/set the generic vehicle states.
#define RADIO_PITCH
Definition: intermcu_ap.h:42
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
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]
#define RADIO_YAW
Definition: intermcu_ap.h:43
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
signed char int8_t
Definition: types.h:15
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]
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)