Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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;
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) {
233  } else {
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++) {
282  }
283 }
284 
286 {
287  for (uint8_t i = 0; i < INDI_DOF; 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 */
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 */
397  STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_ROLL, 1.0 / PERIODIC_FREQUENCY, 0);
399  STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_ROLL, 1.0 / PERIODIC_FREQUENCY, 0);
401  STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_PITCH, 1.0 / PERIODIC_FREQUENCY, 0);
403  STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_PITCH, 1.0 / PERIODIC_FREQUENCY, 0);
405  STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_YAW, 1.0 / PERIODIC_FREQUENCY, 0);
407  STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_YAW, 1.0 / PERIODIC_FREQUENCY, 0);
409  STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_THRUST, 1.0 / PERIODIC_FREQUENCY, 0);
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 */
416  STABILIZATION_ATTITUDE_HELI_INDI_FAST_DYN_ROLL_BW, 0, MAX_PPRZ, PERIODIC_FREQUENCY);
418  STABILIZATION_ATTITUDE_HELI_INDI_FAST_DYN_PITCH_BW, 0, MAX_PPRZ, PERIODIC_FREQUENCY);
419 #endif
420 
421  /* Assign filter functions: */
422  c->apply_actuator_models = &indi_apply_actuator_models;
423  c->apply_compensator_filters = &indi_apply_compensator_filters;
424  c->apply_measurement_filters[0] = &indi_apply_measurement_notch_filters;
425  c->apply_measurement_filters[1] = &indi_apply_measurement_butterworth_filters;
426  c->apply_actuator_filters[0] = &indi_apply_actuator_notch_filters;
427  c->apply_actuator_filters[1] = &indi_apply_actuator_butterworth_filters;
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
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  */
485  c->apply_actuator_models(c->actuator_out, c->command_out[__k - 1]);
486 
487  /* Apply a set of filters, both to the actuator and the measurement */
488  c->apply_actuator_filters[0](c->filtered_actuator[0], c->actuator_out);
489  c->apply_measurement_filters[0](c->filtered_measurement[0], c->measurement);
490  for (uint8_t i = 1; i < INDI_NR_FILTERS; i++) {
491  c->apply_actuator_filters[i](c->filtered_actuator[i], c->filtered_actuator[i - 1]);
492  c->apply_measurement_filters[i](c->filtered_measurement[i], c->filtered_measurement[i - 1]);
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;
531  c->du[INDI_THRUST] >>= 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 */
544  c->apply_compensator_filters(c->command_out[__k], c->u_setpoint);
545 
546  /* At the end, set 'previous' output to current output */
547  int32_vect_copy(c->command_out[__k - 1], c->command_out[__k], INDI_DOF);
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]
553  + c->command_out[__k][INDI_PITCH] * pprz_itrig_sin(c->pitch_comp_angle) / pprz_itrig_cos(c->pitch_comp_angle);
554  stabilization_cmd[COMMAND_PITCH] = c->command_out[__k][INDI_PITCH]
555  + c->command_out[__k][INDI_ROLL] * pprz_itrig_sin(c->roll_comp_angle) / pprz_itrig_cos(c->roll_comp_angle);
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 
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 }
int32_quat_of_eulers
void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e)
Quaternion from Euler angles.
Definition: pprz_algebra_int.c:375
indi_apply_actuator_models
static void indi_apply_actuator_models(int32_t _out[], int32_t _in[])
Definition: stabilization_attitude_heli_indi.c:167
Int32Eulers::theta
int32_t theta
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:148
autopilot_get_motors_on
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:212
c
VIC slots used for the LPC2148 define name e g gps UART1_VIC_SLOT e g modem SPI1_VIC_SLOT SPI1 in mcu_periph spi_arch c or spi_slave_hs_arch c(and some others not using the SPI peripheral yet..) I2C0_VIC_SLOT 8 mcu_periph/i2c_arch.c I2C1_VIC_SLOT 9 mcu_periph/i2c_arch.c USB_VIC_SLOT 10 usb
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
notch_filter_init
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
STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_PITCH
#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_PITCH
Definition: stabilization_attitude_heli_indi.c:107
heli_indi_ctl
struct IndiController_int heli_indi_ctl
Definition: stabilization_attitude_heli_indi.c:139
STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_PITCH
#define STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_PITCH
Definition: stabilization_attitude_heli_indi.c:56
int32_vect_copy
static void int32_vect_copy(int32_t *a, const int32_t *b, const int n)
a = b
Definition: pprz_algebra_int.h:624
pprz_itrig_cos
int32_t pprz_itrig_cos(int32_t angle)
Definition: pprz_trig_int.c:888
stabilization_attitude_read_rc_setpoint_quat_earth_bound_f
void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree, bool coordinated_turn)
Definition: stabilization_attitude_rc_setpoint.c:427
stabilization_attitude.h
quat_from_earth_cmd_i
void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading)
Definition: stabilization_attitude_quat_transformations.c:48
indi_apply_notch_filters
static void indi_apply_notch_filters(struct SecondOrderNotchFilter *filter, int32_t _out[], int32_t _in[])
Definition: stabilization_attitude_heli_indi.c:249
STABILIZATION_ATTITUDE_HELI_INDI_ROLL_COMMAND_ROTATION
#define STABILIZATION_ATTITUDE_HELI_INDI_ROLL_COMMAND_ROTATION
Definition: stabilization_attitude_heli_indi.c:80
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
sp_offset
struct Int32Quat sp_offset
Definition: stabilization_attitude_heli_indi.c:129
actuator_lowpass_filters
Butterworth2LowPass_int actuator_lowpass_filters[INDI_DOF]
Definition: stabilization_attitude_heli_indi.c:146
NedCoor_i::y
int32_t y
East.
Definition: pprz_geodetic_int.h:70
Int32Rates::q
int32_t q
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:181
stabilization_attitude_init
void stabilization_attitude_init(void)
stabilization_attitude_init
Definition: stabilization_attitude_heli_indi.c:342
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
INDI_PITCH
#define INDI_PITCH
Definition: stabilization_attitude_heli_indi.h:34
Int32Quat
Rotation quaternion.
Definition: pprz_algebra_int.h:99
Int32Vect2::y
int32_t y
Definition: pprz_algebra_int.h:85
delayed_first_order_lowpass_filter_t
Definition: delayed_first_order_lowpass_filter.h:35
stabilization_attitude_set_failsafe_setpoint
void stabilization_attitude_set_failsafe_setpoint(void)
Definition: stabilization_attitude_heli_indi.c:571
stateGetNedToBodyQuat_i
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1113
delayed_first_order_lowpass_initialize
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
Definition: delayed_first_order_lowpass_filter.h:120
PPRZ_ITRIG_COS
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:110
INVG_00
#define INVG_00
Definition: stabilization_attitude_heli_indi.c:121
STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_PITCH
#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_PITCH
Definition: stabilization_attitude_heli_indi.c:92
measurement_lowpass_filters
Butterworth2LowPass_int measurement_lowpass_filters[INDI_DOF]
Definition: stabilization_attitude_heli_indi.c:147
actuator_notchfilter
struct SecondOrderNotchFilter actuator_notchfilter[INDI_DOF]
Definition: stabilization_attitude_heli_indi.c:148
alpha
float alpha
Definition: textons.c:107
int32_quat_normalize
static void int32_quat_normalize(struct Int32Quat *q)
normalize a quaternion inplace
Definition: pprz_algebra_int.h:455
pprz_algebra_float.h
Paparazzi floating point algebra.
actuator_model
struct delayed_first_order_lowpass_filter_t actuator_model[INDI_DOF]
Definition: stabilization_attitude_heli_indi.c:142
int32_vect_sum
static void int32_vect_sum(int32_t *o, const int32_t *a, const int32_t *b, const int n)
o = a + b
Definition: pprz_algebra_int.h:631
stateGetBodyRates_i
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
Definition: state.h:1191
stabilization_attitude_get_heading_i
int32_t stabilization_attitude_get_heading_i(void)
Definition: stabilization_attitude_rc_setpoint.c:130
int32_quat_inv_comp
void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions.
Definition: pprz_algebra_int.c:285
paparazzi.h
IndiController_int::motor_rpm
int16_t motor_rpm
RPM of the main motor.
Definition: stabilization_attitude_heli_indi.h:61
IndiController_int::sp_offset_pitch
float sp_offset_pitch
Neutral pitch angle [deg].
Definition: stabilization_attitude_heli_indi.h:63
indi_apply_actuator_butterworth_filters
void indi_apply_actuator_butterworth_filters(int32_t _out[], int32_t _in[])
Definition: stabilization_attitude_heli_indi.c:278
pprz_algebra_int.h
Paparazzi fixed point algebra.
int32_quat_wrap_shortest
static void int32_quat_wrap_shortest(struct Int32Quat *q)
Definition: pprz_algebra_int.h:447
update_butterworth_2_low_pass_int
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).
Definition: low_pass_filter.h:323
rpm_sensor.h
int32_vect_diff
static void int32_vect_diff(int32_t *o, const int32_t *a, const int32_t *b, const int n)
o = a - b
Definition: pprz_algebra_int.h:638
HeliIndiGains::yaw_p
int32_t yaw_p
Definition: stabilization_attitude_heli_indi.h:42
SecondOrderNotchFilter
Definition: notch_filter.h:32
indi_apply_measurement_butterworth_filters
void indi_apply_measurement_butterworth_filters(int32_t _out[], int32_t _in[])
Definition: stabilization_attitude_heli_indi.c:285
IndiController_int
Definition: stabilization_attitude_heli_indi.h:47
NedCoor_i::z
int32_t z
Down.
Definition: pprz_geodetic_int.h:71
int32_quat_comp_inv
void int32_quat_comp_inv(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions.
Definition: pprz_algebra_int.c:277
FloatVect3
Definition: pprz_algebra_float.h:54
STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_ROLL
#define STABILIZATION_ATTITUDE_HELI_INDI_STEADY_STATE_ROLL
Definition: stabilization_attitude_heli_indi.c:53
Int32Rates::p
int32_t p
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:180
telemetry.h
stateGetNedToBodyRMat_i
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
alpha_yaw_dec
int32_t alpha_yaw_dec
Definition: stabilization_attitude_heli_indi.c:144
Int32Vect2::x
int32_t x
Definition: pprz_algebra_int.h:84
Int32Eulers::psi
int32_t psi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:149
DELAYED_FIRST_ORDER_LOWPASS_FILTER_FILTER_ALPHA_SHIFT
#define DELAYED_FIRST_ORDER_LOWPASS_FILTER_FILTER_ALPHA_SHIFT
Definition: delayed_first_order_lowpass_filter.h:33
Int32Eulers::phi
int32_t phi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:147
delayed_first_order_lowpass_filter_t::alpha
int32_t alpha
Definition: delayed_first_order_lowpass_filter.h:39
INT32_INVG_FRAC
#define INT32_INVG_FRAC
Definition: stabilization_attitude_heli_indi.c:125
STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_THRUST
#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_THRUST
Definition: stabilization_attitude_heli_indi.c:113
ANGLE_BFP_OF_REAL
#define ANGLE_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:210
NedCoor_i
vector in North East Down coordinates
Definition: pprz_geodetic_int.h:68
stab_att_sp_euler
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
Definition: stabilization_attitude_heli_indi.c:128
INVG_33
#define INVG_33
Definition: stabilization_attitude_heli_indi.c:124
Int32Quat::qz
int32_t qz
Definition: pprz_algebra_int.h:103
STABILIZATION_ATTITUDE_HELI_INDI_YAW_P
#define STABILIZATION_ATTITUDE_HELI_INDI_YAW_P
Definition: stabilization_attitude_heli_indi.c:65
Int32Vect2
Definition: pprz_algebra_int.h:83
Int32Vect3
Definition: pprz_algebra_int.h:88
INDI_NOTCH_MIN_RPM
#define INDI_NOTCH_MIN_RPM
Definition: stabilization_attitude_heli_indi.c:118
stabilization_attitude_enter
void stabilization_attitude_enter(void)
Definition: stabilization_attitude_heli_indi.c:565
stabilization_attitude_rc_setpoint.h
uint8_t
unsigned char uint8_t
Definition: types.h:14
STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_ROLL
#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_ROLL
Definition: stabilization_attitude_heli_indi.c:89
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
IndiController_int::sp_offset_roll
float sp_offset_roll
Neutral roll angle [deg].
Definition: stabilization_attitude_heli_indi.h:62
QUAT_BFP_OF_REAL
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:752
stabilization_attitude_heli_indi_set_steadystate_roll
void stabilization_attitude_heli_indi_set_steadystate_roll(float roll)
stabilization_attitude_heli_indi_set_steadystate_roll
Definition: stabilization_attitude_heli_indi.c:311
indi_apply_compensator_filters
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...
Definition: stabilization_attitude_heli_indi.c:206
INVG_22
#define INVG_22
Definition: stabilization_attitude_heli_indi.c:123
autopilot.h
STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_YAW
#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_YAW
Definition: stabilization_attitude_heli_indi.c:110
stabilization_attitude_set_earth_cmd_i
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
Definition: stabilization_attitude_heli_indi.c:589
Int32Quat::qi
int32_t qi
Definition: pprz_algebra_int.h:100
INDI_ROLL
#define INDI_ROLL
Definition: stabilization_attitude_heli_indi.h:33
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
stabilization_attitude_set_rpy_setpoint_i
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
Definition: stabilization_attitude_heli_indi.c:581
stabilization_attitude_heli_indi.h
rpm_sensor_get_rpm
uint16_t rpm_sensor_get_rpm(void)
Definition: rpm_sensor.c:68
int32_rmat_vmult
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
Definition: pprz_algebra_int.c:107
IndiController_int::enable_notch
bool enable_notch
Use notch filters.
Definition: stabilization_attitude_heli_indi.h:60
indi_apply_measurement_notch_filters
void indi_apply_measurement_notch_filters(int32_t _out[], int32_t _in[])
Definition: stabilization_attitude_heli_indi.c:273
stabilization_attitude_read_rc_setpoint_quat_f
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.
Definition: stabilization_attitude_rc_setpoint.c:374
stabilization_attitude_quat_transformations.h
INVG_11
#define INVG_11
Definition: stabilization_attitude_heli_indi.c:122
__k
#define __k
Definition: stabilization_attitude_heli_indi.h:30
INDI_THRUST
#define INDI_THRUST
Definition: stabilization_attitude_heli_indi.h:36
Int32Quat::qy
int32_t qy
Definition: pprz_algebra_int.h:102
STABILIZATION_ATTITUDE_HELI_INDI_PITCH_P
#define STABILIZATION_ATTITUDE_HELI_INDI_PITCH_P
Definition: stabilization_attitude_heli_indi.c:62
stabilization_attitude_read_rc
void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
Definition: stabilization_attitude_heli_indi.c:606
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
INDI_NR_FILTERS
#define INDI_NR_FILTERS
Definition: stabilization_attitude_heli_indi.h:31
int32_t
signed long int32_t
Definition: types.h:19
notch_filter_set_filter_frequency
static void notch_filter_set_filter_frequency(struct SecondOrderNotchFilter *filter, float frequency)
Set notch filter frequency in Hz.
Definition: notch_filter.h:65
STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_YAW
#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_YAW
Definition: stabilization_attitude_heli_indi.c:95
notch_filter_update
static void notch_filter_update(struct SecondOrderNotchFilter *filter, int32_t *input_signal, int32_t *output_signal)
Notch filter propagate.
Definition: notch_filter.h:79
pprz_itrig_sin
int32_t pprz_itrig_sin(int32_t angle)
Definition: pprz_trig_int.c:859
HeliIndiGains::roll_p
int32_t roll_p
Definition: stabilization_attitude_heli_indi.h:40
float_quat_of_orientation_vect
void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov)
Quaternion from orientation vector.
Definition: pprz_algebra_float.c:560
INDI_DOF
#define INDI_DOF
Definition: stabilization_attitude_heli_indi.h:32
measurement_notchfilter
struct SecondOrderNotchFilter measurement_notchfilter[INDI_DOF]
Definition: stabilization_attitude_heli_indi.c:149
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
delayed_first_order_lowpass_propagate
static int32_t delayed_first_order_lowpass_propagate(struct delayed_first_order_lowpass_filter_t *f, int32_t input)
delayed_first_order_lowpass_propagate
Definition: delayed_first_order_lowpass_filter.h:54
stabilization_cmd
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
init_butterworth_2_low_pass_int
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.
Definition: low_pass_filter.h:311
HeliIndiGains
Definition: stabilization_attitude_heli_indi.h:39
delayed_first_order_lowpass_filter_t::idx
uint8_t idx
Definition: delayed_first_order_lowpass_filter.h:42
INDI_YAW
#define INDI_YAW
Definition: stabilization_attitude_heli_indi.h:35
indi_apply_actuator_notch_filters
void indi_apply_actuator_notch_filters(int32_t _out[], int32_t _in[])
Definition: stabilization_attitude_heli_indi.c:268
NedCoor_i::x
int32_t x
North.
Definition: pprz_geodetic_int.h:69
stateGetAccelNed_i
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:1020
stabilization_attitude_run
void stabilization_attitude_run(bool in_flight)
Definition: stabilization_attitude_heli_indi.c:434
STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_ROLL
#define STABILIZATION_ATTITUDE_HELI_INDI_BUTTERW_CUTOFF_ROLL
Definition: stabilization_attitude_heli_indi.c:104
pprz_simple_matrix.h
Simple matrix helper macros.
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
MAT_MUL_VECT
#define MAT_MUL_VECT(_n, o, a, b)
o = a * b
Definition: pprz_simple_matrix.h:86
stateGetNedToBodyEulers_i
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
stab_att_sp_quat
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
Definition: stabilization_attitude_heli_indi.c:127
state.h
INT32_TRIG_FRAC
#define INT32_TRIG_FRAC
Definition: pprz_algebra_int.h:154
FALSE
#define FALSE
Definition: std.h:5
HeliIndiGains::yaw_d
int32_t yaw_d
Definition: stabilization_attitude_heli_indi.h:43
heli_indi_gains
struct HeliIndiGains heli_indi_gains
Definition: stabilization_attitude_heli_indi.c:131
TRUE
#define TRUE
Definition: std.h:4
stabilization_attitude_heli_indi_set_steadystate_pitch
void stabilization_attitude_heli_indi_set_steadystate_pitch(float pitch)
stabilization_attitude_heli_indi_set_steadystate_pitch
Definition: stabilization_attitude_heli_indi.c:298
Int32Quat::qx
int32_t qx
Definition: pprz_algebra_int.h:101
STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_THRUST
#define STABILIZATION_ATTITUDE_HELI_INDI_NOTCHFILT_BW_THRUST
Definition: stabilization_attitude_heli_indi.c:98
STABILIZATION_ATTITUDE_HELI_INDI_ROLL_P
#define STABILIZATION_ATTITUDE_HELI_INDI_ROLL_P
Definition: stabilization_attitude_heli_indi.c:59
SecondOrderLowPass_int
Definition: low_pass_filter.h:183
stabilization_attitude_heli_indi_set_steadystate_pitchroll
void stabilization_attitude_heli_indi_set_steadystate_pitchroll()
stabilization_attitude_heli_indi_set_steadystate_pitchroll
Definition: stabilization_attitude_heli_indi.c:323
Int32Rates::r
int32_t r
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:182
INDI_YAW_BUFFER_SIZE
#define INDI_YAW_BUFFER_SIZE
Definition: stabilization_attitude_heli_indi.h:37
STABILIZATION_ATTITUDE_HELI_INDI_YAW_D
#define STABILIZATION_ATTITUDE_HELI_INDI_YAW_D
Definition: stabilization_attitude_heli_indi.c:68
delayed_first_order_lowpass_filter_t::buffer
int32_t buffer[DELAYED_FIRST_ORDER_LOWPASS_FILTER_BUFFER_SIZE]
Definition: delayed_first_order_lowpass_filter.h:41
low_pass_filter.h
Simple first order low pass filter with bilinear transform.
HeliIndiGains::pitch_p
int32_t pitch_p
Definition: stabilization_attitude_heli_indi.h:41
heading
float heading
Definition: wedgebug.c:258
alpha_yaw_inc
int32_t alpha_yaw_inc
Definition: stabilization_attitude_heli_indi.c:143
STABILIZATION_ATTITUDE_HELI_INDI_PITCH_COMMAND_ROTATION
#define STABILIZATION_ATTITUDE_HELI_INDI_PITCH_COMMAND_ROTATION
Definition: stabilization_attitude_heli_indi.c:83
PPRZ_ITRIG_SIN
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:109