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_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 
40 
41 float stabilization_att_fb_cmd[COMMANDS_NB];
42 float stabilization_att_ff_cmd[COMMANDS_NB];
43 
44 #if PERIODIC_TELEMETRY
46 
47 static void send_att(void) {
48  struct FloatRates* body_rate = stateGetBodyRates_f();
49  struct FloatEulers* att = stateGetNedToBodyEulers_f();
50  float foo = 0.0;
51  DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(DefaultChannel, DefaultDevice,
52  &(body_rate->p), &(body_rate->q), &(body_rate->r),
53  &(att->phi), &(att->theta), &(att->psi),
60  &stabilization_att_fb_cmd[COMMAND_ROLL],
61  &stabilization_att_fb_cmd[COMMAND_PITCH],
62  &stabilization_att_fb_cmd[COMMAND_YAW],
63  &stabilization_att_ff_cmd[COMMAND_ROLL],
64  &stabilization_att_ff_cmd[COMMAND_PITCH],
65  &stabilization_att_ff_cmd[COMMAND_YAW],
66  &stabilization_cmd[COMMAND_ROLL],
67  &stabilization_cmd[COMMAND_PITCH],
68  &stabilization_cmd[COMMAND_YAW],
69  &foo, &foo, &foo);
70 }
71 
72 static void send_att_ref(void) {
73  DOWNLINK_SEND_STAB_ATTITUDE_REF_FLOAT(DefaultChannel, DefaultDevice,
86 }
87 #endif
88 
90 
92 
94  STABILIZATION_ATTITUDE_PHI_PGAIN,
95  STABILIZATION_ATTITUDE_THETA_PGAIN,
96  STABILIZATION_ATTITUDE_PSI_PGAIN);
97 
99  STABILIZATION_ATTITUDE_PHI_DGAIN,
100  STABILIZATION_ATTITUDE_THETA_DGAIN,
101  STABILIZATION_ATTITUDE_PSI_DGAIN);
102 
104  STABILIZATION_ATTITUDE_PHI_IGAIN,
105  STABILIZATION_ATTITUDE_THETA_IGAIN,
106  STABILIZATION_ATTITUDE_PSI_IGAIN);
107 
109  STABILIZATION_ATTITUDE_PHI_DDGAIN,
110  STABILIZATION_ATTITUDE_THETA_DDGAIN,
111  STABILIZATION_ATTITUDE_PSI_DDGAIN);
112 
114 
115 #if PERIODIC_TELEMETRY
116  register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att);
117  register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref);
118 #endif
119 }
120 
121 void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
122  stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
123 }
124 
126 
127  /* reset psi setpoint to current psi angle */
129 
131 
133 }
134 
136  stab_att_sp_euler.phi = 0.0;
137  stab_att_sp_euler.theta = 0.0;
139 }
140 
143 }
144 
146  struct FloatVect2 cmd_f;
147  cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x);
148  cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y);
149 
150  /* Rotate horizontal commands to body frame by psi */
151  float psi = stateGetNedToBodyEulers_f()->psi;
152  float s_psi = sinf(psi);
153  float c_psi = cosf(psi);
154  stab_att_sp_euler.phi = -s_psi * cmd_f.x + c_psi * cmd_f.y;
155  stab_att_sp_euler.theta = -c_psi * cmd_f.x - s_psi * cmd_f.y;
157 }
158 
159 #define MAX_SUM_ERR 200
160 
161 void stabilization_attitude_run(bool_t in_flight) {
162 
164 
165  /* Compute feedforward */
166  stabilization_att_ff_cmd[COMMAND_ROLL] =
168  stabilization_att_ff_cmd[COMMAND_PITCH] =
170  stabilization_att_ff_cmd[COMMAND_YAW] =
172 
173  /* Compute feedback */
174  /* attitude error */
175  struct FloatEulers *att_float = stateGetNedToBodyEulers_f();
176  struct FloatEulers att_err;
177  EULERS_DIFF(att_err, stab_att_ref_euler, *att_float);
178  FLOAT_ANGLE_NORMALIZE(att_err.psi);
179 
180  if (in_flight) {
181  /* update integrator */
184  }
185  else {
187  }
188 
189  /* rate error */
190  struct FloatRates* rate_float = stateGetBodyRates_f();
191  struct FloatRates rate_err;
192  RATES_DIFF(rate_err, stab_att_ref_rate, *rate_float);
193 
194  /* PID */
195 
196  stabilization_att_fb_cmd[COMMAND_ROLL] =
197  stabilization_gains.p.x * att_err.phi +
198  stabilization_gains.d.x * rate_err.p +
200 
201  stabilization_att_fb_cmd[COMMAND_PITCH] =
202  stabilization_gains.p.y * att_err.theta +
203  stabilization_gains.d.y * rate_err.q +
205 
206  stabilization_att_fb_cmd[COMMAND_YAW] =
207  stabilization_gains.p.z * att_err.psi +
208  stabilization_gains.d.z * rate_err.r +
210 
211 
212  stabilization_cmd[COMMAND_ROLL] =
213  (stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL]);
214  stabilization_cmd[COMMAND_PITCH] =
215  (stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH]);
216  stabilization_cmd[COMMAND_YAW] =
217  (stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW]);
218 
219  /* bound the result */
220  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
221  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
222  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
223 }
int32_t phi
in rad with INT32_ANGLE_FRAC
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:361
#define ANGLE_FLOAT_OF_BFP(_ai)
#define EULERS_ADD(_a, _b)
Definition: pprz_algebra.h:270
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:284
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1027
void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
Periodic telemetry system header (includes downlink utility and generated code).
angular rates
float psi
in radians
void stabilization_attitude_set_failsafe_setpoint(void)
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
Read an attitude setpoint from the RC.
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
void stabilization_attitude_ref_enter()
int32_t theta
in rad with INT32_ANGLE_FRAC
float theta
in radians
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
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
float p
in rad/s
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.
float phi
in radians
struct FloatAttitudeGains stabilization_gains
float stabilization_att_fb_cmd[COMMANDS_NB]
void stabilization_attitude_init(void)
uint16_t foo
Definition: main_demo5.c:54
#define FLOAT_ANGLE_NORMALIZE(_a)
void stabilization_attitude_ref_init(void)
void stabilization_attitude_ref_update()
float r
in rad/s
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1078
signed long int32_t
Definition: types.h:19
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:107
void stabilization_attitude_run(bool_t in_flight)
#define FLOAT_EULERS_ZERO(_e)
API to get/set the generic vehicle states.
struct FloatEulers stabilization_att_sum_err
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
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:622
#define EULERS_BOUND_CUBE(_v, _min, _max)
Definition: pprz_algebra.h:306
float q
in rad/s
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
float stabilization_att_ff_cmd[COMMANDS_NB]
#define MAX_PPRZ
Definition: paparazzi.h:8
float stabilization_attitude_get_heading_f(void)
struct FloatRates stab_att_ref_rate
with REF_RATE_FRAC
struct FloatRates stab_att_ref_accel
with REF_ACCEL_FRAC
euler angles