Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
stabilization_attitude_euler_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 
28 #include "generated/airframe.h"
29 
32 
33 #include "std.h"
34 #include "paparazzi.h"
35 #include "math/pprz_algebra_int.h"
36 #include "state.h"
37 
39 #ifndef USE_ATTITUDE_REF
40 #define USE_ATTITUDE_REF 1
41 #endif
42 
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 
66 static inline void reset_psi_ref_from_body(void)
67 {
68  //sp has been set from body using stabilization_attitude_get_yaw_i, use that value
72 }
73 
74 #if PERIODIC_TELEMETRY
76 
77 static void send_att(struct transport_tx *trans, struct link_device *dev)
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  pprz_msg_send_STAB_ATTITUDE_REF_INT(trans, dev, AC_ID,
116 }
117 #endif
118 
120 {
121 
123 
125 
126 
128  STABILIZATION_ATTITUDE_PHI_PGAIN,
129  STABILIZATION_ATTITUDE_THETA_PGAIN,
130  STABILIZATION_ATTITUDE_PSI_PGAIN);
131 
133  STABILIZATION_ATTITUDE_PHI_DGAIN,
134  STABILIZATION_ATTITUDE_THETA_DGAIN,
135  STABILIZATION_ATTITUDE_PSI_DGAIN);
136 
138  STABILIZATION_ATTITUDE_PHI_IGAIN,
139  STABILIZATION_ATTITUDE_THETA_IGAIN,
140  STABILIZATION_ATTITUDE_PSI_IGAIN);
141 
143  STABILIZATION_ATTITUDE_PHI_DDGAIN,
144  STABILIZATION_ATTITUDE_THETA_DDGAIN,
145  STABILIZATION_ATTITUDE_PSI_DDGAIN);
146 
147 
149 
150 #if PERIODIC_TELEMETRY
151  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_INT, send_att);
152  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_REF_INT, send_att_ref);
153 #endif
154 }
155 
156 void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
157 {
158  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
159 }
160 
162 {
166 }
167 
169 {
173 }
174 
176 {
177  stab_att_sp_euler = *rpy;
178 }
179 
181 {
182  /* Rotate horizontal commands to body frame by psi */
184  int32_t s_psi, c_psi;
185  PPRZ_ITRIG_SIN(s_psi, psi);
186  PPRZ_ITRIG_COS(c_psi, psi);
187  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
188  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
190 }
191 
192 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
193 #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
194 
195 #define MAX_SUM_ERR 4000000
196 
197 void stabilization_attitude_run(bool_t in_flight)
198 {
199 
200  /* update reference */
201 #if USE_ATTITUDE_REF
203 #else
207 #endif
208 
209  /* compute feedforward command */
210  stabilization_att_ff_cmd[COMMAND_ROLL] =
212  stabilization_att_ff_cmd[COMMAND_PITCH] =
214  stabilization_att_ff_cmd[COMMAND_YAW] =
216 
217  /* compute feedback command */
218  /* attitude error */
219  const struct Int32Eulers att_ref_scaled = {
223  };
224  struct Int32Eulers att_err;
225  struct Int32Eulers *ltp_to_body_euler = stateGetNedToBodyEulers_i();
226  EULERS_DIFF(att_err, att_ref_scaled, (*ltp_to_body_euler));
227  INT32_ANGLE_NORMALIZE(att_err.psi);
228 
229  if (in_flight) {
230  /* update integrator */
233  } else {
235  }
236 
237  /* rate error */
238  const struct Int32Rates rate_ref_scaled = {
242  };
243  struct Int32Rates rate_err;
244  struct Int32Rates *body_rate = stateGetBodyRates_i();
245  RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate));
246 
247  /* PID */
248  stabilization_att_fb_cmd[COMMAND_ROLL] =
249  stabilization_gains.p.x * att_err.phi +
250  stabilization_gains.d.x * rate_err.p +
252 
253  stabilization_att_fb_cmd[COMMAND_PITCH] =
254  stabilization_gains.p.y * att_err.theta +
255  stabilization_gains.d.y * rate_err.q +
257 
258  stabilization_att_fb_cmd[COMMAND_YAW] =
259  stabilization_gains.p.z * att_err.psi +
260  stabilization_gains.d.z * rate_err.r +
262 
263 
264  /* with P gain of 100, att_err of 180deg (3.14 rad)
265  * fb cmd: 100 * 3.14 * 2^12 / 2^CMD_SHIFT = 628
266  * max possible command is 9600
267  */
268 #define CMD_SHIFT 11
269 
270  /* sum feedforward and feedback */
271  stabilization_cmd[COMMAND_ROLL] =
273 
274  stabilization_cmd[COMMAND_PITCH] =
276 
277  stabilization_cmd[COMMAND_YAW] =
279 
280  /* bound the result */
281  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
282  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
283  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
284 
285 }
int32_t psi
in rad with INT32_ANGLE_FRAC
#define INT32_RATE_FRAC
int32_t stabilization_att_fb_cmd[COMMANDS_NB]
angular rates
void stabilization_attitude_enter(void)
struct AttRefEulerInt att_ref_euler_i
Generic transmission transport header.
Definition: transport.h:89
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
#define INT32_ANGLE_FRAC
#define INT_RATES_ZERO(_e)
#define OFFSET_AND_ROUND(_a, _b)
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:124
Read an attitude setpoint from the RC.
struct Int32Rates rate
with REF_RATE_FRAC
struct Int32AttitudeGains stabilization_gains
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
Read attitude setpoint from RC as euler angles.
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:371
#define EULERS_BOUND_CUBE(_v, _min, _max)
Definition: pprz_algebra.h:316
Attitude reference models and state/output (euler int)
void stabilization_attitude_set_failsafe_setpoint(void)
#define INT32_EULERS_LSHIFT(_o, _i, _r)
int32_t r
in rad/s with INT32_RATE_FRAC
#define PPRZ_ITRIG_SIN(_s, _a)
static void reset_psi_ref_from_body(void)
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
Definition: state.h:1162
static float heading
Definition: ahrs_infrared.c:45
void stabilization_attitude_init(void)
#define REF_RATE_FRAC
euler angles
#define INT32_ANGLE_NORMALIZE(_a)
#define OFFSET_AND_ROUND2(_a, _b)
struct Int32Eulers stabilization_att_sum_err
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
#define CMD_SHIFT
void attitude_ref_euler_int_init(struct AttRefEulerInt *ref)
void attitude_ref_euler_int_update(struct AttRefEulerInt *ref, struct Int32Eulers *sp_euler)
Propagate reference model.
#define REF_ANGLE_FRAC
int32_t stabilization_att_ff_cmd[COMMANDS_NB]
signed long int32_t
Definition: types.h:19
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:69
#define INT32_TRIG_FRAC
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
struct Int32Rates accel
with REF_ACCEL_FRAC
int32_t phi
in rad with INT32_ANGLE_FRAC
#define INT_EULERS_ZERO(_e)
API to get/set the generic vehicle states.
struct Int32Eulers euler
with REF_ANGLE_FRAC
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:294
void stabilization_attitude_run(bool_t in_flight)
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:28
int32_t p
in rad/s with INT32_RATE_FRAC
#define EULERS_ADD(_a, _b)
Definition: pprz_algebra.h:280
#define MAX_PPRZ
Definition: paparazzi.h:8
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1096
int32_t q
in rad/s with INT32_RATE_FRAC
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
#define PPRZ_ITRIG_COS(_c, _a)
Paparazzi fixed point algebra.
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC