Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
stabilization_attitude_quat_float.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 
42 
44 
46 
49 
50 float stabilization_att_fb_cmd[COMMANDS_NB];
51 float stabilization_att_ff_cmd[COMMANDS_NB];
52 
54 
55 static const float phi_pgain[] = STABILIZATION_ATTITUDE_PHI_PGAIN;
56 static const float theta_pgain[] = STABILIZATION_ATTITUDE_THETA_PGAIN;
57 static const float psi_pgain[] = STABILIZATION_ATTITUDE_PSI_PGAIN;
58 
59 static const float phi_dgain[] = STABILIZATION_ATTITUDE_PHI_DGAIN;
60 static const float theta_dgain[] = STABILIZATION_ATTITUDE_THETA_DGAIN;
61 static const float psi_dgain[] = STABILIZATION_ATTITUDE_PSI_DGAIN;
62 
63 static const float phi_igain[] = STABILIZATION_ATTITUDE_PHI_IGAIN;
64 static const float theta_igain[] = STABILIZATION_ATTITUDE_THETA_IGAIN;
65 static const float psi_igain[] = STABILIZATION_ATTITUDE_PSI_IGAIN;
66 
67 static const float phi_ddgain[] = STABILIZATION_ATTITUDE_PHI_DDGAIN;
68 static const float theta_ddgain[] = STABILIZATION_ATTITUDE_THETA_DDGAIN;
69 static const float psi_ddgain[] = STABILIZATION_ATTITUDE_PSI_DDGAIN;
70 
71 static const float phi_dgain_d[] = STABILIZATION_ATTITUDE_PHI_DGAIN_D;
72 static const float theta_dgain_d[] = STABILIZATION_ATTITUDE_THETA_DGAIN_D;
73 static const float psi_dgain_d[] = STABILIZATION_ATTITUDE_PSI_DGAIN_D;
74 
75 
76 #if defined COMMAND_ROLL_SURFACE && defined COMMAND_PITCH_SURFACE && defined COMMAND_YAW_SURFACE
77 #define HAS_SURFACE_COMMANDS 1
78 #endif
79 
80 #ifdef HAS_SURFACE_COMMANDS
81 static const float phi_pgain_surface[] = STABILIZATION_ATTITUDE_PHI_PGAIN_SURFACE;
82 static const float theta_pgain_surface[] = STABILIZATION_ATTITUDE_THETA_PGAIN_SURFACE;
83 static const float psi_pgain_surface[] = STABILIZATION_ATTITUDE_PSI_PGAIN_SURFACE;
84 
85 static const float phi_dgain_surface[] = STABILIZATION_ATTITUDE_PHI_DGAIN_SURFACE;
86 static const float theta_dgain_surface[] = STABILIZATION_ATTITUDE_THETA_DGAIN_SURFACE;
87 static const float psi_dgain_surface[] = STABILIZATION_ATTITUDE_PSI_DGAIN_SURFACE;
88 
89 static const float phi_igain_surface[] = STABILIZATION_ATTITUDE_PHI_IGAIN_SURFACE;
90 static const float theta_igain_surface[] = STABILIZATION_ATTITUDE_THETA_IGAIN_SURFACE;
91 static const float psi_igain_surface[] = STABILIZATION_ATTITUDE_PSI_IGAIN_SURFACE;
92 
93 static const float phi_ddgain_surface[] = STABILIZATION_ATTITUDE_PHI_DDGAIN_SURFACE;
94 static const float theta_ddgain_surface[] = STABILIZATION_ATTITUDE_THETA_DDGAIN_SURFACE;
95 static const float psi_ddgain_surface[] = STABILIZATION_ATTITUDE_PSI_DDGAIN_SURFACE;
96 #endif
97 
98 #define IERROR_SCALE 1024
99 
100 #if PERIODIC_TELEMETRY
102 
103 static void send_att(struct transport_tx *trans, struct link_device *dev)
104 {
105  struct FloatRates *body_rate = stateGetBodyRates_f();
106  struct FloatEulers *att = stateGetNedToBodyEulers_f();
107  pprz_msg_send_STAB_ATTITUDE_FLOAT(trans, dev, AC_ID,
108  &(body_rate->p), &(body_rate->q), &(body_rate->r),
109  &(att->phi), &(att->theta), &(att->psi),
116  &stabilization_att_fb_cmd[COMMAND_ROLL],
117  &stabilization_att_fb_cmd[COMMAND_PITCH],
118  &stabilization_att_fb_cmd[COMMAND_YAW],
119  &stabilization_att_ff_cmd[COMMAND_ROLL],
120  &stabilization_att_ff_cmd[COMMAND_PITCH],
121  &stabilization_att_ff_cmd[COMMAND_YAW],
122  &stabilization_cmd[COMMAND_ROLL],
123  &stabilization_cmd[COMMAND_PITCH],
124  &stabilization_cmd[COMMAND_YAW],
126 }
127 
128 static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
129 {
130  pprz_msg_send_STAB_ATTITUDE_REF_FLOAT(trans, dev, AC_ID,
143 }
144 
145 static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
146 {
147  struct Int32Quat *quat = stateGetNedToBodyQuat_i();
148  struct Int32Quat refquat;
150  pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
151  &refquat.qi,
152  &refquat.qx,
153  &refquat.qy,
154  &refquat.qz,
155  &(quat->qi),
156  &(quat->qx),
157  &(quat->qy),
158  &(quat->qz));
159 }
160 #endif
161 
163 {
164  /* setpoints */
167  /* reference */
170 
171  for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
177 #ifdef HAS_SURFACE_COMMANDS
178  VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
179  VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
180  VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
181  VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
182 #endif
183  }
184 
188 
189 #if PERIODIC_TELEMETRY
190  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_FLOAT, send_att);
191  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_REF_FLOAT, send_att_ref);
193 #endif
194 }
195 
197 {
199  // This could be bad -- Just say no.
200  return;
201  }
202  gain_idx = idx;
204 }
205 
207 {
208 
209  /* reset psi setpoint to current psi angle */
211 
212  struct FloatQuat *state_quat = stateGetNedToBodyQuat_f();
213 
215 
217 }
218 
220 {
221  /* set failsafe to zero roll/pitch and current heading */
222  float heading2 = stabilization_attitude_get_heading_f() / 2;
223  stab_att_sp_quat.qi = cosf(heading2);
224  stab_att_sp_quat.qx = 0.0;
225  stab_att_sp_quat.qy = 0.0;
226  stab_att_sp_quat.qz = sinf(heading2);
227 }
228 
230 {
231  // copy euler setpoint for debugging
233 
235 }
236 
238 {
239  struct FloatVect2 cmd_f;
240  cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x);
241  cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y);
242  float heading_f;
243  heading_f = ANGLE_FLOAT_OF_BFP(heading);
244 
245  quat_from_earth_cmd_f(&stab_att_sp_quat, &cmd_f, heading_f);
246 }
247 
248 #ifndef GAIN_PRESCALER_FF
249 #define GAIN_PRESCALER_FF 1
250 #endif
251 static void attitude_run_ff(float ff_commands[], struct FloatAttitudeGains *gains, struct FloatRates *ref_accel)
252 {
253  /* Compute feedforward based on reference acceleration */
254 
255  ff_commands[COMMAND_ROLL] = GAIN_PRESCALER_FF * gains->dd.x * ref_accel->p;
256  ff_commands[COMMAND_PITCH] = GAIN_PRESCALER_FF * gains->dd.y * ref_accel->q;
257  ff_commands[COMMAND_YAW] = GAIN_PRESCALER_FF * gains->dd.z * ref_accel->r;
258 #ifdef HAS_SURFACE_COMMANDS
259  ff_commands[COMMAND_ROLL_SURFACE] = GAIN_PRESCALER_FF * gains->surface_dd.x * ref_accel->p;
260  ff_commands[COMMAND_PITCH_SURFACE] = GAIN_PRESCALER_FF * gains->surface_dd.y * ref_accel->q;
261  ff_commands[COMMAND_YAW_SURFACE] = GAIN_PRESCALER_FF * gains->surface_dd.z * ref_accel->r;
262 #endif
263 }
264 
265 #ifndef GAIN_PRESCALER_P
266 #define GAIN_PRESCALER_P 1
267 #endif
268 #ifndef GAIN_PRESCALER_D
269 #define GAIN_PRESCALER_D 1
270 #endif
271 #ifndef GAIN_PRESCALER_I
272 #define GAIN_PRESCALER_I 1
273 #endif
274 static void attitude_run_fb(float fb_commands[], struct FloatAttitudeGains *gains, struct FloatQuat *att_err,
275  struct FloatRates *rate_err, struct FloatRates *rate_err_d, struct FloatQuat *sum_err)
276 {
277  /* PID feedback */
278  fb_commands[COMMAND_ROLL] =
279  GAIN_PRESCALER_P * gains->p.x * att_err->qx +
280  GAIN_PRESCALER_D * gains->d.x * rate_err->p +
281  GAIN_PRESCALER_D * gains->rates_d.x * rate_err_d->p +
282  GAIN_PRESCALER_I * gains->i.x * sum_err->qx;
283 
284  fb_commands[COMMAND_PITCH] =
285  GAIN_PRESCALER_P * gains->p.y * att_err->qy +
286  GAIN_PRESCALER_D * gains->d.y * rate_err->q +
287  GAIN_PRESCALER_D * gains->rates_d.y * rate_err_d->q +
288  GAIN_PRESCALER_I * gains->i.y * sum_err->qy;
289 
290  fb_commands[COMMAND_YAW] =
291  GAIN_PRESCALER_P * gains->p.z * att_err->qz +
292  GAIN_PRESCALER_D * gains->d.z * rate_err->r +
293  GAIN_PRESCALER_D * gains->rates_d.z * rate_err_d->r +
294  GAIN_PRESCALER_I * gains->i.z * sum_err->qz;
295 
296 #ifdef HAS_SURFACE_COMMANDS
297  fb_commands[COMMAND_ROLL_SURFACE] =
298  GAIN_PRESCALER_P * gains->surface_p.x * att_err->qx +
299  GAIN_PRESCALER_D * gains->surface_d.x * rate_err->p +
300  GAIN_PRESCALER_I * gains->surface_i.x * sum_err->qx;
301 
302  fb_commands[COMMAND_PITCH_SURFACE] =
303  GAIN_PRESCALER_P * gains->surface_p.y * att_err->qy +
304  GAIN_PRESCALER_D * gains->surface_d.y * rate_err->q +
305  GAIN_PRESCALER_I * gains->surface_i.y * sum_err->qy;
306 
307  fb_commands[COMMAND_YAW_SURFACE] =
308  GAIN_PRESCALER_P * gains->surface_p.z * att_err->qz +
309  GAIN_PRESCALER_D * gains->surface_d.z * rate_err->r +
310  GAIN_PRESCALER_I * gains->surface_i.z * sum_err->qz;
311 #endif
312 }
313 
314 void stabilization_attitude_run(bool enable_integrator)
315 {
316 
317  /*
318  * Update reference
319  */
320  static const float dt = (1./PERIODIC_FREQUENCY);
322 
323  /*
324  * Compute errors for feedback
325  */
326 
327  /* attitude error */
328  struct FloatQuat att_err;
329  struct FloatQuat *att_quat = stateGetNedToBodyQuat_f();
330  float_quat_inv_comp(&att_err, att_quat, &att_ref_quat_f.quat);
331  /* wrap it in the shortest direction */
332  float_quat_wrap_shortest(&att_err);
333 
334  /* rate error */
335  struct FloatRates rate_err;
336  struct FloatRates *body_rate = stateGetBodyRates_f();
337  RATES_DIFF(rate_err, att_ref_quat_f.rate, *body_rate);
338  /* rate_d error */
339  RATES_DIFF(body_rate_d, *body_rate, last_body_rate);
340  RATES_COPY(last_body_rate, *body_rate);
341 
342 #define INTEGRATOR_BOUND 1.0
343  /* integrated error */
344  if (enable_integrator) {
351  } else {
352  /* reset accumulator */
354  }
355 
357 
360 
361  stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];
362  stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];
363  stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
364 
365 #ifdef HAS_SURFACE_COMMANDS
366  stabilization_cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] +
367  stabilization_att_ff_cmd[COMMAND_ROLL_SURFACE];
368  stabilization_cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] +
369  stabilization_att_ff_cmd[COMMAND_PITCH_SURFACE];
370  stabilization_cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] +
371  stabilization_att_ff_cmd[COMMAND_YAW_SURFACE];
372 #endif
373 
374  /* bound the result */
375  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
376  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
377  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
378 }
379 
380 void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
381 {
382 #if USE_EARTH_BOUND_RC_SETPOINT
383  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&stab_att_sp_quat, in_flight, in_carefree, coordinated_turn);
384 #else
385  stabilization_attitude_read_rc_setpoint_quat_f(&stab_att_sp_quat, in_flight, in_carefree, coordinated_turn);
386 #endif
387  //float_quat_wrap_shortest(&stab_att_sp_quat);
388 }
FloatQuat::qz
float qz
Definition: pprz_algebra_float.h:67
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
FloatAttitudeGains
Definition: stabilization_attitude_common_float.h:37
stabilization_attitude_set_rpy_setpoint_i
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
Definition: stabilization_attitude_quat_float.c:229
FloatAttitudeGains::surface_i
struct FloatVect3 surface_i
Definition: stabilization_attitude_common_float.h:46
ANGLE_FLOAT_OF_BFP
#define ANGLE_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:211
IERROR_SCALE
#define IERROR_SCALE
Definition: stabilization_attitude_quat_float.c:98
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
GAIN_PRESCALER_D
#define GAIN_PRESCALER_D
Definition: stabilization_attitude_quat_float.c:269
stabilization_attitude.h
GAIN_PRESCALER_I
#define GAIN_PRESCALER_I
Definition: stabilization_attitude_quat_float.c:272
float_quat_identity
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
Definition: pprz_algebra_float.h:365
FLOAT_RATES_ZERO
#define FLOAT_RATES_ZERO(_r)
Definition: pprz_algebra_float.h:191
stabilization_gains
struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_GAIN_NB]
Definition: stabilization_attitude_quat_float.c:38
stabilization_attitude_init
void stabilization_attitude_init(void)
stabilization_attitude_init
Definition: stabilization_attitude_quat_float.c:162
Int32Quat
Rotation quaternion.
Definition: pprz_algebra_int.h:99
Int32Vect2::y
int32_t y
Definition: pprz_algebra_int.h:85
phi_ddgain
static const float phi_ddgain[]
Definition: stabilization_attitude_quat_float.c:67
stateGetNedToBodyQuat_i
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1113
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
stateGetBodyRates_f
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
FLOAT_EULERS_ZERO
#define FLOAT_EULERS_ZERO(_e)
Definition: pprz_algebra_float.h:516
last_body_rate
struct FloatRates last_body_rate
Definition: stabilization_attitude_quat_float.c:47
quat_from_earth_cmd_f
void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading)
Definition: stabilization_attitude_quat_transformations.c:63
theta_dgain
static const float theta_dgain[]
Definition: stabilization_attitude_quat_float.c:60
stab_att_sp_quat
struct FloatQuat stab_att_sp_quat
with INT32_QUAT_FRAC
Definition: stabilization_attitude_quat_float.c:41
attitude_run_fb
static void attitude_run_fb(float fb_commands[], struct FloatAttitudeGains *gains, struct FloatQuat *att_err, struct FloatRates *rate_err, struct FloatRates *rate_err_d, struct FloatQuat *sum_err)
Definition: stabilization_attitude_quat_float.c:274
pprz_algebra_float.h
Paparazzi floating point algebra.
theta_pgain
static const float theta_pgain[]
Definition: stabilization_attitude_quat_float.c:56
phi_dgain_d
static const float phi_dgain_d[]
Definition: stabilization_attitude_quat_float.c:71
stabilization_attitude_read_rc
void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
Definition: stabilization_attitude_quat_float.c:380
attitude_ref_quat_float_init
void attitude_ref_quat_float_init(struct AttRefQuatFloat *ref)
Definition: stabilization_attitude_ref_quat_float.c:55
STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT
#define STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT
Definition: stabilization_attitude_quat_float.h:41
GAIN_PRESCALER_P
#define GAIN_PRESCALER_P
Definition: stabilization_attitude_quat_float.c:266
paparazzi.h
stabilization_attitude_enter
void stabilization_attitude_enter(void)
Definition: stabilization_attitude_quat_float.c:206
idx
static uint32_t idx
Definition: nps_radio_control_spektrum.c:105
pprz_algebra_int.h
Paparazzi fixed point algebra.
stabilization_attitude_get_heading_f
float stabilization_attitude_get_heading_f(void)
Definition: stabilization_attitude_rc_setpoint.c:149
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
phi_pgain
static const float phi_pgain[]
Definition: stabilization_attitude_quat_float.c:55
float_quat_inv_comp
void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions.
Definition: pprz_algebra_float.c:336
FloatVect2
Definition: pprz_algebra_float.h:49
psi_igain
static const float psi_igain[]
Definition: stabilization_attitude_quat_float.c:65
telemetry.h
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
std.h
attitude_ref_quat_float_update
void attitude_ref_quat_float_update(struct AttRefQuatFloat *ref, struct FloatQuat *sp_quat, float dt)
Definition: stabilization_attitude_ref_quat_float.c:88
Int32Vect2::x
int32_t x
Definition: pprz_algebra_int.h:84
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
RATES_DIFF
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
stateGetNedToBodyQuat_f
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
AttRefQuatFloat::euler
struct FloatEulers euler
Definition: stabilization_attitude_ref_quat_float.h:51
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
send_att
static void send_att(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_attitude_quat_float.c:103
GAIN_PRESCALER_FF
#define GAIN_PRESCALER_FF
Definition: stabilization_attitude_quat_float.c:249
Int32Quat::qz
int32_t qz
Definition: pprz_algebra_int.h:103
FloatAttitudeGains::surface_dd
struct FloatVect3 surface_dd
Definition: stabilization_attitude_common_float.h:45
FloatAttitudeGains::i
struct FloatVect3 i
Definition: stabilization_attitude_common_float.h:42
Int32Vect2
Definition: pprz_algebra_int.h:83
stabilization_att_fb_cmd
float stabilization_att_fb_cmd[COMMANDS_NB]
Definition: stabilization_attitude_quat_float.c:50
stabilization_attitude_rc_setpoint.h
uint8_t
unsigned char uint8_t
Definition: types.h:14
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
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
stabilization_att_ff_cmd
float stabilization_att_ff_cmd[COMMANDS_NB]
Definition: stabilization_attitude_quat_float.c:51
AttRefQuatFloat::rate
struct FloatRates rate
Definition: stabilization_attitude_ref_quat_float.h:53
QUAT_BFP_OF_REAL
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:752
EULERS_FLOAT_OF_BFP
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:709
stab_att_sp_euler
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
Definition: stabilization_attitude_quat_float.c:40
STABILIZATION_ATTITUDE_GAIN_NB
#define STABILIZATION_ATTITUDE_GAIN_NB
Definition: stabilization_attitude_quat_float.h:37
Int32Quat::qi
int32_t qi
Definition: pprz_algebra_int.h:100
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
AttRefQuatFloat
Attitude reference models and state/output (float)
Definition: stabilization_attitude_ref_quat_float.h:50
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
FloatQuat::qx
float qx
Definition: pprz_algebra_float.h:65
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
send_att_ref
static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_attitude_quat_float.c:128
theta_ddgain
static const float theta_ddgain[]
Definition: stabilization_attitude_quat_float.c:68
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
float_quat_wrap_shortest
static void float_quat_wrap_shortest(struct FloatQuat *q)
Definition: pprz_algebra_float.h:396
FloatAttitudeGains::d
struct FloatVect3 d
Definition: stabilization_attitude_common_float.h:39
stabilization_attitude_quat_transformations.h
Int32Quat::qy
int32_t qy
Definition: pprz_algebra_int.h:102
stabilization_attitude_run
void stabilization_attitude_run(bool enable_integrator)
Definition: stabilization_attitude_quat_float.c:314
attitude_ref_quat_float_schedule
void attitude_ref_quat_float_schedule(struct AttRefQuatFloat *ref, uint8_t idx)
Definition: stabilization_attitude_ref_quat_float.c:177
phi_igain
static const float phi_igain[]
Definition: stabilization_attitude_quat_float.c:63
AttRefQuatFloat::quat
struct FloatQuat quat
Definition: stabilization_attitude_ref_quat_float.h:52
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
int32_t
signed long int32_t
Definition: types.h:19
attitude_ref_quat_float_enter
void attitude_ref_quat_float_enter(struct AttRefQuatFloat *ref, struct FloatQuat *state_quat)
Definition: stabilization_attitude_ref_quat_float.c:78
theta_dgain_d
static const float theta_dgain_d[]
Definition: stabilization_attitude_quat_float.c:72
FloatQuat::qi
float qi
Definition: pprz_algebra_float.h:64
psi_pgain
static const float psi_pgain[]
Definition: stabilization_attitude_quat_float.c:57
FloatAttitudeGains::rates_d
struct FloatVect3 rates_d
Definition: stabilization_attitude_common_float.h:41
stabilization_attitude_gain_schedule
void stabilization_attitude_gain_schedule(uint8_t idx)
Definition: stabilization_attitude_quat_float.c:196
theta_igain
static const float theta_igain[]
Definition: stabilization_attitude_quat_float.c:64
stabilization_cmd
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
FloatAttitudeGains::p
struct FloatVect3 p
Definition: stabilization_attitude_common_float.h:38
gain_idx
static int gain_idx
Definition: stabilization_attitude_quat_float.c:53
stabilization_att_sum_err_quat
struct FloatQuat stabilization_att_sum_err_quat
Definition: stabilization_attitude_quat_float.c:45
stabilization_attitude_set_failsafe_setpoint
void stabilization_attitude_set_failsafe_setpoint(void)
Definition: stabilization_attitude_quat_float.c:219
stabilization_attitude_set_earth_cmd_i
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
Definition: stabilization_attitude_quat_float.c:237
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
phi_dgain
static const float phi_dgain[]
Definition: stabilization_attitude_quat_float.c:59
FloatAttitudeGains::dd
struct FloatVect3 dd
Definition: stabilization_attitude_common_float.h:40
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
send_ahrs_ref_quat
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_attitude_quat_float.c:145
FloatQuat::qy
float qy
Definition: pprz_algebra_float.h:66
FloatAttitudeGains::surface_d
struct FloatVect3 surface_d
Definition: stabilization_attitude_common_float.h:44
state.h
AttRefQuatFloat::accel
struct FloatRates accel
Definition: stabilization_attitude_ref_quat_float.h:54
psi_dgain_d
static const float psi_dgain_d[]
Definition: stabilization_attitude_quat_float.c:73
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
VECT3_ASSIGN
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
FloatAttitudeGains::surface_p
struct FloatVect3 surface_p
Definition: stabilization_attitude_common_float.h:43
psi_dgain
static const float psi_dgain[]
Definition: stabilization_attitude_quat_float.c:61
RATES_COPY
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:337
Int32Quat::qx
int32_t qx
Definition: pprz_algebra_int.h:101
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
attitude_run_ff
static void attitude_run_ff(float ff_commands[], struct FloatAttitudeGains *gains, struct FloatRates *ref_accel)
Definition: stabilization_attitude_quat_float.c:251
body_rate_d
struct FloatRates body_rate_d
Definition: stabilization_attitude_quat_float.c:48
p
static float p[2][2]
Definition: ins_alt_float.c:268
INTEGRATOR_BOUND
#define INTEGRATOR_BOUND
att_ref_quat_f
struct AttRefQuatFloat att_ref_quat_f
Definition: stabilization_attitude_quat_float.c:43
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
float_quat_of_eulers
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
quat of euler roation 'ZYX'
Definition: pprz_algebra_float.c:477
psi_ddgain
static const float psi_ddgain[]
Definition: stabilization_attitude_quat_float.c:69
heading
float heading
Definition: wedgebug.c:258