Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
stabilization_attitude_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  {STABILIZATION_ATTITUDE_PHI_PGAIN, STABILIZATION_ATTITUDE_THETA_PGAIN, STABILIZATION_ATTITUDE_PSI_PGAIN },
40  {STABILIZATION_ATTITUDE_PHI_DGAIN, STABILIZATION_ATTITUDE_THETA_DGAIN, STABILIZATION_ATTITUDE_PSI_DGAIN },
41  {STABILIZATION_ATTITUDE_PHI_DDGAIN, STABILIZATION_ATTITUDE_THETA_DDGAIN, STABILIZATION_ATTITUDE_PSI_DDGAIN },
42  {STABILIZATION_ATTITUDE_PHI_IGAIN, STABILIZATION_ATTITUDE_THETA_IGAIN, STABILIZATION_ATTITUDE_PSI_IGAIN }
43 };
44 
45 /* warn if some gains are still negative */
46 #if (STABILIZATION_ATTITUDE_PHI_PGAIN < 0) || \
47  (STABILIZATION_ATTITUDE_THETA_PGAIN < 0) || \
48  (STABILIZATION_ATTITUDE_PSI_PGAIN < 0) || \
49  (STABILIZATION_ATTITUDE_PHI_DGAIN < 0) || \
50  (STABILIZATION_ATTITUDE_THETA_DGAIN < 0) || \
51  (STABILIZATION_ATTITUDE_PSI_DGAIN < 0) || \
52  (STABILIZATION_ATTITUDE_PHI_IGAIN < 0) || \
53  (STABILIZATION_ATTITUDE_THETA_IGAIN < 0) || \
54  (STABILIZATION_ATTITUDE_PSI_IGAIN < 0)
55 #error "ALL control gains have to be positive!!!"
56 #endif
57 
59 
62 
65 
67 
68 #define IERROR_SCALE 128
69 #define GAIN_PRESCALER_FF 48
70 #define GAIN_PRESCALER_P 12
71 #define GAIN_PRESCALER_D 3
72 #define GAIN_PRESCALER_I 3
73 
74 #if PERIODIC_TELEMETRY
76 
77 static void send_att(struct transport_tx *trans, struct link_device *dev) //FIXME really use this message here ?
78 {
79  struct Int32Rates *body_rate = stateGetBodyRates_i();
80  struct Int32Eulers *att = stateGetNedToBodyEulers_i();
81  pprz_msg_send_STAB_ATTITUDE_INT(trans, dev, AC_ID,
82  &(body_rate->p), &(body_rate->q), &(body_rate->r),
83  &(att->phi), &(att->theta), &(att->psi),
90  &stabilization_att_fb_cmd[COMMAND_ROLL],
91  &stabilization_att_fb_cmd[COMMAND_PITCH],
92  &stabilization_att_fb_cmd[COMMAND_YAW],
93  &stabilization_att_ff_cmd[COMMAND_ROLL],
94  &stabilization_att_ff_cmd[COMMAND_PITCH],
95  &stabilization_att_ff_cmd[COMMAND_YAW],
96  &stabilization_cmd[COMMAND_ROLL],
97  &stabilization_cmd[COMMAND_PITCH],
98  &stabilization_cmd[COMMAND_YAW]);
99 }
100 
101 static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
102 {
103  // ref eulers in message are with REF_ANGLE_FRAC, convert
104  struct Int32Eulers ref_euler;
106  pprz_msg_send_STAB_ATTITUDE_REF_INT(trans, dev, AC_ID,
110  &ref_euler.phi,
111  &ref_euler.theta,
112  &ref_euler.psi,
119 }
120 
121 static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
122 {
123  struct Int32Quat *quat = stateGetNedToBodyQuat_i();
124  pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
129  &(quat->qi),
130  &(quat->qx),
131  &(quat->qy),
132  &(quat->qz));
133 }
134 #endif
135 
137 {
138 
140 
142 
143 #if PERIODIC_TELEMETRY
144  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_INT, send_att);
145  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_REF_INT, send_att_ref);
147 #endif
148 }
149 
151 {
152 
153  /* reset psi setpoint to current psi angle */
155 
157 
159 
160 }
161 
163 {
164  /* set failsafe to zero roll/pitch and current heading */
167  stab_att_sp_quat.qx = 0;
168  stab_att_sp_quat.qy = 0;
170 }
171 
173 {
174  // stab_att_sp_euler.psi still used in ref..
175  stab_att_sp_euler = *rpy;
176 
178 }
179 
181 {
182  // stab_att_sp_euler.psi still used in ref..
184 
185  // compute sp_euler phi/theta for debugging/telemetry
186  /* Rotate horizontal commands to body frame by psi */
188  int32_t s_psi, c_psi;
189  PPRZ_ITRIG_SIN(s_psi, psi);
190  PPRZ_ITRIG_COS(c_psi, psi);
191  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
192  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
193 
194  quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
195 }
196 
197 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
198 #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
199 
200 static void attitude_run_ff(int32_t ff_commands[], struct Int32AttitudeGains *gains, struct Int32Rates *ref_accel)
201 {
202  /* Compute feedforward based on reference acceleration */
203 
204  ff_commands[COMMAND_ROLL] = GAIN_PRESCALER_FF * gains->dd.x * RATE_FLOAT_OF_BFP(ref_accel->p) / (1 << 7);
205  ff_commands[COMMAND_PITCH] = GAIN_PRESCALER_FF * gains->dd.y * RATE_FLOAT_OF_BFP(ref_accel->q) / (1 << 7);
206  ff_commands[COMMAND_YAW] = GAIN_PRESCALER_FF * gains->dd.z * RATE_FLOAT_OF_BFP(ref_accel->r) / (1 << 7);
207 }
208 
209 static void attitude_run_fb(int32_t fb_commands[], struct Int32AttitudeGains *gains, struct Int32Quat *att_err,
210  struct Int32Rates *rate_err, struct Int32Quat *sum_err)
211 {
212  /* PID feedback */
213  fb_commands[COMMAND_ROLL] =
214  GAIN_PRESCALER_P * gains->p.x * QUAT1_FLOAT_OF_BFP(att_err->qx) +
215  GAIN_PRESCALER_D * gains->d.x * RATE_FLOAT_OF_BFP(rate_err->p) +
216  GAIN_PRESCALER_I * gains->i.x * QUAT1_FLOAT_OF_BFP(sum_err->qx);
217 
218  fb_commands[COMMAND_PITCH] =
219  GAIN_PRESCALER_P * gains->p.y * QUAT1_FLOAT_OF_BFP(att_err->qy) +
220  GAIN_PRESCALER_D * gains->d.y * RATE_FLOAT_OF_BFP(rate_err->q) +
221  GAIN_PRESCALER_I * gains->i.y * QUAT1_FLOAT_OF_BFP(sum_err->qy);
222 
223  fb_commands[COMMAND_YAW] =
224  GAIN_PRESCALER_P * gains->p.z * QUAT1_FLOAT_OF_BFP(att_err->qz) +
225  GAIN_PRESCALER_D * gains->d.z * RATE_FLOAT_OF_BFP(rate_err->r) +
226  GAIN_PRESCALER_I * gains->i.z * QUAT1_FLOAT_OF_BFP(sum_err->qz);
227 
228 }
229 
230 void stabilization_attitude_run(bool enable_integrator)
231 {
232 
233  /*
234  * Update reference
235  * Warning: dt is currently not used in the quat_int ref impl
236  * PERIODIC_FREQUENCY is assumed to be 512Hz
237  */
238  static const float dt = (1./PERIODIC_FREQUENCY);
240 
241  /*
242  * Compute errors for feedback
243  */
244 
245  /* attitude error */
246  struct Int32Quat att_err;
247  struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
248  int32_quat_inv_comp(&att_err, att_quat, &att_ref_quat_i.quat);
249  /* wrap it in the shortest direction */
250  int32_quat_wrap_shortest(&att_err);
251  int32_quat_normalize(&att_err);
252 
253  /* rate error */
254  const struct Int32Rates rate_ref_scaled = {
258  };
259  struct Int32Rates rate_err;
260  struct Int32Rates *body_rate = stateGetBodyRates_i();
261  RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate));
262 
263 #define INTEGRATOR_BOUND 100000
264  /* integrated error */
265  if (enable_integrator) {
272  } else {
273  /* reset accumulator */
275  }
276 
277  /* compute the feed forward command */
279 
280  /* compute the feed back command */
281  attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains, &att_err, &rate_err, &stabilization_att_sum_err_quat);
282 
283  /* sum feedforward and feedback */
284  stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];
285  stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];
286  stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
287 
288  /* bound the result */
289  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
290  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
291  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
292 }
293 
294 void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
295 {
296  struct FloatQuat q_sp;
297 #if USE_EARTH_BOUND_RC_SETPOINT
298  stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
299 #else
300  stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
301 #endif
303 }
int32_t psi
in rad with INT32_ANGLE_FRAC
#define INT32_RATE_FRAC
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
angular rates
void attitude_ref_quat_int_init(struct AttRefQuatInt *ref)
#define INTEGRATOR_BOUND
int32_t stabilization_att_fb_cmd[COMMANDS_NB]
struct Int32Rates rate
with REF_RATE_FRAC
void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree, bool coordinated_turn)
Read attitude setpoint from RC as quaternion Interprets the stick positions as axes.
static void int32_quat_normalize(struct Int32Quat *q)
normalize a quaternion inplace
void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions.
Quaternion transformation functions.
Periodic telemetry system header (includes downlink utility and generated code).
General attitude stabilization interface for rotorcrafts.
int32_t theta
in rad with INT32_ANGLE_FRAC
void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e)
Quaternion from Euler angles.
#define INT32_ANGLE_FRAC
void stabilization_attitude_set_failsafe_setpoint(void)
int32_t stabilization_attitude_get_heading_i(void)
Read an attitude setpoint from the RC.
void stabilization_attitude_init(void)
stabilization_attitude_init
#define GAIN_PRESCALER_I
void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
#define OFFSET_AND_ROUND(_a, _b)
void attitude_ref_quat_int_enter(struct AttRefQuatInt *ref, int32_t psi)
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:371
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1113
Roation quaternion.
#define GAIN_PRESCALER_D
void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree, bool coordinated_turn)
#define INT32_EULERS_LSHIFT(_o, _i, _r)
int32_t r
in rad/s with INT32_RATE_FRAC
#define PPRZ_ITRIG_SIN(_s, _a)
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
Definition: state.h:1191
static float heading
Definition: ahrs_infrared.c:45
Paparazzi floating point algebra.
#define REF_RATE_FRAC
static void attitude_run_fb(int32_t fb_commands[], struct Int32AttitudeGains *gains, struct Int32Quat *att_err, struct Int32Rates *rate_err, struct Int32Quat *sum_err)
euler angles
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:696
struct Int32AttitudeGains stabilization_gains
void attitude_ref_quat_int_update(struct AttRefQuatInt *ref, struct Int32Quat *sp_quat, float dt)
Propagate reference.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
static void int32_quat_identity(struct Int32Quat *q)
initialises a quaternion to identity
#define IERROR_SCALE
void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading)
#define REF_ANGLE_FRAC
#define RATE_FLOAT_OF_BFP(_ai)
signed long int32_t
Definition: types.h:19
struct Int32Eulers euler
with INT32_ANGLE_FRAC
#define QUAT1_FLOAT_OF_BFP(_qi)
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
#define INT32_TRIG_FRAC
int32_t stabilization_att_ff_cmd[COMMANDS_NB]
#define GAIN_PRESCALER_FF
int32_t phi
in rad with INT32_ANGLE_FRAC
API to get/set the generic vehicle states.
Attitude reference models and state/output (quat int)
static void attitude_run_ff(int32_t ff_commands[], struct Int32AttitudeGains *gains, struct Int32Rates *ref_accel)
void stabilization_attitude_run(bool enable_integrator)
struct AttRefQuatInt att_ref_quat_i
static void int32_quat_wrap_shortest(struct Int32Quat *q)
struct Int32Rates accel
with REF_ACCEL_FRAC
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:28
void stabilization_attitude_enter(void)
int32_t p
in rad/s with INT32_RATE_FRAC
static void send_att(struct transport_tx *trans, struct link_device *dev)
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
#define MAX_PPRZ
Definition: paparazzi.h:8
#define GAIN_PRESCALER_P
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
int32_t q
in rad/s with INT32_RATE_FRAC
struct Int32Quat stabilization_att_sum_err_quat
Rotation quaternion.
static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
#define PPRZ_ITRIG_COS(_c, _a)
Paparazzi fixed point algebra.