Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
stabilization_attitude_euler_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 
28 #include "generated/airframe.h"
29 
32 
33 #include "std.h"
34 #include "paparazzi.h"
36 #include "state.h"
37 
38 #ifndef USE_ATT_REF
39 #define USE_ATT_REF 1
40 #endif
41 
44 
47 
48 float stabilization_att_fb_cmd[COMMANDS_NB];
49 float stabilization_att_ff_cmd[COMMANDS_NB];
50 
51 #if PERIODIC_TELEMETRY
53 
54 static void send_att(struct transport_tx *trans, struct link_device *dev)
55 {
56  struct FloatRates *body_rate = stateGetBodyRates_f();
57  struct FloatEulers *att = stateGetNedToBodyEulers_f();
58  float foo = 0.0;
59  pprz_msg_send_STAB_ATTITUDE_FLOAT(trans, dev, AC_ID,
60  &(body_rate->p), &(body_rate->q), &(body_rate->r),
61  &(att->phi), &(att->theta), &(att->psi),
68  &stabilization_att_fb_cmd[COMMAND_ROLL],
69  &stabilization_att_fb_cmd[COMMAND_PITCH],
70  &stabilization_att_fb_cmd[COMMAND_YAW],
71  &stabilization_att_ff_cmd[COMMAND_ROLL],
72  &stabilization_att_ff_cmd[COMMAND_PITCH],
73  &stabilization_att_ff_cmd[COMMAND_YAW],
74  &stabilization_cmd[COMMAND_ROLL],
75  &stabilization_cmd[COMMAND_PITCH],
76  &stabilization_cmd[COMMAND_YAW],
77  &foo, &foo, &foo);
78 }
79 
80 static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
81 {
82  pprz_msg_send_STAB_ATTITUDE_REF_FLOAT(trans, dev, AC_ID,
95 }
96 #endif
97 
99 {
100 
102 
104  STABILIZATION_ATTITUDE_PHI_PGAIN,
105  STABILIZATION_ATTITUDE_THETA_PGAIN,
106  STABILIZATION_ATTITUDE_PSI_PGAIN);
107 
109  STABILIZATION_ATTITUDE_PHI_DGAIN,
110  STABILIZATION_ATTITUDE_THETA_DGAIN,
111  STABILIZATION_ATTITUDE_PSI_DGAIN);
112 
114  STABILIZATION_ATTITUDE_PHI_IGAIN,
115  STABILIZATION_ATTITUDE_THETA_IGAIN,
116  STABILIZATION_ATTITUDE_PSI_IGAIN);
117 
119  STABILIZATION_ATTITUDE_PHI_DDGAIN,
120  STABILIZATION_ATTITUDE_THETA_DDGAIN,
121  STABILIZATION_ATTITUDE_PSI_DDGAIN);
122 
124 
125 #if PERIODIC_TELEMETRY
126  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_FLOAT, send_att);
127  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_REF_FLOAT, send_att_ref);
128 #endif
129 }
130 
131 void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
132 {
133  stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
134 }
135 
137 {
138 
139  /* reset psi setpoint to current psi angle */
141 
143 
145 }
146 
148 {
149  stab_att_sp_euler.phi = 0.0;
150  stab_att_sp_euler.theta = 0.0;
152 }
153 
155 {
157 }
158 
160 {
161  struct FloatVect2 cmd_f;
162  cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x);
163  cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y);
164 
165  /* Rotate horizontal commands to body frame by psi */
166  float psi = stateGetNedToBodyEulers_f()->psi;
167  float s_psi = sinf(psi);
168  float c_psi = cosf(psi);
169  stab_att_sp_euler.phi = -s_psi * cmd_f.x + c_psi * cmd_f.y;
170  stab_att_sp_euler.theta = -c_psi * cmd_f.x - s_psi * cmd_f.y;
172 }
173 
174 #define MAX_SUM_ERR 200
175 
176 void stabilization_attitude_run(bool in_flight)
177 {
178 
179 #if USE_ATT_REF
180  static const float dt = (1./PERIODIC_FREQUENCY);
182 #else
186 #endif
187 
188  /* Compute feedforward */
189  stabilization_att_ff_cmd[COMMAND_ROLL] =
191  stabilization_att_ff_cmd[COMMAND_PITCH] =
193  stabilization_att_ff_cmd[COMMAND_YAW] =
195 
196  /* Compute feedback */
197  /* attitude error */
198  struct FloatEulers *att_float = stateGetNedToBodyEulers_f();
199  struct FloatEulers att_err;
200  EULERS_DIFF(att_err, att_ref_euler_f.euler, *att_float);
201  FLOAT_ANGLE_NORMALIZE(att_err.psi);
202 
203  if (in_flight) {
204  /* update integrator */
207  } else {
209  }
210 
211  /* rate error */
212  struct FloatRates *rate_float = stateGetBodyRates_f();
213  struct FloatRates rate_err;
214  RATES_DIFF(rate_err, att_ref_euler_f.rate, *rate_float);
215 
216  /* PID */
217 
218  stabilization_att_fb_cmd[COMMAND_ROLL] =
219  stabilization_gains.p.x * att_err.phi +
220  stabilization_gains.d.x * rate_err.p +
222 
223  stabilization_att_fb_cmd[COMMAND_PITCH] =
224  stabilization_gains.p.y * att_err.theta +
225  stabilization_gains.d.y * rate_err.q +
227 
228  stabilization_att_fb_cmd[COMMAND_YAW] =
229  stabilization_gains.p.z * att_err.psi +
230  stabilization_gains.d.z * rate_err.r +
232 
233 
234  stabilization_cmd[COMMAND_ROLL] =
235  (stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL]);
236  stabilization_cmd[COMMAND_PITCH] =
237  (stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH]);
238  stabilization_cmd[COMMAND_YAW] =
239  (stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW]);
240 
241  /* bound the result */
242  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
243  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
244  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
245 }
stabilization_attitude_enter
void stabilization_attitude_enter(void)
Definition: stabilization_attitude_euler_float.c:136
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
AttRefEulerFloat::rate
struct FloatRates rate
Definition: stabilization_attitude_ref_euler_float.h:37
FloatAttitudeGains
Definition: stabilization_attitude_common_float.h:37
EULERS_ADD
#define EULERS_ADD(_a, _b)
Definition: pprz_algebra.h:281
att_ref_euler_f
struct AttRefEulerFloat att_ref_euler_f
Definition: stabilization_attitude_euler_float.c:46
ANGLE_FLOAT_OF_BFP
#define ANGLE_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:211
attitude_ref_euler_float_update
void attitude_ref_euler_float_update(struct AttRefEulerFloat *ref, struct FloatEulers *sp_eulers, float dt)
Definition: stabilization_attitude_ref_euler_float.c:67
FLOAT_ANGLE_NORMALIZE
#define FLOAT_ANGLE_NORMALIZE(_a)
Definition: pprz_algebra_float.h:99
stabilization_attitude.h
attitude_ref_euler_float_init
void attitude_ref_euler_float_init(struct AttRefEulerFloat *ref)
Definition: stabilization_attitude_ref_euler_float.c:41
stabilization_att_sum_err
struct FloatEulers stabilization_att_sum_err
Definition: stabilization_attitude_euler_float.c:43
FLOAT_RATES_ZERO
#define FLOAT_RATES_ZERO(_r)
Definition: pprz_algebra_float.h:191
Int32Vect2::y
int32_t y
Definition: pprz_algebra_int.h:85
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
stabilization_attitude_set_failsafe_setpoint
void stabilization_attitude_set_failsafe_setpoint(void)
Definition: stabilization_attitude_euler_float.c:147
pprz_algebra_float.h
Paparazzi floating point algebra.
paparazzi.h
foo
uint16_t foo
Definition: main_demo5.c:59
stabilization_attitude_init
void stabilization_attitude_init(void)
Definition: stabilization_attitude_euler_float.c:98
stabilization_attitude_read_rc_setpoint_eulers_f
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool in_flight, bool in_carefree, bool coordinated_turn)
Definition: stabilization_attitude_rc_setpoint.c:252
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
stabilization_attitude_set_rpy_setpoint_i
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
Definition: stabilization_attitude_euler_float.c:154
FloatVect2
Definition: pprz_algebra_float.h:49
stabilization_att_ff_cmd
float stabilization_att_ff_cmd[COMMANDS_NB]
Definition: stabilization_attitude_euler_float.c:49
telemetry.h
std.h
Int32Vect2::x
int32_t x
Definition: pprz_algebra_int.h:84
stabilization_att_fb_cmd
float stabilization_att_fb_cmd[COMMANDS_NB]
Definition: stabilization_attitude_euler_float.c:48
stabilization_gains
struct FloatAttitudeGains stabilization_gains
Definition: stabilization_attitude_euler_float.c:42
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
EULERS_COPY
#define EULERS_COPY(_a, _b)
Definition: pprz_algebra.h:268
MAX_SUM_ERR
#define MAX_SUM_ERR
Definition: stabilization_attitude_euler_float.c:174
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
EULERS_DIFF
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:295
send_att
static void send_att(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_attitude_euler_float.c:54
FloatAttitudeGains::i
struct FloatVect3 i
Definition: stabilization_attitude_common_float.h:42
Int32Vect2
Definition: pprz_algebra_int.h:83
stabilization_attitude_run
void stabilization_attitude_run(bool in_flight)
Definition: stabilization_attitude_euler_float.c:176
stabilization_attitude_rc_setpoint.h
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
AttRefEulerFloat::accel
struct FloatRates accel
Definition: stabilization_attitude_ref_euler_float.h:38
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
EULERS_FLOAT_OF_BFP
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:709
AttRefEulerFloat::euler
struct FloatEulers euler
Definition: stabilization_attitude_ref_euler_float.h:36
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
EULERS_BOUND_CUBE
#define EULERS_BOUND_CUBE(_v, _min, _max)
Definition: pprz_algebra.h:317
FloatAttitudeGains::d
struct FloatVect3 d
Definition: stabilization_attitude_common_float.h:39
attitude_ref_euler_float_enter
void attitude_ref_euler_float_enter(struct AttRefEulerFloat *ref, float psi)
Definition: stabilization_attitude_ref_euler_float.c:62
AttRefEulerFloat
Attitude reference state/output (euler float)
Definition: stabilization_attitude_ref_euler_float.h:35
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
int32_t
signed long int32_t
Definition: types.h:19
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
stabilization_attitude_read_rc
void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
Definition: stabilization_attitude_euler_float.c:131
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
stab_att_sp_euler
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
Definition: stabilization_attitude_euler_float.c:45
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_att_ref
static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_attitude_euler_float.c:80
state.h
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
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
stabilization_attitude_set_earth_cmd_i
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
Definition: stabilization_attitude_euler_float.c:159
heading
float heading
Definition: wedgebug.c:258