Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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 
24 
26 #include "state.h"
28 
29 #include "generated/airframe.h"
30 
31 
34 
35 float stabilization_att_fb_cmd[COMMANDS_NB];
36 float stabilization_att_ff_cmd[COMMANDS_NB];
37 
38 
40 
42 
44  STABILIZATION_ATTITUDE_PHI_PGAIN,
45  STABILIZATION_ATTITUDE_THETA_PGAIN,
46  STABILIZATION_ATTITUDE_PSI_PGAIN);
47 
49  STABILIZATION_ATTITUDE_PHI_DGAIN,
50  STABILIZATION_ATTITUDE_THETA_DGAIN,
51  STABILIZATION_ATTITUDE_PSI_DGAIN);
52 
54  STABILIZATION_ATTITUDE_PHI_IGAIN,
55  STABILIZATION_ATTITUDE_THETA_IGAIN,
56  STABILIZATION_ATTITUDE_PSI_IGAIN);
57 
59  STABILIZATION_ATTITUDE_PHI_DDGAIN,
60  STABILIZATION_ATTITUDE_THETA_DDGAIN,
61  STABILIZATION_ATTITUDE_PSI_DDGAIN);
62 
64 
65 }
66 
67 
68 void stabilization_attitude_read_rc(bool_t in_flight) {
70 }
71 
72 
74 
75  /* reset psi setpoint to current psi angle */
77 
79 
81 }
82 
84  stab_att_sp_euler.phi = 0.0;
87 }
88 
91 }
92 
93 
94 #define MAX_SUM_ERR RadOfDeg(56000)
95 
96 void stabilization_attitude_run(bool_t in_flight) {
97 
99 
100  /* Compute feedforward */
101  stabilization_att_ff_cmd[COMMAND_ROLL] =
103  stabilization_att_ff_cmd[COMMAND_PITCH] =
105  stabilization_att_ff_cmd[COMMAND_YAW] =
107 
108  /* Compute feedback */
109  /* attitude error */
110  struct FloatEulers *att_float = stateGetNedToBodyEulers_f();
111  struct FloatEulers att_err;
112  EULERS_DIFF(att_err, stab_att_ref_euler, *att_float);
113  FLOAT_ANGLE_NORMALIZE(att_err.psi);
114 
115  if (in_flight) {
116  /* update integrator */
119  }
120  else {
122  }
123 
124  /* rate error */
125  struct FloatRates* rate_float = stateGetBodyRates_f();
126  struct FloatRates rate_err;
127  RATES_DIFF(rate_err, stab_att_ref_rate, *rate_float);
128 
129  /* PID */
130 
131  stabilization_att_fb_cmd[COMMAND_ROLL] =
132  stabilization_gains.p.x * att_err.phi +
133  stabilization_gains.d.x * rate_err.p +
135 
136  stabilization_att_fb_cmd[COMMAND_PITCH] =
137  stabilization_gains.p.y * att_err.theta +
138  stabilization_gains.d.y * rate_err.q +
140 
141  stabilization_att_fb_cmd[COMMAND_YAW] =
142  stabilization_gains.p.z * att_err.psi +
143  stabilization_gains.d.z * rate_err.r +
145 
146 
147  stabilization_cmd[COMMAND_ROLL] =
148  (stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL])/16.;
149  stabilization_cmd[COMMAND_PITCH] =
150  (stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH])/16.;
151  stabilization_cmd[COMMAND_YAW] =
152  (stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW])/16.;
153 
154 }
int32_t phi
in rad with INT32_ANGLE_FRAC
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:336
#define EULERS_ADD(_a, _b)
Definition: pprz_algebra.h:247
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:261
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1027
angular rates
float psi
in radians
void stabilization_attitude_set_failsafe_setpoint(void)
Read an attitude setpoint from the RC.
void stabilization_attitude_ref_enter()
int32_t theta
in rad with INT32_ANGLE_FRAC
float theta
in radians
euler angles
void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler)
float p
in rad/s^2
void stabilization_attitude_read_rc(bool_t in_flight)
void stabilization_attitude_enter(void)
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)
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
#define FLOAT_ANGLE_NORMALIZE(_a)
void stabilization_attitude_ref_init(void)
void stabilization_attitude_ref_update()
float r
in rad/s^2
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1078
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:97
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:567
#define EULERS_BOUND_CUBE(_v, _min, _max)
Definition: pprz_algebra.h:283
float q
in rad/s^2
float stabilization_att_ff_cmd[COMMANDS_NB]
float stabilization_attitude_get_heading_f(void)
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight)
struct FloatRates stab_att_ref_rate
with REF_RATE_FRAC
struct FloatRates stab_att_ref_accel
with REF_ACCEL_FRAC
euler angles