Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 #ifndef STABILIZATION_ATTITUDE_PHI_FFDGAIN
45 #define STABILIZATION_ATTITUDE_PHI_FFDGAIN 0
46 #endif
47 #ifndef STABILIZATION_ATTITUDE_THETA_FFDGAIN
48 #define STABILIZATION_ATTITUDE_THETA_FFDGAIN 0
49 #endif
50 #ifndef STABILIZATION_ATTITUDE_PSI_FFDGAIN
51 #define STABILIZATION_ATTITUDE_PSI_FFDGAIN 0
52 #endif
53 
55 
56 /* warn if some gains are still negative */
57 #if (STABILIZATION_ATTITUDE_PHI_PGAIN < 0) || \
58  (STABILIZATION_ATTITUDE_THETA_PGAIN < 0) || \
59  (STABILIZATION_ATTITUDE_PSI_PGAIN < 0) || \
60  (STABILIZATION_ATTITUDE_PHI_DGAIN < 0) || \
61  (STABILIZATION_ATTITUDE_THETA_DGAIN < 0) || \
62  (STABILIZATION_ATTITUDE_PSI_DGAIN < 0) || \
63  (STABILIZATION_ATTITUDE_PHI_IGAIN < 0) || \
64  (STABILIZATION_ATTITUDE_THETA_IGAIN < 0) || \
65  (STABILIZATION_ATTITUDE_PSI_IGAIN < 0)
66 #error "ALL control gains have to be positive!!!"
67 #endif
68 
70 
73 
76 
77 static inline void reset_psi_ref_from_body(void)
78 {
79  //sp has been set from body using stabilization_attitude_get_yaw_i, use that value
83 }
84 
85 #if PERIODIC_TELEMETRY
87 
88 static void send_att(struct transport_tx *trans, struct link_device *dev)
89 {
90  struct Int32Rates *body_rate = stateGetBodyRates_i();
91  struct Int32Eulers *att = stateGetNedToBodyEulers_i();
92  pprz_msg_send_STAB_ATTITUDE_INT(trans, dev, AC_ID,
93  &(body_rate->p), &(body_rate->q), &(body_rate->r),
94  &(att->phi), &(att->theta), &(att->psi),
101  &stabilization_att_fb_cmd[COMMAND_ROLL],
102  &stabilization_att_fb_cmd[COMMAND_PITCH],
103  &stabilization_att_fb_cmd[COMMAND_YAW],
104  &stabilization_att_ff_cmd[COMMAND_ROLL],
105  &stabilization_att_ff_cmd[COMMAND_PITCH],
106  &stabilization_att_ff_cmd[COMMAND_YAW],
107  &stabilization_cmd[COMMAND_ROLL],
108  &stabilization_cmd[COMMAND_PITCH],
109  &stabilization_cmd[COMMAND_YAW]);
110 }
111 
112 static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
113 {
114  pprz_msg_send_STAB_ATTITUDE_REF_INT(trans, dev, AC_ID,
127 }
128 #endif
129 
131 {
132 
134 
136 
137 
139  STABILIZATION_ATTITUDE_PHI_PGAIN,
140  STABILIZATION_ATTITUDE_THETA_PGAIN,
141  STABILIZATION_ATTITUDE_PSI_PGAIN);
142 
144  STABILIZATION_ATTITUDE_PHI_DGAIN,
145  STABILIZATION_ATTITUDE_THETA_DGAIN,
146  STABILIZATION_ATTITUDE_PSI_DGAIN);
147 
149  STABILIZATION_ATTITUDE_PHI_IGAIN,
150  STABILIZATION_ATTITUDE_THETA_IGAIN,
151  STABILIZATION_ATTITUDE_PSI_IGAIN);
152 
154  STABILIZATION_ATTITUDE_PHI_DDGAIN,
155  STABILIZATION_ATTITUDE_THETA_DDGAIN,
156  STABILIZATION_ATTITUDE_PSI_DDGAIN);
157 
162 
164 
165 #if PERIODIC_TELEMETRY
166  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_INT, send_att);
167  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_REF_INT, send_att_ref);
168 #endif
169 }
170 
171 void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
172 {
173  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
174 }
175 
177 {
181 }
182 
184 {
188 }
189 
191 {
192  stab_att_sp_euler = *rpy;
193 }
194 
196 {
197  /* Rotate horizontal commands to body frame by psi */
199  int32_t s_psi, c_psi;
200  PPRZ_ITRIG_SIN(s_psi, psi);
201  PPRZ_ITRIG_COS(c_psi, psi);
202  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
203  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
205 }
206 
207 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
208 #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
209 
210 #define MAX_SUM_ERR 4000000
211 
212 void stabilization_attitude_run(bool in_flight)
213 {
214 
215  /* update reference */
216 #if USE_ATTITUDE_REF
218 #else
222 #endif
223 
224  const struct Int32Eulers att_ref_scaled = {
228  };
229  const struct Int32Rates rate_ref_scaled = {
233  };
234 
235  /* compute feedforward command */
236  stabilization_att_ff_cmd[COMMAND_ROLL] =
238  stabilization_gains.ffd.x * rate_ref_scaled.p;
239  stabilization_att_ff_cmd[COMMAND_PITCH] =
241  stabilization_gains.ffd.y * rate_ref_scaled.q;
242  stabilization_att_ff_cmd[COMMAND_YAW] =
244  stabilization_gains.ffd.z * rate_ref_scaled.r;
245 
246  /* with FFD gain of 100, reference rate of 180deg/s (3.14 rad/s)
247  * INT32_RATE_FRAC = 12, CMD_SHIFT = 11
248  * fb cmd: 100 * 3.14 * 2^INT32_RATE_FRAC / 2^CMD_SHIFT = 628
249  * max possible command is 9600
250  */
251 
252  /* compute feedback command */
253  /* attitude error */
254  struct Int32Eulers att_err;
255  struct Int32Eulers *ltp_to_body_euler = stateGetNedToBodyEulers_i();
256  EULERS_DIFF(att_err, att_ref_scaled, (*ltp_to_body_euler));
257  INT32_ANGLE_NORMALIZE(att_err.psi);
258 
259  if (in_flight) {
260  /* update integrator */
263  } else {
265  }
266 
267  /* rate error */
268  struct Int32Rates rate_err;
269  struct Int32Rates *body_rate = stateGetBodyRates_i();
270  RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate));
271 
272  /* PID */
273  stabilization_att_fb_cmd[COMMAND_ROLL] =
274  stabilization_gains.p.x * att_err.phi +
275  stabilization_gains.d.x * rate_err.p +
277 
278  stabilization_att_fb_cmd[COMMAND_PITCH] =
279  stabilization_gains.p.y * att_err.theta +
280  stabilization_gains.d.y * rate_err.q +
282 
283  stabilization_att_fb_cmd[COMMAND_YAW] =
284  stabilization_gains.p.z * att_err.psi +
285  stabilization_gains.d.z * rate_err.r +
287 
288 
289  /* with P gain of 100, att_err of 180deg (3.14 rad)
290  * fb cmd: 100 * 3.14 * 2^12 / 2^CMD_SHIFT = 628
291  * max possible command is 9600
292  */
293 #define CMD_SHIFT 11
294 
295  /* sum feedforward and feedback */
296  stabilization_cmd[COMMAND_ROLL] =
298 
299  stabilization_cmd[COMMAND_PITCH] =
301 
302  stabilization_cmd[COMMAND_YAW] =
304 
305  /* bound the result */
306  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
307  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
308  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
309 
310 }
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_euler_int.c:72
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
EULERS_ADD
#define EULERS_ADD(_a, _b)
Definition: pprz_algebra.h:281
reset_psi_ref_from_body
static void reset_psi_ref_from_body(void)
Definition: stabilization_attitude_euler_int.c:77
stabilization_attitude_init
void stabilization_attitude_init(void)
Definition: stabilization_attitude_euler_int.c:130
stabilization_attitude.h
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
INT_RATES_ZERO
#define INT_RATES_ZERO(_e)
Definition: pprz_algebra_int.h:575
Int32Rates::q
int32_t q
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:181
MAX_SUM_ERR
#define MAX_SUM_ERR
Definition: stabilization_attitude_euler_int.c:210
STABILIZATION_ATTITUDE_PSI_FFDGAIN
#define STABILIZATION_ATTITUDE_PSI_FFDGAIN
Definition: stabilization_attitude_euler_int.c:51
stab_att_sp_euler
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
Definition: stabilization_attitude_euler_int.c:74
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
INT32_ANGLE_FRAC
#define INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:116
Int32Vect2::y
int32_t y
Definition: pprz_algebra_int.h:85
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
INT_EULERS_ZERO
#define INT_EULERS_ZERO(_e)
Definition: pprz_algebra_int.h:549
stateGetBodyRates_i
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
Definition: state.h:1191
Int32AttitudeGains::dd
struct Int32Vect3 dd
Definition: stabilization_attitude_common_int.h:38
paparazzi.h
attitude_ref_euler_int_init
void attitude_ref_euler_int_init(struct AttRefEulerInt *ref)
Definition: stabilization_attitude_ref_euler_int.c:73
STABILIZATION_ATTITUDE_THETA_FFDGAIN
#define STABILIZATION_ATTITUDE_THETA_FFDGAIN
Definition: stabilization_attitude_euler_int.c:48
AttRefEulerInt
Attitude reference models and state/output (euler int)
Definition: stabilization_attitude_ref_euler_int.h:33
pprz_algebra_int.h
Paparazzi fixed point algebra.
Int32Rates::p
int32_t p
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:180
telemetry.h
std.h
Int32Vect2::x
int32_t x
Definition: pprz_algebra_int.h:84
OFFSET_AND_ROUND
#define OFFSET_AND_ROUND(_a, _b)
Definition: stabilization_attitude_euler_int.c:207
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_set_rpy_setpoint_i
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
Definition: stabilization_attitude_euler_int.c:190
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
Int32AttitudeGains::i
struct Int32Vect3 i
Definition: stabilization_attitude_common_int.h:39
AttRefEulerInt::rate
struct Int32Rates rate
with REF_RATE_FRAC
Definition: stabilization_attitude_ref_euler_int.h:35
STABILIZATION_ATTITUDE_PHI_FFDGAIN
#define STABILIZATION_ATTITUDE_PHI_FFDGAIN
explicitly define to zero to disable feed-forward rate term by default
Definition: stabilization_attitude_euler_int.c:45
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
stabilization_attitude_run
void stabilization_attitude_run(bool in_flight)
Definition: stabilization_attitude_euler_int.c:212
Int32AttitudeGains::ffd
struct Int32Vect3 ffd
Definition: stabilization_attitude_common_int.h:40
EULERS_DIFF
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:295
Int32Vect2
Definition: pprz_algebra_int.h:83
stabilization_attitude_rc_setpoint.h
stabilization_att_sum_err
struct Int32Eulers stabilization_att_sum_err
Definition: stabilization_attitude_euler_int.c:69
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
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
REF_RATE_FRAC
#define REF_RATE_FRAC
Definition: stabilization_attitude_ref_int.h:33
EULERS_BOUND_CUBE
#define EULERS_BOUND_CUBE(_v, _min, _max)
Definition: pprz_algebra.h:317
stabilization_attitude_enter
void stabilization_attitude_enter(void)
Definition: stabilization_attitude_euler_int.c:176
att_ref_euler_i
struct AttRefEulerInt att_ref_euler_i
Definition: stabilization_attitude_euler_int.c:75
send_att
static void send_att(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_attitude_euler_int.c:88
OFFSET_AND_ROUND2
#define OFFSET_AND_ROUND2(_a, _b)
Definition: stabilization_attitude_euler_int.c:208
send_att_ref
static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_attitude_euler_int.c:112
int32_t
signed long int32_t
Definition: types.h:19
stabilization_attitude_read_rc_setpoint_eulers
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool in_flight, bool in_carefree, bool coordinated_turn)
Read attitude setpoint from RC as euler angles.
Definition: stabilization_attitude_rc_setpoint.c:173
attitude_ref_euler_int_update
void attitude_ref_euler_int_update(struct AttRefEulerInt *ref, struct Int32Eulers *sp_euler)
Propagate reference model.
Definition: stabilization_attitude_ref_euler_int.c:84
stabilization_attitude_set_failsafe_setpoint
void stabilization_attitude_set_failsafe_setpoint(void)
Definition: stabilization_attitude_euler_int.c:183
AttRefEulerInt::accel
struct Int32Rates accel
with REF_ACCEL_FRAC
Definition: stabilization_attitude_ref_euler_int.h:36
AttRefEulerInt::euler
struct Int32Eulers euler
with REF_ANGLE_FRAC
Definition: stabilization_attitude_ref_euler_int.h:34
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_gains
struct Int32AttitudeGains stabilization_gains
Definition: stabilization_attitude_euler_int.c:54
stabilization_attitude_read_rc
void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
Definition: stabilization_attitude_euler_int.c:171
stabilization_att_fb_cmd
int32_t stabilization_att_fb_cmd[COMMANDS_NB]
Definition: stabilization_attitude_euler_int.c:71
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
VECT3_ASSIGN
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
CMD_SHIFT
#define CMD_SHIFT
stabilization_attitude_set_earth_cmd_i
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
Definition: stabilization_attitude_euler_int.c:195
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
INT32_ANGLE_NORMALIZE
#define INT32_ANGLE_NORMALIZE(_a)
Definition: pprz_algebra_int.h:126
heading
float heading
Definition: wedgebug.c:258
PPRZ_ITRIG_SIN
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:109