Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
stabilization_attitude_quat_int.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
26 #include "generated/airframe.h"
27 
31 
32 #include "std.h"
33 #include "paparazzi.h"
35 #include "math/pprz_algebra_int.h"
36 #include "state.h"
37 
39 #ifndef STABILIZATION_ATTITUDE_PHI_FFDGAIN
40 #define STABILIZATION_ATTITUDE_PHI_FFDGAIN 0
41 #endif
42 #ifndef STABILIZATION_ATTITUDE_THETA_FFDGAIN
43 #define STABILIZATION_ATTITUDE_THETA_FFDGAIN 0
44 #endif
45 #ifndef STABILIZATION_ATTITUDE_PSI_FFDGAIN
46 #define STABILIZATION_ATTITUDE_PSI_FFDGAIN 0
47 #endif
48 
50  {STABILIZATION_ATTITUDE_PHI_PGAIN, STABILIZATION_ATTITUDE_THETA_PGAIN, STABILIZATION_ATTITUDE_PSI_PGAIN },
51  {STABILIZATION_ATTITUDE_PHI_DGAIN, STABILIZATION_ATTITUDE_THETA_DGAIN, STABILIZATION_ATTITUDE_PSI_DGAIN },
52  {STABILIZATION_ATTITUDE_PHI_DDGAIN, STABILIZATION_ATTITUDE_THETA_DDGAIN, STABILIZATION_ATTITUDE_PSI_DDGAIN },
53  {STABILIZATION_ATTITUDE_PHI_IGAIN, STABILIZATION_ATTITUDE_THETA_IGAIN, STABILIZATION_ATTITUDE_PSI_IGAIN },
55 };
56 
57 /* warn if some gains are still negative */
58 #if (STABILIZATION_ATTITUDE_PHI_PGAIN < 0) || \
59  (STABILIZATION_ATTITUDE_THETA_PGAIN < 0) || \
60  (STABILIZATION_ATTITUDE_PSI_PGAIN < 0) || \
61  (STABILIZATION_ATTITUDE_PHI_DGAIN < 0) || \
62  (STABILIZATION_ATTITUDE_THETA_DGAIN < 0) || \
63  (STABILIZATION_ATTITUDE_PSI_DGAIN < 0) || \
64  (STABILIZATION_ATTITUDE_PHI_IGAIN < 0) || \
65  (STABILIZATION_ATTITUDE_THETA_IGAIN < 0) || \
66  (STABILIZATION_ATTITUDE_PSI_IGAIN < 0)
67 #error "ALL control gains have to be positive!!!"
68 #endif
69 
71 
74 
77 
79 
80 #define IERROR_SCALE 128
81 #define GAIN_PRESCALER_FF 48
82 #define GAIN_PRESCALER_P 12
83 #define GAIN_PRESCALER_D 3
84 #define GAIN_PRESCALER_I 3
85 
86 #if PERIODIC_TELEMETRY
88 
89 static void send_att(struct transport_tx *trans, struct link_device *dev) //FIXME really use this message here ?
90 {
91  struct Int32Rates *body_rate = stateGetBodyRates_i();
92  struct Int32Eulers *att = stateGetNedToBodyEulers_i();
93  pprz_msg_send_STAB_ATTITUDE_INT(trans, dev, AC_ID,
94  &(body_rate->p), &(body_rate->q), &(body_rate->r),
95  &(att->phi), &(att->theta), &(att->psi),
102  &stabilization_att_fb_cmd[COMMAND_ROLL],
103  &stabilization_att_fb_cmd[COMMAND_PITCH],
104  &stabilization_att_fb_cmd[COMMAND_YAW],
105  &stabilization_att_ff_cmd[COMMAND_ROLL],
106  &stabilization_att_ff_cmd[COMMAND_PITCH],
107  &stabilization_att_ff_cmd[COMMAND_YAW],
108  &stabilization_cmd[COMMAND_ROLL],
109  &stabilization_cmd[COMMAND_PITCH],
110  &stabilization_cmd[COMMAND_YAW]);
111 }
112 
113 static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
114 {
115  // ref eulers in message are with REF_ANGLE_FRAC, convert
116  struct Int32Eulers ref_euler;
118  pprz_msg_send_STAB_ATTITUDE_REF_INT(trans, dev, AC_ID,
122  &ref_euler.phi,
123  &ref_euler.theta,
124  &ref_euler.psi,
131 }
132 
133 static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
134 {
135  struct Int32Quat *quat = stateGetNedToBodyQuat_i();
136  pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
141  &(quat->qi),
142  &(quat->qx),
143  &(quat->qy),
144  &(quat->qz));
145 }
146 #endif
147 
149 {
150 
152 
154 
155 #if PERIODIC_TELEMETRY
156  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_INT, send_att);
157  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_REF_INT, send_att_ref);
159 #endif
160 }
161 
163 {
164 
165  /* reset psi setpoint to current psi angle */
167 
168  struct Int32Quat *state_quat = stateGetNedToBodyQuat_i();
169 
171 
173 
174 }
175 
177 {
178  /* set failsafe to zero roll/pitch and current heading */
181  stab_att_sp_quat.qx = 0;
182  stab_att_sp_quat.qy = 0;
184 }
185 
187 {
188  // stab_att_sp_euler.psi still used in ref..
189  stab_att_sp_euler = *rpy;
190 
192 }
193 
195 {
196  // stab_att_sp_euler.psi still used in ref..
198 
199  // compute sp_euler phi/theta for debugging/telemetry
200  /* Rotate horizontal commands to body frame by psi */
202  int32_t s_psi, c_psi;
203  PPRZ_ITRIG_SIN(s_psi, psi);
204  PPRZ_ITRIG_COS(c_psi, psi);
205  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
206  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
207 
209 }
210 
211 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
212 #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
213 
214 static void attitude_run_ff(int32_t ff_commands[], struct Int32AttitudeGains *gains, struct Int32Rates *ref_accel)
215 {
216  /* Compute feedforward based on reference acceleration */
217 
218  ff_commands[COMMAND_ROLL] = GAIN_PRESCALER_FF * gains->dd.x * RATE_FLOAT_OF_BFP(ref_accel->p) / (1 << 7);
219  ff_commands[COMMAND_PITCH] = GAIN_PRESCALER_FF * gains->dd.y * RATE_FLOAT_OF_BFP(ref_accel->q) / (1 << 7);
220  ff_commands[COMMAND_YAW] = GAIN_PRESCALER_FF * gains->dd.z * RATE_FLOAT_OF_BFP(ref_accel->r) / (1 << 7);
221 }
222 
223 static void attitude_run_fb(int32_t fb_commands[], struct Int32AttitudeGains *gains, struct Int32Quat *att_err,
224  struct Int32Rates *rate_err, struct Int32Quat *sum_err)
225 {
226  /* PID feedback */
227  fb_commands[COMMAND_ROLL] =
228  GAIN_PRESCALER_P * gains->p.x * QUAT1_FLOAT_OF_BFP(att_err->qx) +
229  GAIN_PRESCALER_D * gains->d.x * RATE_FLOAT_OF_BFP(rate_err->p) +
230  GAIN_PRESCALER_I * gains->i.x * QUAT1_FLOAT_OF_BFP(sum_err->qx);
231 
232  fb_commands[COMMAND_PITCH] =
233  GAIN_PRESCALER_P * gains->p.y * QUAT1_FLOAT_OF_BFP(att_err->qy) +
234  GAIN_PRESCALER_D * gains->d.y * RATE_FLOAT_OF_BFP(rate_err->q) +
235  GAIN_PRESCALER_I * gains->i.y * QUAT1_FLOAT_OF_BFP(sum_err->qy);
236 
237  fb_commands[COMMAND_YAW] =
238  GAIN_PRESCALER_P * gains->p.z * QUAT1_FLOAT_OF_BFP(att_err->qz) +
239  GAIN_PRESCALER_D * gains->d.z * RATE_FLOAT_OF_BFP(rate_err->r) +
240  GAIN_PRESCALER_I * gains->i.z * QUAT1_FLOAT_OF_BFP(sum_err->qz);
241 
242 }
243 
244 void stabilization_attitude_run(bool enable_integrator)
245 {
246 
247  /*
248  * Update reference
249  * Warning: dt is currently not used in the quat_int ref impl
250  * PERIODIC_FREQUENCY is assumed to be 512Hz
251  */
252  static const float dt = (1./PERIODIC_FREQUENCY);
254 
255  /*
256  * Compute errors for feedback
257  */
258 
259  /* attitude error */
260  struct Int32Quat att_err;
261  struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
262  int32_quat_inv_comp(&att_err, att_quat, &att_ref_quat_i.quat);
263  /* wrap it in the shortest direction */
264  int32_quat_wrap_shortest(&att_err);
265  int32_quat_normalize(&att_err);
266 
267  /* rate error */
268  const struct Int32Rates rate_ref_scaled = {
272  };
273  struct Int32Rates rate_err;
274  struct Int32Rates *body_rate = stateGetBodyRates_i();
275  RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate));
276 
277 #define INTEGRATOR_BOUND 100000
278  /* integrated error */
279  if (enable_integrator) {
286  } else {
287  /* reset accumulator */
289  }
290 
291  /* compute the feed forward command */
293 
294  /* compute the feed back command */
296 
297  /* sum feedforward and feedback */
298  stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];
299  stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];
300  stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
301 
302  /* bound the result */
303  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
304  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
305  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
306 }
307 
308 void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
309 {
310  struct FloatQuat q_sp;
311 #if USE_EARTH_BOUND_RC_SETPOINT
312  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
313 #else
314  stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
315 #endif
317 }
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
GAIN_PRESCALER_FF
#define GAIN_PRESCALER_FF
Definition: stabilization_attitude_quat_int.c:81
INTEGRATOR_BOUND
#define INTEGRATOR_BOUND
Int32Eulers::theta
int32_t theta
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:148
stabilization_att_ff_cmd
int32_t stabilization_att_ff_cmd[COMMANDS_NB]
Definition: stabilization_attitude_quat_int.c:73
stabilization_attitude_enter
void stabilization_attitude_enter(void)
Definition: stabilization_attitude_quat_int.c:162
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
AttRefQuatInt::euler
struct Int32Eulers euler
with INT32_ANGLE_FRAC
Definition: stabilization_attitude_ref_quat_int.h:50
stab_att_sp_euler
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
Definition: stabilization_attitude_quat_int.c:76
AttRefQuatInt::quat
struct Int32Quat quat
Definition: stabilization_attitude_ref_quat_int.h:51
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
Int32AttitudeGains::p
struct Int32Vect3 p
Definition: stabilization_attitude_common_int.h:36
INT32_EULERS_LSHIFT
#define INT32_EULERS_LSHIFT(_o, _i, _r)
Definition: pprz_algebra_int.h:558
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
stabilization_gains
struct Int32AttitudeGains stabilization_gains
Definition: stabilization_attitude_quat_int.c:49
stabilization_attitude_init
void stabilization_attitude_init(void)
stabilization_attitude_init
Definition: stabilization_attitude_quat_int.c:148
STABILIZATION_ATTITUDE_PHI_FFDGAIN
#define STABILIZATION_ATTITUDE_PHI_FFDGAIN
explicitly define to zero to disable feed-forward rate term by default
Definition: stabilization_attitude_quat_int.c:40
Int32Rates::q
int32_t q
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:181
attitude_ref_quat_int_enter
void attitude_ref_quat_int_enter(struct AttRefQuatInt *ref, struct Int32Quat *state_quat)
Definition: stabilization_attitude_ref_quat_int.c:77
attitude_run_fb
static void attitude_run_fb(int32_t fb_commands[], struct Int32AttitudeGains *gains, struct Int32Quat *att_err, struct Int32Rates *rate_err, struct Int32Quat *sum_err)
Definition: stabilization_attitude_quat_int.c:223
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
INT32_ANGLE_FRAC
#define INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:116
Int32Quat
Rotation quaternion.
Definition: pprz_algebra_int.h:99
Int32Vect2::y
int32_t y
Definition: pprz_algebra_int.h:85
stateGetNedToBodyQuat_i
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1113
INT32_RATE_FRAC
#define INT32_RATE_FRAC
Definition: pprz_algebra_int.h:117
PPRZ_ITRIG_COS
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:110
send_att
static void send_att(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_attitude_quat_int.c:89
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.
IERROR_SCALE
#define IERROR_SCALE
Definition: stabilization_attitude_quat_int.c:80
stateGetBodyRates_i
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
Definition: state.h:1191
GAIN_PRESCALER_I
#define GAIN_PRESCALER_I
Definition: stabilization_attitude_quat_int.c:84
stabilization_attitude_set_failsafe_setpoint
void stabilization_attitude_set_failsafe_setpoint(void)
Definition: stabilization_attitude_quat_int.c:176
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
stabilization_attitude_get_heading_i
int32_t stabilization_attitude_get_heading_i(void)
Definition: stabilization_attitude_rc_setpoint.c:130
Int32AttitudeGains::dd
struct Int32Vect3 dd
Definition: stabilization_attitude_common_int.h:38
paparazzi.h
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
attitude_ref_quat_int_init
void attitude_ref_quat_int_init(struct AttRefQuatInt *ref)
Definition: stabilization_attitude_ref_quat_int.c:50
Int32Rates::p
int32_t p
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:180
telemetry.h
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
std.h
AttRefQuatInt::rate
struct Int32Rates rate
with REF_RATE_FRAC
Definition: stabilization_attitude_ref_quat_int.h:52
Int32Vect2::x
int32_t x
Definition: pprz_algebra_int.h:84
send_att_ref
static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_attitude_quat_int.c:113
Int32Eulers::psi
int32_t psi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:149
Int32Eulers::phi
int32_t phi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:147
stabilization_attitude_run
void stabilization_attitude_run(bool enable_integrator)
Definition: stabilization_attitude_quat_int.c:244
RATES_DIFF
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
Int32AttitudeGains::d
struct Int32Vect3 d
Definition: stabilization_attitude_common_int.h:37
attitude_run_ff
static void attitude_run_ff(int32_t ff_commands[], struct Int32AttitudeGains *gains, struct Int32Rates *ref_accel)
Definition: stabilization_attitude_quat_int.c:214
Int32AttitudeGains::i
struct Int32Vect3 i
Definition: stabilization_attitude_common_int.h:39
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
GAIN_PRESCALER_D
#define GAIN_PRESCALER_D
Definition: stabilization_attitude_quat_int.c:83
Int32Quat::qz
int32_t qz
Definition: pprz_algebra_int.h:103
Int32Vect2
Definition: pprz_algebra_int.h:83
stabilization_attitude_rc_setpoint.h
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
OFFSET_AND_ROUND
#define OFFSET_AND_ROUND(_a, _b)
Definition: stabilization_attitude_quat_int.c:211
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
stabilization_attitude_set_rpy_setpoint_i
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
Definition: stabilization_attitude_quat_int.c:186
QUAT_BFP_OF_REAL
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:752
stabilization_att_sum_err_quat
struct Int32Quat stabilization_att_sum_err_quat
Definition: stabilization_attitude_quat_int.c:70
STABILIZATION_ATTITUDE_PSI_FFDGAIN
#define STABILIZATION_ATTITUDE_PSI_FFDGAIN
Definition: stabilization_attitude_quat_int.c:46
REF_RATE_FRAC
#define REF_RATE_FRAC
Definition: stabilization_attitude_ref_int.h:33
Int32Quat::qi
int32_t qi
Definition: pprz_algebra_int.h:100
QUAT1_FLOAT_OF_BFP
#define QUAT1_FLOAT_OF_BFP(_qi)
Definition: pprz_algebra_int.h:213
send_ahrs_ref_quat
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_attitude_quat_int.c:133
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
stabilization_attitude_set_earth_cmd_i
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
Definition: stabilization_attitude_quat_int.c:194
Int32Quat::qy
int32_t qy
Definition: pprz_algebra_int.h:102
stabilization_attitude_read_rc
void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
Definition: stabilization_attitude_quat_int.c:308
AttRefQuatInt
Attitude reference models and state/output (quat int)
Definition: stabilization_attitude_ref_quat_int.h:49
int32_t
signed long int32_t
Definition: types.h:19
AttRefQuatInt::accel
struct Int32Rates accel
with REF_ACCEL_FRAC
Definition: stabilization_attitude_ref_quat_int.h:53
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
stabilization_cmd
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
stabilization_att_fb_cmd
int32_t stabilization_att_fb_cmd[COMMANDS_NB]
Definition: stabilization_attitude_quat_int.c:72
STABILIZATION_ATTITUDE_THETA_FFDGAIN
#define STABILIZATION_ATTITUDE_THETA_FFDGAIN
Definition: stabilization_attitude_quat_int.c:43
stab_att_sp_quat
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
Definition: stabilization_attitude_quat_int.c:75
stateGetNedToBodyEulers_i
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
Int32AttitudeGains
Definition: stabilization_attitude_common_int.h:35
state.h
INT32_TRIG_FRAC
#define INT32_TRIG_FRAC
Definition: pprz_algebra_int.h:154
int32_quat_identity
static void int32_quat_identity(struct Int32Quat *q)
initialises a quaternion to identity
Definition: pprz_algebra_int.h:431
RATE_FLOAT_OF_BFP
#define RATE_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:209
Int32Quat::qx
int32_t qx
Definition: pprz_algebra_int.h:101
att_ref_quat_i
struct AttRefQuatInt att_ref_quat_i
Definition: stabilization_attitude_quat_int.c:78
Int32Rates::r
int32_t r
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:182
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
REF_ANGLE_FRAC
#define REF_ANGLE_FRAC
Definition: stabilization_attitude_ref_int.h:34
GAIN_PRESCALER_P
#define GAIN_PRESCALER_P
Definition: stabilization_attitude_quat_int.c:82
attitude_ref_quat_int_update
void attitude_ref_quat_int_update(struct AttRefQuatInt *ref, struct Int32Quat *sp_quat, float dt)
Propagate reference.
Definition: stabilization_attitude_ref_quat_int.c:97
heading
float heading
Definition: wedgebug.c:258
PPRZ_ITRIG_SIN
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:109