Paparazzi UAS  v5.15_devel-230-gc96ce27
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 // variables needed for estimation
123 float g1g2_trans_mult[INDI_OUTPUTS][INDI_OUTPUTS];
124 float g1g2inv[INDI_OUTPUTS][INDI_OUTPUTS];
125 float actuator_state_filt_vectd[INDI_NUM_ACT];
126 float actuator_state_filt_vectdd[INDI_NUM_ACT];
127 float estimation_rate_d[INDI_NUM_ACT];
128 float estimation_rate_dd[INDI_NUM_ACT];
129 float du_estimation[INDI_NUM_ACT];
130 float ddu_estimation[INDI_NUM_ACT];
131 
132 // The learning rate per axis (roll, pitch, yaw, thrust)
133 float mu1[INDI_OUTPUTS] = {0.00001, 0.00001, 0.000003, 0.000002};
134 // The learning rate for the propeller inertia (scaled by 512 wrt mu1)
135 float mu2 = 0.002;
136 
137 // other variables
138 float act_obs[INDI_NUM_ACT];
139 
140 // Number of actuators used to provide thrust
142 
145 
147 static void rpm_cb(uint8_t sender_id, uint16_t *rpm, uint8_t num_act);
148 
150 static void thrust_cb(uint8_t sender_id, float thrust_increment);
153 
154 float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS];
155 float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
156 float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
157  STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
158  };
159 float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
160 float g1_est[INDI_OUTPUTS][INDI_NUM_ACT];
161 float g2_est[INDI_NUM_ACT];
162 float g1_init[INDI_OUTPUTS][INDI_NUM_ACT];
163 float g2_init[INDI_NUM_ACT];
164 
170 
172 
173 void init_filters(void);
174 
175 #if PERIODIC_TELEMETRY
177 static void send_indi_g(struct transport_tx *trans, struct link_device *dev)
178 {
179  pprz_msg_send_INDI_G(trans, dev, AC_ID, INDI_NUM_ACT, g1_est[0],
180  INDI_NUM_ACT, g1_est[1],
181  INDI_NUM_ACT, g1_est[2],
182  INDI_NUM_ACT, g1_est[3],
183  INDI_NUM_ACT, g2_est);
184 }
185 
186 static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
187 {
188  struct Int32Quat *quat = stateGetNedToBodyQuat_i();
189  pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
194  &(quat->qi),
195  &(quat->qx),
196  &(quat->qy),
197  &(quat->qz));
198 }
199 #endif
200 
205 {
206  // Initialize filters
207  init_filters();
208 
209  AbiBindMsgRPM(RPM_SENSOR_ID, &rpm_ev, rpm_cb);
210  AbiBindMsgTHRUST(THRUST_INCREMENT_ID, &thrust_ev, thrust_cb);
211 
214  float_vect_zero(estimation_rate_d, INDI_NUM_ACT);
215  float_vect_zero(estimation_rate_dd, INDI_NUM_ACT);
217 
218  //Calculate G1G2_PSEUDO_INVERSE
220 
221  // Initialize the array of pointers to the rows of g1g2
222  uint8_t i;
223  for (i = 0; i < INDI_OUTPUTS; i++) {
224  Bwls[i] = g1g2[i];
225  }
226 
227  // Initialize the estimator matrices
228  float_vect_copy(g1_est[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
229  float_vect_copy(g2_est, g2, INDI_NUM_ACT);
230  // Remember the initial matrices
231  float_vect_copy(g1_init[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT);
232  float_vect_copy(g2_init, g2, INDI_NUM_ACT);
233 
234  // Assume all non-servos are delivering thrust
235  num_thrusters = INDI_NUM_ACT;
236  for (i = 0; i < INDI_NUM_ACT; i++) {
238  }
239 
240 #if PERIODIC_TELEMETRY
243 #endif
244 }
245 
255 {
256  /* reset psi setpoint to current psi angle */
258 
259  float_vect_zero(du_estimation, INDI_NUM_ACT);
260  float_vect_zero(ddu_estimation, INDI_NUM_ACT);
261 }
262 
266 void init_filters(void)
267 {
268  // tau = 1/(2*pi*Fc)
269  float tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF);
270  float tau_est = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF);
271  float sample_time = 1.0 / PERIODIC_FREQUENCY;
272  // Filtering of the gyroscope
273  int8_t i;
274  for (i = 0; i < 3; i++) {
275  init_butterworth_2_low_pass(&measurement_lowpass_filters[i], tau, sample_time, 0.0);
276  init_butterworth_2_low_pass(&estimation_output_lowpass_filters[i], tau_est, sample_time, 0.0);
277  }
278 
279  // Filtering of the actuators
280  for (i = 0; i < INDI_NUM_ACT; i++) {
281  init_butterworth_2_low_pass(&actuator_lowpass_filters[i], tau, sample_time, 0.0);
282  init_butterworth_2_low_pass(&estimation_input_lowpass_filters[i], tau_est, sample_time, 0.0);
283  }
284 
285  // Filtering of the accel body z
286  init_butterworth_2_low_pass(&acceleration_lowpass_filter, tau_est, sample_time, 0.0);
287 }
288 
293 {
294  /* set failsafe to zero roll/pitch and current heading */
297  stab_att_sp_quat.qx = 0;
298  stab_att_sp_quat.qy = 0;
300 }
301 
308 {
309  // stab_att_sp_euler.psi still used in ref..
310  stab_att_sp_euler = *rpy;
311 
313 }
314 
322 {
323  // stab_att_sp_euler.psi still used in ref..
325 
326  // compute sp_euler phi/theta for debugging/telemetry
327  /* Rotate horizontal commands to body frame by psi */
329  int32_t s_psi, c_psi;
330  PPRZ_ITRIG_SIN(s_psi, psi);
331  PPRZ_ITRIG_COS(c_psi, psi);
332  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
333  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
334 
335  quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
336 }
337 
345 static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_control, bool in_flight)
346 {
347 
348  struct FloatRates rate_ref;
349  if (rate_control) { //Check if we are running the rate controller
352  rate_ref.r = (float)radio_control.values[RADIO_YAW] / MAX_PPRZ * STABILIZATION_INDI_MAX_RATE;
353  } else {
354  //calculate the virtual control (reference acceleration) based on a PD controller
355  rate_ref.p = reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
356  / reference_acceleration.rate_p;
357  rate_ref.q = reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
358  / reference_acceleration.rate_q;
359  rate_ref.r = reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
360  / reference_acceleration.rate_r;
361 
362  // Possibly we can use some bounding here
363  /*BoundAbs(rate_ref.r, 5.0);*/
364  }
365 
366  struct FloatRates *body_rates = stateGetBodyRates_f();
367 
368  //calculate the virtual control (reference acceleration) based on a PD controller
369  angular_accel_ref.p = (rate_ref.p - body_rates->p) * reference_acceleration.rate_p;
370  angular_accel_ref.q = (rate_ref.q - body_rates->q) * reference_acceleration.rate_q;
371  angular_accel_ref.r = (rate_ref.r - body_rates->r) * reference_acceleration.rate_r;
372 
373  g2_times_du = 0.0;
374  int8_t i;
375  for (i = 0; i < INDI_NUM_ACT; i++) {
376  g2_times_du += g2[i] * indi_du[i];
377  }
378  //G2 is scaled by INDI_G_SCALING to make it readable
380 
381  float v_thrust = 0.0;
382  if (indi_thrust_increment_set && in_flight) {
383  v_thrust = indi_thrust_increment;
384 
385  //update thrust command such that the current is correctly estimated
386  stabilization_cmd[COMMAND_THRUST] = 0;
387  for (i = 0; i < INDI_NUM_ACT; i++) {
388  stabilization_cmd[COMMAND_THRUST] += actuator_state[i] * -((int32_t) act_is_servo[i] - 1);
389  }
390  stabilization_cmd[COMMAND_THRUST] /= num_thrusters;
391 
392  } else {
393  // incremental thrust
394  for (i = 0; i < INDI_NUM_ACT; i++) {
395  v_thrust +=
396  (stabilization_cmd[COMMAND_THRUST] - actuator_state_filt_vect[i]) * Bwls[3][i];
397  }
398  }
399 
400  // The control objective in array format
401  indi_v[0] = (angular_accel_ref.p - angular_acceleration[0]);
402  indi_v[1] = (angular_accel_ref.q - angular_acceleration[1]);
403  indi_v[2] = (angular_accel_ref.r - angular_acceleration[2] + g2_times_du);
404  indi_v[3] = v_thrust;
405 
406 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
407  // Calculate the increment for each actuator
408  for (i = 0; i < INDI_NUM_ACT; i++) {
409  indi_du[i] = (g1g2_pseudo_inv[i][0] * indi_v[0])
410  + (g1g2_pseudo_inv[i][1] * indi_v[1])
411  + (g1g2_pseudo_inv[i][2] * indi_v[2])
412  + (g1g2_pseudo_inv[i][3] * indi_v[3]);
413  }
414 #else
415  // Calculate the min and max increments
416  for (i = 0; i < INDI_NUM_ACT; i++) {
418  du_max[i] = MAX_PPRZ - actuator_state_filt_vect[i];
419  du_pref[i] = act_pref[i] - actuator_state_filt_vect[i];
420  }
421 
422  // WLS Control Allocator
423  num_iter =
424  wls_alloc(indi_du, indi_v, du_min, du_max, Bwls, 0, 0, Wv, 0, du_pref, 10000, 10);
425 #endif
426 
427  // Add the increments to the actuators
429 
430  // Bound the inputs to the actuators
431  for (i = 0; i < INDI_NUM_ACT; i++) {
432  if (act_is_servo[i]) {
433  BoundAbs(indi_u[i], MAX_PPRZ);
434  } else {
435  Bound(indi_u[i], 0, MAX_PPRZ);
436  }
437  }
438 
439  //Don't increment if not flying (not armed)
440  if (!in_flight) {
441  float_vect_zero(indi_u, INDI_NUM_ACT);
442  float_vect_zero(indi_du, INDI_NUM_ACT);
443  }
444 
445  // Propagate actuator filters
447  for (i = 0; i < INDI_NUM_ACT; i++) {
448  update_butterworth_2_low_pass(&actuator_lowpass_filters[i], actuator_state[i]);
449  update_butterworth_2_low_pass(&estimation_input_lowpass_filters[i], actuator_state[i]);
450  actuator_state_filt_vect[i] = actuator_lowpass_filters[i].o[0];
451 
452  // calculate derivatives for estimation
453  float actuator_state_filt_vectd_prev = actuator_state_filt_vectd[i];
454  actuator_state_filt_vectd[i] = (estimation_input_lowpass_filters[i].o[0] - estimation_input_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
455  actuator_state_filt_vectdd[i] = (actuator_state_filt_vectd[i] - actuator_state_filt_vectd_prev) * PERIODIC_FREQUENCY;
456  }
457 
458  // Use online effectiveness estimation only when flying
459  if (in_flight && indi_use_adaptive) {
460  lms_estimation();
461  }
462 
463  /*Commit the actuator command*/
464  for (i = 0; i < INDI_NUM_ACT; i++) {
465  actuators_pprz[i] = (int16_t) indi_u[i];
466  }
467 }
468 
475 void stabilization_indi_run(bool in_flight, bool rate_control)
476 {
477 
478  /* Propagate the filter on the gyroscopes */
479  struct FloatRates *body_rates = stateGetBodyRates_f();
480  float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r};
481  int8_t i;
482  for (i = 0; i < 3; i++) {
483  update_butterworth_2_low_pass(&measurement_lowpass_filters[i], rate_vect[i]);
484  update_butterworth_2_low_pass(&estimation_output_lowpass_filters[i], rate_vect[i]);
485 
486  //Calculate the angular acceleration via finite difference
487  angular_acceleration[i] = (measurement_lowpass_filters[i].o[0]
488  - measurement_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
489 
490  // Calculate derivatives for estimation
491  float estimation_rate_d_prev = estimation_rate_d[i];
492  estimation_rate_d[i] = (estimation_output_lowpass_filters[i].o[0] - estimation_output_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
493  estimation_rate_dd[i] = (estimation_rate_d[i] - estimation_rate_d_prev) * PERIODIC_FREQUENCY;
494  }
495 
496  /* attitude error */
497  struct Int32Quat att_err;
498  struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
499  int32_quat_inv_comp(&att_err, att_quat, &stab_att_sp_quat);
500  /* wrap it in the shortest direction */
501  int32_quat_wrap_shortest(&att_err);
502  int32_quat_normalize(&att_err);
503 
504  /* compute the INDI command */
505  stabilization_indi_calc_cmd(&att_err, rate_control, in_flight);
506 
507  // Set the stab_cmd to 42 to indicate that it is not used
508  stabilization_cmd[COMMAND_ROLL] = 42;
509  stabilization_cmd[COMMAND_PITCH] = 42;
510  stabilization_cmd[COMMAND_YAW] = 42;
511 
512  // Reset thrust increment boolean
514 }
515 
516 // This function reads rc commands
517 void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
518 {
519  struct FloatQuat q_sp;
520 #if USE_EARTH_BOUND_RC_SETPOINT
521  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
522 #else
523  stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
524 #endif
525 
527 }
528 
536 {
537 #if INDI_RPM_FEEDBACK
538  float_vect_copy(actuator_state, act_obs, INDI_NUM_ACT);
539 #else
540  //actuator dynamics
541  int8_t i;
542  float UNUSED prev_actuator_state;
543  for (i = 0; i < INDI_NUM_ACT; i++) {
544  prev_actuator_state = actuator_state[i];
545 
547  + act_dyn[i] * (indi_u[i] - actuator_state[i]);
548 
549 #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
550  if ((actuator_state[i] - prev_actuator_state) > act_rate_limit[i]) {
551  actuator_state[i] = prev_actuator_state + act_rate_limit[i];
552  } else if ((actuator_state[i] - prev_actuator_state) < -act_rate_limit[i]) {
553  actuator_state[i] = prev_actuator_state - act_rate_limit[i];
554  }
555 #endif
556  }
557 
558 #endif
559 }
560 
571 void calc_g1_element(float ddx_error, int8_t i, int8_t j, float mu)
572 {
573  g1_est[i][j] = g1_est[i][j] - du_estimation[j] * mu * ddx_error;
574 }
575 
585 void calc_g2_element(float ddx_error, int8_t j, float mu)
586 {
587  g2_est[j] = g2_est[j] - ddu_estimation[j] * mu * ddx_error;
588 }
589 
595 void lms_estimation(void)
596 {
597 
598  // Get the acceleration in body axes
599  struct Int32Vect3 *body_accel_i;
600  body_accel_i = stateGetAccelBody_i();
601  ACCELS_FLOAT_OF_BFP(body_accel_f, *body_accel_i);
602 
603  // Filter the acceleration in z axis
604  update_butterworth_2_low_pass(&acceleration_lowpass_filter, body_accel_f.z);
605 
606  // Calculate the derivative of the acceleration via finite difference
607  float indi_accel_d = (acceleration_lowpass_filter.o[0]
608  - acceleration_lowpass_filter.o[1]) * PERIODIC_FREQUENCY;
609 
610  // scale the inputs to avoid numerical errors
612  float_vect_smul(ddu_estimation, actuator_state_filt_vectdd, 0.001 / PERIODIC_FREQUENCY, INDI_NUM_ACT);
613 
614  float ddx_estimation[INDI_OUTPUTS] = {estimation_rate_dd[0], estimation_rate_dd[1], estimation_rate_dd[2], indi_accel_d};
615 
616  //Estimation of G
617  // TODO: only estimate when du_norm2 is large enough (enough input)
618  /*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];*/
619  int8_t i;
620  for (i = 0; i < INDI_OUTPUTS; i++) {
621  // Calculate the error between prediction and measurement
622  float ddx_error = - ddx_estimation[i];
623  int8_t j;
624  for (j = 0; j < INDI_NUM_ACT; j++) {
625  ddx_error += g1_est[i][j] * du_estimation[j];
626  if (i == 2) {
627  // Changing the momentum of the rotors gives a counter torque
628  ddx_error += g2_est[j] * ddu_estimation[j];
629  }
630  }
631 
632  // when doing the yaw axis, also use G2
633  if (i == 2) {
634  for (j = 0; j < INDI_NUM_ACT; j++) {
635  calc_g2_element(ddx_error, j, mu2);
636  }
637  } else if (i == 3) {
638  // If the acceleration change is very large (rough landing), don't adapt
639  if (fabs(indi_accel_d) > 60.0) {
640  ddx_error = 0.0;
641  }
642  }
643 
644  // Calculate the row of the G1 matrix corresponding to this axis
645  for (j = 0; j < INDI_NUM_ACT; j++) {
646  calc_g1_element(ddx_error, i, j, mu1[i]);
647  }
648  }
649 
650  bound_g_mat();
651 
652  // Save the calculated matrix to G1 and G2
653  // until thrust is included, first part of the array
654  float_vect_copy(g1[0], g1_est[0], INDI_OUTPUTS * INDI_NUM_ACT);
655  float_vect_copy(g2, g2_est, INDI_NUM_ACT);
656 
657 #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
658  // Calculate the inverse of (G1+G2)
660 #endif
661 }
662 
667 {
668 
669  //sum of G1 and G2
670  int8_t i;
671  int8_t j;
672  for (i = 0; i < INDI_OUTPUTS; i++) {
673  for (j = 0; j < INDI_NUM_ACT; j++) {
674  if (i != 2) {
675  g1g2[i][j] = g1[i][j] / INDI_G_SCALING;
676  } else {
677  g1g2[i][j] = (g1[i][j] + g2[j]) / INDI_G_SCALING;
678  }
679  }
680  }
681 
682  //G1G2*transpose(G1G2)
683  //calculate matrix multiplication of its transpose INDI_OUTPUTSxnum_act x num_actxINDI_OUTPUTS
684  float element = 0;
685  int8_t row;
686  int8_t col;
687  for (row = 0; row < INDI_OUTPUTS; row++) {
688  for (col = 0; col < INDI_OUTPUTS; col++) {
689  element = 0;
690  for (i = 0; i < INDI_NUM_ACT; i++) {
691  element = element + g1g2[row][i] * g1g2[col][i];
692  }
693  g1g2_trans_mult[row][col] = element;
694  }
695  }
696 
697  //there are numerical errors if the scaling is not right.
698  float_vect_scale(g1g2_trans_mult[0], 100.0, INDI_OUTPUTS * INDI_OUTPUTS);
699 
700  //inverse of 4x4 matrix
702 
703  //scale back
704  float_vect_scale(g1g2inv[0], 100.0, INDI_OUTPUTS * INDI_OUTPUTS);
705 
706  //G1G2'*G1G2inv
707  //calculate matrix multiplication INDI_NUM_ACTxINDI_OUTPUTS x INDI_OUTPUTSxINDI_OUTPUTS
708  for (row = 0; row < INDI_NUM_ACT; row++) {
709  for (col = 0; col < INDI_OUTPUTS; col++) {
710  element = 0;
711  for (i = 0; i < INDI_OUTPUTS; i++) {
712  element = element + g1g2[i][row] * g1g2inv[col][i];
713  }
714  g1g2_pseudo_inv[row][col] = element;
715  }
716  }
717 }
718 
719 static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t UNUSED *rpm, uint8_t UNUSED num_act)
720 {
721 #if INDI_RPM_FEEDBACK
722  int8_t i;
723  for (i = 0; i < num_act; i++) {
724  act_obs[i] = (rpm[i] - get_servo_min(i));
725  act_obs[i] *= (MAX_PPRZ / (float)(get_servo_max(i) - get_servo_min(i)));
726  Bound(act_obs[i], 0, MAX_PPRZ);
727  }
728 #endif
729 }
730 
734 static void thrust_cb(uint8_t UNUSED sender_id, float thrust_increment)
735 {
736  indi_thrust_increment = thrust_increment;
738 }
739 
740 static void bound_g_mat(void)
741 {
742  int8_t i;
743  int8_t j;
744  for (j = 0; j < INDI_NUM_ACT; j++) {
745  float max_limit;
746  float min_limit;
747 
748  // Limit the values of the estimated G1 matrix
749  for (i = 0; i < INDI_OUTPUTS; i++) {
750  if (g1_init[i][j] > 0.0) {
751  max_limit = g1_init[i][j] * INDI_ALLOWED_G_FACTOR;
752  min_limit = g1_init[i][j] / INDI_ALLOWED_G_FACTOR;
753  } else {
754  max_limit = g1_init[i][j] / INDI_ALLOWED_G_FACTOR;
755  min_limit = g1_init[i][j] * INDI_ALLOWED_G_FACTOR;
756  }
757 
758  if (g1_est[i][j] > max_limit) {
759  g1_est[i][j] = max_limit;
760  }
761  if (g1_est[i][j] < min_limit) {
762  g1_est[i][j] = min_limit;
763  }
764  }
765 
766  // Do the same for the G2 matrix
767  if (g2_init[j] > 0.0) {
768  max_limit = g2_init[j] * INDI_ALLOWED_G_FACTOR;
769  min_limit = g2_init[j] / INDI_ALLOWED_G_FACTOR;
770  } else {
771  max_limit = g2_init[j] / INDI_ALLOWED_G_FACTOR;
772  min_limit = g2_init[j] * INDI_ALLOWED_G_FACTOR;
773  }
774 
775  if (g2_est[j] > max_limit) {
776  g2_est[j] = max_limit;
777  }
778  if (g2_est[j] < min_limit) {
779  g2_est[j] = min_limit;
780  }
781  }
782 }
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]
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.
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:92
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)
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]
#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]
static float heading
Definition: ahrs_infrared.c:45
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
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:73
#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]
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)