Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
stabilization_attitude_heli_indi.c
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2016 Bart Slinger <bartslinger@gmail.com>
3  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
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 
27 #include "paparazzi.h"
29 #include "math/pprz_algebra_int.h"
31 #include "state.h"
32 #include "generated/airframe.h"
33 #include "autopilot.h"
34 
40 
41 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_USE_NOTCHFILTER
42 #define STABILIZATION_ATTITUDE_HELI_INDI_USE_NOTCHFILTER 0
43 #endif
44 #if STABILIZATION_ATTITUDE_HELI_INDI_USE_NOTCHFILTER
45 #ifndef RPM_PWM_CHANNEL
46 #error notch filter requires module rpm_sensor.xml
47 #endif
49 #endif // USE_NOTCHFILTER
50 
51 /* Setup of all default values, these are configured for walkera genius cp v2 */
52 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_ROLL
53 #define STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_ROLL 4.5
54 #endif
55 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_PITCH
56 #define STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_PITCH 0
57 #endif
58 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_ROLL_P
59 #define STABILIZATION_ATTITUDE_HELI_INDI_ROLL_P 12
60 #endif
61 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_PITCH_P
62 #define STABILIZATION_ATTITUDE_HELI_INDI_PITCH_P 8
63 #endif
64 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_YAW_P
65 #define STABILIZATION_ATTITUDE_HELI_INDI_YAW_P 10
66 #endif
67 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_YAW_D
68 #define STABILIZATION_ATTITUDE_HELI_INDI_YAW_D 30
69 #endif
70 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_GINV_ROLL_TO_ROLL
71 #define STABILIZATION_ATTITUDE_HELI_INDI_GINV_ROLL_TO_ROLL 11681
72 #endif
73 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_GINV_PITCH_TO_PITCH
74 #define STABILIZATION_ATTITUDE_HELI_INDI_GINV_PITCH_TO_PITCH 13873
75 #endif
76 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_GINV_YAW_TO_YAW
77 #define STABILIZATION_ATTITUDE_HELI_INDI_GINV_YAW_TO_YAW 730
78 #endif
79 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_ROLL_COMMAND_ROTATION
80 #define STABILIZATION_ATTITUDE_HELI_INDI_ROLL_COMMAND_ROTATION 11.0
81 #endif
82 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_PITCH_COMMAND_ROTATION
83 #define STABILIZATION_ATTITUDE_HELI_INDI_PITCH_COMMAND_ROTATION -30.0
84 #endif
85 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT
86 #define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT 10.0
87 #endif
88 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_ROLL
89 #define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_ROLL STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT
90 #endif
91 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_PITCH
92 #define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_PITCH STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT
93 #endif
94 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_YAW
95 #define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_YAW STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT
96 #endif
97 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_THRUST
98 #define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_THRUST STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_DEFAULT
99 #endif
100 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT
101 #define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT 40.0
102 #endif
103 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_ROLL
104 #define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_ROLL STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT
105 #endif
106 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_PITCH
107 #define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_PITCH STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT
108 #endif
109 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_YAW
110 #define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_YAW STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT
111 #endif
112 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_THRUST
113 #define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_THRUST STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_DEFAULT
114 #endif
115 #ifndef STABILIZATION_ATTITUDE_HELI_INDI_NOTCH_MIN_RPM
116 #define STABILIZATION_ATTITUDE_HELI_INDI_NOTCH_MIN_RPM 1500
117 #endif
118 #define INDI_NOTCH_MIN_RPM STABILIZATION_ATTITUDE_HELI_INDI_NOTCH_MIN_RPM
119 
120 /* Shorter defines to use lateron in the matrix */
121 #define INVG_00 STABILIZATION_ATTITUDE_HELI_INDI_GINV_ROLL_TO_ROLL
122 #define INVG_11 STABILIZATION_ATTITUDE_HELI_INDI_GINV_PITCH_TO_PITCH
123 #define INVG_22 STABILIZATION_ATTITUDE_HELI_INDI_GINV_YAW_TO_YAW
124 #define INVG_33 -50000 // Not used at the moment
125 #define INT32_INVG_FRAC 16
126 
129 struct Int32Quat sp_offset; // non-zero neutral attitude
130 
136 };
137 
138 /* Main controller struct */
140 
141 /* Filter functions */
143 int32_t alpha_yaw_inc; // Tail model parameters for spinning up
144 int32_t alpha_yaw_dec; // Tail model parameters for spinning down
145 /* Measurement filters */
150 #if STABILIZATION_ATTITUDE_HELI_INDI_USE_FAST_DYN_FILTERS
151 struct delayed_first_order_lowpass_filter_t fast_dynamics_model[2]; // only pitch and roll
152 #endif
153 
154 /* Private functions declarations */
159 
160 #if PERIODIC_TELEMETRY
162 
163 /* Telemetry messages here, at the moment there are none */
164 
165 #endif // PERIODIC_TELEMETRY
166 
167 static inline void indi_apply_actuator_models(int32_t _out[], int32_t _in[])
168 {
169  int32_t temp_roll;
170  int32_t temp_pitch;
171  temp_roll = delayed_first_order_lowpass_propagate(&actuator_model[INDI_ROLL], _in[INDI_ROLL]);
172  temp_pitch = delayed_first_order_lowpass_propagate(&actuator_model[INDI_PITCH], _in[INDI_PITCH]);
173 
174  /* Depending on yaw direction, change filter coefficient */
176  if (_in[INDI_YAW] - prev > 0) {
177  // Tail spinning up
179  } else {
180  // Tail spinning down
182  }
184 
186 
187 #if STABILIZATION_ATTITUDE_HELI_INDI_USE_FAST_DYN_FILTERS
188  /* Also apply first order filter that represents fast damping dynamics in pitch and roll rate */
189  _out[INDI_ROLL] = delayed_first_order_lowpass_propagate(&fast_dynamics_model[INDI_ROLL], temp_roll);
190  _out[INDI_PITCH] = delayed_first_order_lowpass_propagate(&fast_dynamics_model[INDI_PITCH], temp_pitch);
191 #else
192  _out[INDI_ROLL] = temp_roll;
193  _out[INDI_PITCH] = temp_pitch;
194 #endif
195 }
196 
206 static inline void indi_apply_compensator_filters(int32_t _out[], int32_t _in[])
207 {
208  _out[INDI_ROLL] = _in[INDI_ROLL];
209  _out[INDI_PITCH] = _in[INDI_PITCH];
210 
211  /* Delay the tail by 9 samples */
212  static int32_t yaw_output_buffer[INDI_YAW_BUFFER_SIZE];
213  static uint8_t buf_idx = 0;
214 
215  buf_idx %= (INDI_YAW_BUFFER_SIZE - 1);
216  _out[INDI_YAW] = yaw_output_buffer[buf_idx];
217  yaw_output_buffer[buf_idx] = _in[INDI_YAW];
218  buf_idx++;
219 
220  // Disregard, just use input:
221  _out[INDI_YAW] = _in[INDI_YAW];
222 
223  /* Thrust compensated for slow tail dynamics:
224  * Step 1. What would be the next output of the system if the thrust cmd would be applied to the tail dynamics?
225  * Step 2. What input is required to obtain this output when assuming dynamics of thrust actuator?
226  *
227  * We can re-use alpha_yaw_dec and alpha_yaw_inc
228  */
229  static int32_t prev_thrust_out = 0;
230  int32_t alpha;
231  if (_in[INDI_THRUST] - prev_thrust_out > 0) {
232  alpha = alpha_yaw_inc;
233  } else {
234  alpha = alpha_yaw_dec;
235  }
236  int32_t output_target = (alpha * prev_thrust_out + ((1 << DELAYED_FIRST_ORDER_LOWPASS_FILTER_FILTER_ALPHA_SHIFT) -
238 
239  /* Now the target output is known, the collective dynamics is known. What input is required? */
240  int32_t alpha_thrust = actuator_model[INDI_THRUST].alpha;
241  _out[INDI_THRUST] = ((output_target << DELAYED_FIRST_ORDER_LOWPASS_FILTER_FILTER_ALPHA_SHIFT) - alpha_thrust *
242  prev_thrust_out) / ((1 << DELAYED_FIRST_ORDER_LOWPASS_FILTER_FILTER_ALPHA_SHIFT) - alpha_thrust);
243  prev_thrust_out = _out[INDI_THRUST];
244 
245  /* At the moment, thrust is not fully implemented */
246  //_out[INDI_THRUST] = _in[INDI_THRUST];
247 }
248 
249 static inline void indi_apply_notch_filters(struct SecondOrderNotchFilter *filter, int32_t _out[], int32_t _in[])
250 {
256  notch_filter_update(&filter[INDI_ROLL], &_in[INDI_ROLL], &_out[INDI_ROLL]);
257  notch_filter_update(&filter[INDI_PITCH], &_in[INDI_PITCH], &_out[INDI_PITCH]);
258  notch_filter_update(&filter[INDI_YAW], &_in[INDI_YAW], &_out[INDI_YAW]);
259  notch_filter_update(&filter[INDI_THRUST], &_in[INDI_THRUST], &_out[INDI_THRUST]);
260  } else {
261  _out[INDI_ROLL] = _in[INDI_ROLL];
262  _out[INDI_PITCH] = _in[INDI_PITCH];
263  _out[INDI_YAW] = _in[INDI_YAW];
264  _out[INDI_THRUST] = _in[INDI_THRUST];
265  }
266 }
267 
269 {
271 }
272 
274 {
276 }
277 
279 {
280  for (uint8_t i = 0; i < INDI_DOF; i++) {
281  _out[i] = update_butterworth_2_low_pass_int(&actuator_lowpass_filters[i], _in[i]);
282  }
283 }
284 
286 {
287  for (uint8_t i = 0; i < INDI_DOF; i++) {
288  _out[i] = update_butterworth_2_low_pass_int(&measurement_lowpass_filters[i], _in[i]);
289  }
290 }
291 
299 {
302 }
303 
312 {
315 }
316 
324 {
325  /* Pitch roll setpoint not zero, because then helicopter drifts sideways */
326  /* orientation vector describing simultaneous rotation of roll/pitch */
327  struct FloatVect3 ov;
328  struct FloatQuat q;
329  ov.x = -heli_indi_ctl.sp_offset_roll * M_PI / 180;
330  ov.y = -heli_indi_ctl.sp_offset_pitch * M_PI / 180;
331  ov.z = 0.0;
332  /* quaternion from that orientation vector */
335 }
336 
343 {
344  /* Initialization code INDI */
345  struct IndiController_int *c = &heli_indi_ctl;
348 #if STABILIZATION_ATTITUDE_HELI_INDI_USE_NOTCHFILTER
349  c->enable_notch = TRUE;
350 #else
351  c->enable_notch = FALSE;
352 #endif
353  c->motor_rpm = 0;
354 
355  /* Set steady-state pitch and roll values */
359 
360  /* Initialize inv(G) */
361  c->invG[0][0] = INVG_00; c->invG[0][1] = 0; c->invG[0][2] = 0; c->invG[0][3] = 0;
362  c->invG[1][0] = 0; c->invG[1][1] = INVG_11; c->invG[1][2] = 0; c->invG[1][3] = 0;
363  c->invG[2][0] = 0; c->invG[2][1] = 0; c->invG[2][2] = INVG_22; c->invG[2][3] = 0;
364  c->invG[3][0] = 0; c->invG[3][1] = 0; c->invG[3][2] = 0; c->invG[3][3] = INVG_33;
365 
366  /* Actuator filter initialization */
367  delayed_first_order_lowpass_initialize(&actuator_model[INDI_ROLL], 70, 9, 900, PERIODIC_FREQUENCY);
368  delayed_first_order_lowpass_initialize(&actuator_model[INDI_PITCH], 70, 9, 900, PERIODIC_FREQUENCY);
369  delayed_first_order_lowpass_initialize(&actuator_model[INDI_YAW], 37, 0, 9600, PERIODIC_FREQUENCY);
371  PERIODIC_FREQUENCY); /* 450 because dynamic range is only 0-9600 */
372  /* Different dynamics for up and down */
375  (PERIODIC_FREQUENCY + 13); // OMEGA_DOWN = 13 rad/s, shift = 14
376 
377  /* Notch filter initialization, bandwidth in Hz */
379  PERIODIC_FREQUENCY);
381  PERIODIC_FREQUENCY);
383  PERIODIC_FREQUENCY);
385  PERIODIC_FREQUENCY);
387  PERIODIC_FREQUENCY);
389  PERIODIC_FREQUENCY);
391  PERIODIC_FREQUENCY);
393  PERIODIC_FREQUENCY);
394 
395  /* Low pass filter initialization, cutoff frequency in Hz */
396  init_butterworth_2_low_pass_int(&actuator_lowpass_filters[INDI_ROLL],
397  STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_ROLL, 1.0 / PERIODIC_FREQUENCY, 0);
398  init_butterworth_2_low_pass_int(&measurement_lowpass_filters[INDI_ROLL],
399  STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_ROLL, 1.0 / PERIODIC_FREQUENCY, 0);
400  init_butterworth_2_low_pass_int(&actuator_lowpass_filters[INDI_PITCH],
401  STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_PITCH, 1.0 / PERIODIC_FREQUENCY, 0);
402  init_butterworth_2_low_pass_int(&measurement_lowpass_filters[INDI_PITCH],
403  STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_PITCH, 1.0 / PERIODIC_FREQUENCY, 0);
404  init_butterworth_2_low_pass_int(&actuator_lowpass_filters[INDI_YAW],
405  STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_YAW, 1.0 / PERIODIC_FREQUENCY, 0);
406  init_butterworth_2_low_pass_int(&measurement_lowpass_filters[INDI_YAW],
407  STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_YAW, 1.0 / PERIODIC_FREQUENCY, 0);
408  init_butterworth_2_low_pass_int(&actuator_lowpass_filters[INDI_THRUST],
409  STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_THRUST, 1.0 / PERIODIC_FREQUENCY, 0);
410  init_butterworth_2_low_pass_int(&measurement_lowpass_filters[INDI_THRUST],
411  STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_THRUST, 1.0 / PERIODIC_FREQUENCY, 0);
412 
413 #if STABILIZATION_ATTITUDE_HELI_INDI_USE_FAST_DYN_FILTERS
414  /* Fast dynamics in roll and pitch model */
415  delayed_first_order_lowpass_initialize(&fast_dynamics_model[INDI_ROLL],
416  STABILIZATION_ATTITUDE_HELI_INDI_FAST_DYN_ROLL_BW, 0, MAX_PPRZ, PERIODIC_FREQUENCY);
417  delayed_first_order_lowpass_initialize(&fast_dynamics_model[INDI_PITCH],
418  STABILIZATION_ATTITUDE_HELI_INDI_FAST_DYN_PITCH_BW, 0, MAX_PPRZ, PERIODIC_FREQUENCY);
419 #endif
420 
421  /* Assign filter functions: */
428 
429 #if PERIODIC_TELEMETRY
430  //register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_<<MSG>>, function);
431 #endif
432 }
433 
434 void stabilization_attitude_run(bool in_flight)
435 {
436  (void) in_flight; // unused variable
437  struct IndiController_int *c = &heli_indi_ctl;
438 
439  /* calculate acceleration in body frame */
440  struct NedCoor_i *ltp_accel_nedcoor = stateGetAccelNed_i();
441  struct Int32Vect3 ltp_accel;
442  struct Int32Vect3 body_accel; // Acceleration measurement in body frame
443  ltp_accel.x = ltp_accel_nedcoor->x;
444  ltp_accel.y = ltp_accel_nedcoor->y;
445  ltp_accel.z = ltp_accel_nedcoor->z;
446  int32_rmat_vmult(&body_accel, stateGetNedToBodyRMat_i(), &ltp_accel);
447 
448  /* attitude error */
449  struct Int32Quat att_err;
450  struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
451 
452  /* Add steady-state value to attitude setpoint, because heli has
453  * non-zero roll angle by default
454  */
455  struct Int32Quat corr_att_sp_quat; // Corrected attitude setpoint
456  int32_quat_comp_inv(&corr_att_sp_quat, &stab_att_sp_quat, &sp_offset);
457 
458  int32_quat_inv_comp(&att_err, att_quat, &corr_att_sp_quat);
459  /* wrap it in the shortest direction */
460  int32_quat_wrap_shortest(&att_err);
461  int32_quat_normalize(&att_err);
462 
463  /* rate error (setpoint for rates = 0) */
464  struct Int32Rates *body_rate = stateGetBodyRates_i();
465 
466  /* Inform INDI about the measurement */
467  c->measurement[INDI_ROLL] = body_rate->p;
468  c->measurement[INDI_PITCH] = body_rate->q;
469  c->measurement[INDI_YAW] = body_rate->r;
470  c->measurement[INDI_THRUST] = body_accel.z;
471 
472  /* Get RPM measurement */
473 #if STABILIZATION_ATTITUDE_HELI_INDI_USE_NOTCHFILTER
476  } else {
478  }
479 #endif
480 
481  /* Apply actuator dynamics model to previously commanded values
482  * input = actuator command in previous cycle
483  * output = actual actuator position right now
484  */
486 
487  /* Apply a set of filters, both to the actuator and the measurement */
490  for (uint8_t i = 1; i < INDI_NR_FILTERS; i++) {
493  }
494 
495  /* RADIO throttle stick value, for 4dof mode */
496  //int32_t accel_z_sp = (-1)*3*((guidance_v_rc_delta_t - MAX_PPRZ/2) << INT32_ACCEL_FRAC) / (MAX_PPRZ/2);
497  //accel_z_sp = ((accel_z_sp << INT32_TRIG_FRAC) / guidance_v_thrust_coeff);
498 
499  /* Transform yaw into a delta yaw while keeping filtered yawrate (kinda hacky) */
500  int32_t filtered_measurement_vector[INDI_DOF];
501  int32_vect_copy(filtered_measurement_vector, c->filtered_measurement[INDI_NR_FILTERS - 1], INDI_DOF);
502  static int32_t previous_filt_yawrate = 0;
503  filtered_measurement_vector[INDI_YAW] = 512 * (c->filtered_measurement[INDI_NR_FILTERS - 1][INDI_YAW] -
504  previous_filt_yawrate); // = approximately yaw acceleration error
505  previous_filt_yawrate = c->filtered_measurement[INDI_NR_FILTERS - 1][INDI_YAW];
506 
507  /* Obtain virtual control input with P controller on pitch and roll */
508  int32_t roll_virtual_control = (heli_indi_gains.roll_p * att_err.qx) / 4;
509  int32_t pitch_virtual_control = (heli_indi_gains.pitch_p * att_err.qy) / 4;
510 
511  /* Yaw with cascaded PD-controller to generate virtual control */
512  int32_t yaw_rate_reference = (heli_indi_gains.yaw_p * att_err.qz / 8);
513  int32_t yaw_virtual_control = heli_indi_gains.yaw_d * (yaw_rate_reference - body_rate->r);
514 
515  /* Set INDI references */
516  c->reference[INDI_ROLL] = roll_virtual_control;
517  c->reference[INDI_PITCH] = pitch_virtual_control;
518  c->reference[INDI_YAW] = yaw_virtual_control;
519  //c->reference[INDI_THRUST] = accel_z_sp;
520 
521  /* Subtract (filtered) measurement from reference to get the error */
522  int32_vect_diff(c->error, c->reference, filtered_measurement_vector, INDI_DOF);
523 
524  /* Multiply error with inverse of actuator effectiveness, to get delta u (required increment in input) */
525  MAT_MUL_VECT(INDI_DOF, c->du, c->invG, c->error);
526 
527  /* Bitshift back */
528  c->du[INDI_ROLL] >>= INT32_INVG_FRAC;
529  c->du[INDI_PITCH] >>= INT32_INVG_FRAC;
530  c->du[INDI_YAW] >>= INT32_INVG_FRAC;
532 
533  /* Take the current (filtered) actuator position and add the incremental value. */
534  int32_vect_sum(c->u_setpoint, c->filtered_actuator[INDI_NR_FILTERS - 1], c->du, INDI_DOF);
535  //c->u_setpoint[INDI_THRUST] = stabilization_cmd[COMMAND_THRUST];
536 
537  /* bound the result */
538  BoundAbs(c->u_setpoint[INDI_ROLL], MAX_PPRZ);
539  BoundAbs(c->u_setpoint[INDI_PITCH], MAX_PPRZ);
540  Bound(c->u_setpoint[INDI_YAW], 0, MAX_PPRZ);
541  Bound(c->u_setpoint[INDI_THRUST], 0.15 * MAX_PPRZ, MAX_PPRZ);
542 
543  /* Apply a compensator to the actuator setpoint to obtain actuator command */
545 
546  /* At the end, set 'previous' output to current output */
548 
549  /* Two correction angles, don't rotate but just add.
550  * sin/cos = tan
551  */
552  stabilization_cmd[COMMAND_ROLL] = c->command_out[__k][INDI_ROLL]
554  stabilization_cmd[COMMAND_PITCH] = c->command_out[__k][INDI_PITCH]
556  stabilization_cmd[COMMAND_YAW] = c->command_out[__k][INDI_YAW];
557  /* Thrust is not applied */
558 
559  /* Disable tail when not armed, because this thing goes crazy */
560  if (!autopilot_get_motors_on()) {
561  stabilization_cmd[COMMAND_YAW] = 0;
562  }
563 }
564 
566 {
567  /* reset psi setpoint to current psi angle */
569 }
570 
572 {
573  /* set failsafe to zero roll/pitch and current heading */
576  stab_att_sp_quat.qx = 0;
577  stab_att_sp_quat.qy = 0;
579 }
580 
582 {
583  // stab_att_sp_euler.psi still used in ref..
584  stab_att_sp_euler = *rpy;
585 
587 }
588 
590 {
591  // stab_att_sp_euler.psi still used in ref..
593 
594  // compute sp_euler phi/theta for debugging/telemetry
595  /* Rotate horizontal commands to body frame by psi */
597  int32_t s_psi, c_psi;
598  PPRZ_ITRIG_SIN(s_psi, psi);
599  PPRZ_ITRIG_COS(c_psi, psi);
600  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
601  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
602 
603  quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
604 }
605 
606 void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
607 {
608  struct FloatQuat q_sp;
609 #if USE_EARTH_BOUND_RC_SETPOINT
610  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
611 #else
612  stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
613 #endif
615 }
int16_t motor_rpm
RPM of the main motor.
int32_t psi
in rad with INT32_ANGLE_FRAC
#define STABILIZATION_ATTITUDE_HELI_INDI_ROLL_P
static void delayed_first_order_lowpass_initialize(struct delayed_first_order_lowpass_filter_t *f, uint32_t omega, uint8_t delay, uint16_t max_inc, uint16_t sample_frequency)
delayed_first_order_lowpass_initialize
int32_t filtered_measurement[INDI_NR_FILTERS][INDI_DOF]
Filtered measurement.
angular rates
int32_t error[INDI_DOF]
virtual control minus measurement
static int32_t delayed_first_order_lowpass_propagate(struct delayed_first_order_lowpass_filter_t *f, int32_t input)
delayed_first_order_lowpass_propagate
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 int32_quat_normalize(struct Int32Quat *q)
normalize a quaternion inplace
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
static void notch_filter_init(struct SecondOrderNotchFilter *filter, float cutoff_frequency, float bandwidth, uint16_t sample_frequency)
Initialize second order notch filter.
Definition: notch_filter.h:101
void stabilization_attitude_heli_indi_set_steadystate_roll(float roll)
stabilization_attitude_heli_indi_set_steadystate_roll
void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions.
Simple matrix helper macros.
int32_t command_out[2][INDI_DOF]
Command and command from previous measurement.
Quaternion transformation functions.
static void indi_apply_actuator_models(int32_t _out[], int32_t _in[])
void stabilization_attitude_init(void)
stabilization_attitude_init
Periodic telemetry system header (includes downlink utility and generated code).
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
General attitude stabilization interface for rotorcrafts.
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:207
#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_YAW
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)
#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_YAW
void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
#define STABILIZATION_ATTITUDE_HELI_INDI_ROLL_COMMAND_ROTATION
float alpha
Definition: textons.c:107
Read an attitude setpoint from the RC.
struct HeliIndiGains heli_indi_gains
#define DELAYED_FIRST_ORDER_LOWPASS_FILTER_FILTER_ALPHA_SHIFT
#define INDI_YAW_BUFFER_SIZE
int32_t invG[INDI_DOF][INDI_DOF]
Inverse control effectiveness matrix.
#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_THRUST
void indi_apply_actuator_notch_filters(int32_t _out[], int32_t _in[])
static void int32_vect_sum(int32_t *o, const int32_t *a, const int32_t *b, const int n)
o = a + b
static void init_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, float cut_off, float sample_time, int32_t value)
Init a second order Butterworth filter.
struct Int32Quat sp_offset
#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_ROLL
#define STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_PITCH
int32_t z
Down.
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1113
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
#define FALSE
Definition: std.h:5
void(* apply_actuator_filters[INDI_NR_FILTERS])(int32_t _out[], int32_t _in[])
Roation quaternion.
int32_t y
East.
int32_t actuator_out[INDI_DOF]
Actuator position.
void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree, bool coordinated_turn)
void(* apply_compensator_filters)(int32_t _out[], int32_t _in[])
int32_t r
in rad/s with INT32_RATE_FRAC
#define PPRZ_ITRIG_SIN(_s, _a)
#define STABILIZATION_ATTITUDE_HELI_INDI_PITCH_P
#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_THRUST
#define TRUE
Definition: std.h:4
int32_t roll_comp_angle
Angle to rotate pitch/roll commands with INT32_ANGLE_FRAC.
#define INDI_NR_FILTERS
#define STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_ROLL
void indi_apply_measurement_butterworth_filters(int32_t _out[], int32_t _in[])
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
Definition: state.h:1191
static float heading
Definition: ahrs_infrared.c:45
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_ROLL
int32_t filtered_actuator[INDI_NR_FILTERS][INDI_DOF]
Filtered actuator position.
void stabilization_attitude_enter(void)
float sp_offset_pitch
Neutral pitch angle [deg].
Paparazzi floating point algebra.
int32_t reference[INDI_DOF]
Range -MAX_PPRZ:MAX_PPRZ.
static void indi_apply_compensator_filters(int32_t _out[], int32_t _in[])
The main idea of this function is to slow down a certain actuator, so that the actuator dynamics filt...
euler angles
#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_PITCH
int32_t x
North.
void stabilization_attitude_set_failsafe_setpoint(void)
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
struct SecondOrderNotchFilter measurement_notchfilter[INDI_DOF]
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:696
void stabilization_attitude_heli_indi_set_steadystate_pitch(float pitch)
stabilization_attitude_heli_indi_set_steadystate_pitch
void indi_apply_measurement_notch_filters(int32_t _out[], int32_t _in[])
float sp_offset_roll
Neutral roll angle [deg].
#define STABILIZATION_ATTITUDE_HELI_INDI_YAW_P
Butterworth2LowPass_int measurement_lowpass_filters[INDI_DOF]
#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_PITCH
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
static int32_t update_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, int32_t value)
Update second order Butterworth low pass filter state with a new value(fixed point version)...
void int32_quat_comp_inv(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions.
bool enable_notch
Use notch filters.
void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading)
#define MAT_MUL_VECT(_n, o, a, b)
o = a * b
signed long int32_t
Definition: types.h:19
static void indi_apply_notch_filters(struct SecondOrderNotchFilter *filter, int32_t _out[], int32_t _in[])
#define ANGLE_BFP_OF_REAL(_af)
#define INT32_TRIG_FRAC
Core autopilot interface common to all firmwares.
int32_t du[INDI_DOF]
Actuator commanded increment.
static void notch_filter_set_filter_frequency(struct SecondOrderNotchFilter *filter, float frequency)
Set notch filter frequency in Hz.
Definition: notch_filter.h:65
static void notch_filter_update(struct SecondOrderNotchFilter *filter, int32_t *input_signal, int32_t *output_signal)
Notch filter propagate.
Definition: notch_filter.h:79
void stabilization_attitude_run(bool in_flight)
Butterworth2LowPass_int actuator_lowpass_filters[INDI_DOF]
int32_t phi
in rad with INT32_ANGLE_FRAC
unsigned char uint8_t
Definition: types.h:14
int32_t pprz_itrig_sin(int32_t angle)
API to get/set the generic vehicle states.
struct IndiController_int heli_indi_ctl
vector in North East Down coordinates
int32_t buffer[DELAYED_FIRST_ORDER_LOWPASS_FILTER_BUFFER_SIZE]
void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov)
Quaternion from orientation vector.
void(* apply_measurement_filters[INDI_NR_FILTERS])(int32_t _out[], int32_t _in[])
int32_t measurement[INDI_DOF]
Raw measurement.
static void int32_quat_wrap_shortest(struct Int32Quat *q)
static void int32_vect_diff(int32_t *o, const int32_t *a, const int32_t *b, const int n)
o = a - b
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:28
int32_t p
in rad/s with INT32_RATE_FRAC
uint16_t rpm_sensor_get_rpm(void)
Definition: rpm_sensor.c:68
int32_t pitch_comp_angle
Angle to rotate pitch/roll commands with INT32_ANGLE_FRAC.
#define INDI_NOTCH_MIN_RPM
static void int32_vect_copy(int32_t *a, const int32_t *b, const int n)
a = b
#define MAX_PPRZ
Definition: paparazzi.h:8
#define STABILIZATION_ATTITUDE_HELI_INDI_PITCH_COMMAND_ROTATION
struct SecondOrderNotchFilter actuator_notchfilter[INDI_DOF]
void indi_apply_actuator_butterworth_filters(int32_t _out[], int32_t _in[])
void(* apply_actuator_models)(int32_t _out[], int32_t _in[])
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
int32_t pprz_itrig_cos(int32_t angle)
void stabilization_attitude_heli_indi_set_steadystate_pitchroll()
stabilization_attitude_heli_indi_set_steadystate_pitchroll
int32_t q
in rad/s with INT32_RATE_FRAC
Rotation quaternion.
#define STABILIZATION_ATTITUDE_HELI_INDI_YAW_D
struct delayed_first_order_lowpass_filter_t actuator_model[INDI_DOF]
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:1020
int32_t u_setpoint[INDI_DOF]
Actuator setpoint without compensator.
#define PPRZ_ITRIG_COS(_c, _a)
Paparazzi fixed point algebra.