Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
45 
46 float stabilization_att_fb_cmd[COMMANDS_NB];
47 float stabilization_att_ff_cmd[COMMANDS_NB];
48 
50 
51 static const float phi_pgain[] = STABILIZATION_ATTITUDE_PHI_PGAIN;
52 static const float theta_pgain[] = STABILIZATION_ATTITUDE_THETA_PGAIN;
53 static const float psi_pgain[] = STABILIZATION_ATTITUDE_PSI_PGAIN;
54 
55 static const float phi_dgain[] = STABILIZATION_ATTITUDE_PHI_DGAIN;
56 static const float theta_dgain[] = STABILIZATION_ATTITUDE_THETA_DGAIN;
57 static const float psi_dgain[] = STABILIZATION_ATTITUDE_PSI_DGAIN;
58 
59 static const float phi_igain[] = STABILIZATION_ATTITUDE_PHI_IGAIN;
60 static const float theta_igain[] = STABILIZATION_ATTITUDE_THETA_IGAIN;
61 static const float psi_igain[] = STABILIZATION_ATTITUDE_PSI_IGAIN;
62 
63 static const float phi_ddgain[] = STABILIZATION_ATTITUDE_PHI_DDGAIN;
64 static const float theta_ddgain[] = STABILIZATION_ATTITUDE_THETA_DDGAIN;
65 static const float psi_ddgain[] = STABILIZATION_ATTITUDE_PSI_DDGAIN;
66 
67 static const float phi_dgain_d[] = STABILIZATION_ATTITUDE_PHI_DGAIN_D;
68 static const float theta_dgain_d[] = STABILIZATION_ATTITUDE_THETA_DGAIN_D;
69 static const float psi_dgain_d[] = STABILIZATION_ATTITUDE_PSI_DGAIN_D;
70 
71 
72 #if defined COMMAND_ROLL_SURFACE && defined COMMAND_PITCH_SURFACE && defined COMMAND_YAW_SURFACE
73 #define HAS_SURFACE_COMMANDS 1
74 #endif
75 
76 #ifdef HAS_SURFACE_COMMANDS
77 static const float phi_pgain_surface[] = STABILIZATION_ATTITUDE_PHI_PGAIN_SURFACE;
78 static const float theta_pgain_surface[] = STABILIZATION_ATTITUDE_THETA_PGAIN_SURFACE;
79 static const float psi_pgain_surface[] = STABILIZATION_ATTITUDE_PSI_PGAIN_SURFACE;
80 
81 static const float phi_dgain_surface[] = STABILIZATION_ATTITUDE_PHI_DGAIN_SURFACE;
82 static const float theta_dgain_surface[] = STABILIZATION_ATTITUDE_THETA_DGAIN_SURFACE;
83 static const float psi_dgain_surface[] = STABILIZATION_ATTITUDE_PSI_DGAIN_SURFACE;
84 
85 static const float phi_igain_surface[] = STABILIZATION_ATTITUDE_PHI_IGAIN_SURFACE;
86 static const float theta_igain_surface[] = STABILIZATION_ATTITUDE_THETA_IGAIN_SURFACE;
87 static const float psi_igain_surface[] = STABILIZATION_ATTITUDE_PSI_IGAIN_SURFACE;
88 
89 static const float phi_ddgain_surface[] = STABILIZATION_ATTITUDE_PHI_DDGAIN_SURFACE;
90 static const float theta_ddgain_surface[] = STABILIZATION_ATTITUDE_THETA_DDGAIN_SURFACE;
91 static const float psi_ddgain_surface[] = STABILIZATION_ATTITUDE_PSI_DDGAIN_SURFACE;
92 #endif
93 
94 #define IERROR_SCALE 1024
95 
96 #if PERIODIC_TELEMETRY
98 
99 static void send_att(void) {
100  struct FloatRates* body_rate = stateGetBodyRates_f();
101  struct FloatEulers* att = stateGetNedToBodyEulers_f();
102  DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(DefaultChannel, DefaultDevice,
103  &(body_rate->p), &(body_rate->q), &(body_rate->r),
104  &(att->phi), &(att->theta), &(att->psi),
111  &stabilization_att_fb_cmd[COMMAND_ROLL],
112  &stabilization_att_fb_cmd[COMMAND_PITCH],
113  &stabilization_att_fb_cmd[COMMAND_YAW],
114  &stabilization_att_ff_cmd[COMMAND_ROLL],
115  &stabilization_att_ff_cmd[COMMAND_PITCH],
116  &stabilization_att_ff_cmd[COMMAND_YAW],
117  &stabilization_cmd[COMMAND_ROLL],
118  &stabilization_cmd[COMMAND_PITCH],
119  &stabilization_cmd[COMMAND_YAW],
121 }
122 
123 static void send_att_ref(void) {
124  DOWNLINK_SEND_STAB_ATTITUDE_REF_FLOAT(DefaultChannel, DefaultDevice,
137 }
138 #endif
139 
141 
143 
144  for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
150 #ifdef HAS_SURFACE_COMMANDS
151  VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
152  VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
153  VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
154  VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
155 #endif
156  }
157 
162 
163 #if PERIODIC_TELEMETRY
164  register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att);
165  register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref);
166 #endif
167 }
168 
170 {
172  // This could be bad -- Just say no.
173  return;
174  }
175  gain_idx = idx;
177 }
178 
180 
181  /* reset psi setpoint to current psi angle */
183 
185 
188 }
189 
191  /* set failsafe to zero roll/pitch and current heading */
192  float heading2 = stabilization_attitude_get_heading_f() / 2;
193  stab_att_sp_quat.qi = cosf(heading2);
194  stab_att_sp_quat.qx = 0.0;
195  stab_att_sp_quat.qy = 0.0;
196  stab_att_sp_quat.qz = sinf(heading2);
197 }
198 
200  // copy euler setpoint for debugging
202 
204 }
205 
207  struct FloatVect2 cmd_f;
208  cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x);
209  cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y);
210  float heading_f;
211  heading_f = ANGLE_FLOAT_OF_BFP(heading);
212 
213  quat_from_earth_cmd_f(&stab_att_sp_quat, &cmd_f, heading_f);
214 }
215 
216 #ifndef GAIN_PRESCALER_FF
217 #define GAIN_PRESCALER_FF 1
218 #endif
219 static void attitude_run_ff(float ff_commands[], struct FloatAttitudeGains *gains, struct FloatRates *ref_accel)
220 {
221  /* Compute feedforward based on reference acceleration */
222 
223  ff_commands[COMMAND_ROLL] = GAIN_PRESCALER_FF * gains->dd.x * ref_accel->p;
224  ff_commands[COMMAND_PITCH] = GAIN_PRESCALER_FF * gains->dd.y * ref_accel->q;
225  ff_commands[COMMAND_YAW] = GAIN_PRESCALER_FF * gains->dd.z * ref_accel->r;
226 #ifdef HAS_SURFACE_COMMANDS
227  ff_commands[COMMAND_ROLL_SURFACE] = GAIN_PRESCALER_FF * gains->surface_dd.x * ref_accel->p;
228  ff_commands[COMMAND_PITCH_SURFACE] = GAIN_PRESCALER_FF * gains->surface_dd.y * ref_accel->q;
229  ff_commands[COMMAND_YAW_SURFACE] = GAIN_PRESCALER_FF * gains->surface_dd.z * ref_accel->r;
230 #endif
231 }
232 
233 #ifndef GAIN_PRESCALER_P
234 #define GAIN_PRESCALER_P 1
235 #endif
236 #ifndef GAIN_PRESCALER_D
237 #define GAIN_PRESCALER_D 1
238 #endif
239 #ifndef GAIN_PRESCALER_I
240 #define GAIN_PRESCALER_I 1
241 #endif
242 static void attitude_run_fb(float fb_commands[], struct FloatAttitudeGains *gains, struct FloatQuat *att_err,
243  struct FloatRates *rate_err, struct FloatRates *rate_err_d, struct FloatQuat *sum_err)
244 {
245  /* PID feedback */
246  fb_commands[COMMAND_ROLL] =
247  GAIN_PRESCALER_P * gains->p.x * att_err->qx +
248  GAIN_PRESCALER_D * gains->d.x * rate_err->p +
249  GAIN_PRESCALER_D * gains->rates_d.x * rate_err_d->p +
250  GAIN_PRESCALER_I * gains->i.x * sum_err->qx;
251 
252  fb_commands[COMMAND_PITCH] =
253  GAIN_PRESCALER_P * gains->p.y * att_err->qy +
254  GAIN_PRESCALER_D * gains->d.y * rate_err->q +
255  GAIN_PRESCALER_D * gains->rates_d.y * rate_err_d->q +
256  GAIN_PRESCALER_I * gains->i.y * sum_err->qy;
257 
258  fb_commands[COMMAND_YAW] =
259  GAIN_PRESCALER_P * gains->p.z * att_err->qz +
260  GAIN_PRESCALER_D * gains->d.z * rate_err->r +
261  GAIN_PRESCALER_D * gains->rates_d.z * rate_err_d->r +
262  GAIN_PRESCALER_I * gains->i.z * sum_err->qz;
263 
264 #ifdef HAS_SURFACE_COMMANDS
265  fb_commands[COMMAND_ROLL_SURFACE] =
266  GAIN_PRESCALER_P * gains->surface_p.x * att_err->qx +
267  GAIN_PRESCALER_D * gains->surface_d.x * rate_err->p +
268  GAIN_PRESCALER_I * gains->surface_i.x * sum_err->qx;
269 
270  fb_commands[COMMAND_PITCH_SURFACE] =
271  GAIN_PRESCALER_P * gains->surface_p.y * att_err->qy +
272  GAIN_PRESCALER_D * gains->surface_d.y * rate_err->q +
273  GAIN_PRESCALER_I * gains->surface_i.y * sum_err->qy;
274 
275  fb_commands[COMMAND_YAW_SURFACE] =
276  GAIN_PRESCALER_P * gains->surface_p.z * att_err->qz +
277  GAIN_PRESCALER_D * gains->surface_d.z * rate_err->r +
278  GAIN_PRESCALER_I * gains->surface_i.z * sum_err->qz;
279 #endif
280 }
281 
282 void stabilization_attitude_run(bool_t enable_integrator) {
283 
284  /*
285  * Update reference
286  */
288 
289  /*
290  * Compute errors for feedback
291  */
292 
293  /* attitude error */
294  struct FloatQuat att_err;
295  struct FloatQuat* att_quat = stateGetNedToBodyQuat_f();
296  FLOAT_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat);
297  /* wrap it in the shortest direction */
298  FLOAT_QUAT_WRAP_SHORTEST(att_err);
299 
300  /* rate error */
301  struct FloatRates rate_err;
302  struct FloatRates* body_rate = stateGetBodyRates_f();
303  RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate);
304  /* rate_d error */
305  RATES_DIFF(body_rate_d, *body_rate, last_body_rate);
306  RATES_COPY(last_body_rate, *body_rate);
307 
308  /* integrated error */
309  if (enable_integrator) {
310  struct FloatQuat new_sum_err, scaled_att_err;
311  /* update accumulator */
312  scaled_att_err.qi = att_err.qi;
313  scaled_att_err.qx = att_err.qx / IERROR_SCALE;
314  scaled_att_err.qy = att_err.qy / IERROR_SCALE;
315  scaled_att_err.qz = att_err.qz / IERROR_SCALE;
316  FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
317  FLOAT_QUAT_NORMALIZE(new_sum_err);
320  } else {
321  /* reset accumulator */
324  }
325 
327 
329 
330  stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];
331  stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];
332  stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
333 
334 #ifdef HAS_SURFACE_COMMANDS
335  stabilization_cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] + stabilization_att_ff_cmd[COMMAND_ROLL_SURFACE];
336  stabilization_cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] + stabilization_att_ff_cmd[COMMAND_PITCH_SURFACE];
337  stabilization_cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] + stabilization_att_ff_cmd[COMMAND_YAW_SURFACE];
338 #endif
339 
340  /* bound the result */
341  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
342  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
343  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
344 }
345 
346 void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
347 
348  stabilization_attitude_read_rc_setpoint_quat_f(&stab_att_sp_quat, in_flight, in_carefree, coordinated_turn);
349  //FLOAT_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
350 }
#define FLOAT_QUAT_COPY(_qo, _qi)
#define FLOAT_RATES_ZERO(_r)
int32_t phi
in rad with INT32_ANGLE_FRAC
void stabilization_attitude_ref_schedule(uint8_t idx)
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:361
struct FloatRates last_body_rate
static const float theta_dgain[]
static const float theta_ddgain[]
#define ANGLE_FLOAT_OF_BFP(_ai)
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1027
Quaternion transformation functions.
void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
static int gain_idx
Periodic telemetry system header (includes downlink utility and generated code).
static const float phi_igain[]
angular rates
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
float psi
in radians
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
static const float psi_dgain_d[]
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:326
float stabilization_att_fb_cmd[COMMANDS_NB]
Read an attitude setpoint from the RC.
void stabilization_attitude_ref_enter()
int32_t theta
in rad with INT32_ANGLE_FRAC
static const float phi_pgain[]
struct FloatRates body_rate_d
float theta
in radians
struct FloatQuat stab_att_ref_quat
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
euler angles
struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_GAIN_NB]
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1017
static void attitude_run_ff(float ff_commands[], struct FloatAttitudeGains *gains, struct FloatRates *ref_accel)
float p
in rad/s
static const float phi_dgain_d[]
Roation quaternion.
void stabilization_attitude_gain_schedule(uint8_t idx)
void stabilization_attitude_enter(void)
float heading
Definition: ahrs_infrared.c:40
struct FloatEulers stab_att_ref_euler
with REF_ANGLE_FRAC
Paparazzi floating point algebra.
static const float psi_dgain[]
float phi
in radians
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)
static const float psi_ddgain[]
static const float theta_dgain_d[]
static const float psi_pgain[]
struct FloatEulers stabilization_att_sum_err
void stabilization_attitude_run(bool_t enable_integrator)
void stabilization_attitude_init(void)
#define FLOAT_QUAT_WRAP_SHORTEST(_q)
void stabilization_attitude_ref_init(void)
static const float psi_igain[]
void stabilization_attitude_ref_update()
static const float theta_igain[]
float r
in rad/s
#define FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c)
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1078
signed long int32_t
Definition: types.h:19
static const float phi_dgain[]
#define FLOAT_QUAT_NORMALIZE(_q)
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:107
#define FLOAT_EULERS_ZERO(_e)
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
struct FloatQuat stab_att_sp_quat
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading)
void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
Read attitude setpoint from RC as quaternion Interprets the stick positions as axes.
#define FLOAT_EULERS_OF_QUAT(_e, _q)
int32_t psi
in rad with INT32_ANGLE_FRAC
General stabilization interface for rotorcrafts.
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:28
void quat_from_rpy_cmd_f(struct FloatQuat *quat, struct FloatEulers *rpy)
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:622
static float p[2][2]
float q
in rad/s
#define STABILIZATION_ATTITUDE_GAIN_NB
float stabilization_att_ff_cmd[COMMANDS_NB]
#define MAX_PPRZ
Definition: paparazzi.h:8
float stabilization_attitude_get_heading_f(void)
static const float theta_pgain[]
static const float phi_ddgain[]
#define FLOAT_QUAT_ZERO(_q)
struct FloatQuat stabilization_att_sum_err_quat
struct FloatRates stab_att_ref_rate
with REF_RATE_FRAC
void stabilization_attitude_set_failsafe_setpoint(void)
#define STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT
struct FloatRates stab_att_ref_accel
with REF_ACCEL_FRAC
Paparazzi fixed point algebra.
euler angles