Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
stabilization_attitude_euler_float.c
Go to the documentation of this file.
1 /*
2  * $Id: stabilization_attitude_euler.c 3795 2009-07-24 23:43:02Z poine $
3  *
4  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  */
23 
25 
27 #include "subsystems/ahrs.h"
29 
30 #include "generated/airframe.h"
31 
32 
34 
35 /* warn if some gains are still negative */
36 #if (STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN < 0) || \
37  (STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN < 0) || \
38  (STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN < 0) || \
39  (STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN < 0) || \
40  (STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN < 0) || \
41  (STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN < 0) || \
42  (STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN < 0) || \
43  (STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN < 0) || \
44  (STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN < 0)
45 #warning "ALL control gains are now positive!!!"
46 #endif
47 
49 
50 float stabilization_att_fb_cmd[COMMANDS_NB];
51 float stabilization_att_ff_cmd[COMMANDS_NB];
52 
53 
55 
57 
59  STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN,
60  STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN,
61  STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN);
62 
64  STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN,
65  STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN,
66  STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN);
67 
69  STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN,
70  STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN,
71  STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN);
72 
74  STABILIZATION_ATTITUDE_FLOAT_PHI_DDGAIN,
75  STABILIZATION_ATTITUDE_FLOAT_THETA_DDGAIN,
76  STABILIZATION_ATTITUDE_FLOAT_PSI_DDGAIN);
77 
79 
80 }
81 
82 
83 void stabilization_attitude_read_rc(bool_t in_flight) {
84 
86 
87 }
88 
89 
91 
92  STABILIZATION_ATTITUDE_FLOAT_RESET_PSI_REF( stab_att_sp_euler );
94 
95 }
96 
97 
98 #define MAX_SUM_ERR RadOfDeg(56000)
99 
100 void stabilization_attitude_run(bool_t in_flight) {
101 
103 
104  /* Compute feedforward */
105  stabilization_att_ff_cmd[COMMAND_ROLL] =
107  stabilization_att_ff_cmd[COMMAND_PITCH] =
109  stabilization_att_ff_cmd[COMMAND_YAW] =
111 
112  /* Compute feedback */
113  /* attitude error */
114  struct FloatEulers att_float;
116  struct FloatEulers att_err;
117  EULERS_DIFF(att_err, stab_att_ref_euler, att_float);
118  FLOAT_ANGLE_NORMALIZE(att_err.psi);
119 
120  if (in_flight) {
121  /* update integrator */
124  }
125  else {
127  }
128 
129  /* rate error */
130  struct FloatRates rate_float;
131  RATES_FLOAT_OF_BFP(rate_float, ahrs.body_rate);
132  struct FloatRates rate_err;
133  RATES_DIFF(rate_err, stab_att_ref_rate, rate_float);
134 
135  /* PID */
136 
137  stabilization_att_fb_cmd[COMMAND_ROLL] =
138  stabilization_gains.p.x * att_err.phi +
139  stabilization_gains.d.x * rate_err.p +
141 
142  stabilization_att_fb_cmd[COMMAND_PITCH] =
143  stabilization_gains.p.y * att_err.theta +
144  stabilization_gains.d.y * rate_err.q +
146 
147  stabilization_att_fb_cmd[COMMAND_YAW] =
148  stabilization_gains.p.z * att_err.psi +
149  stabilization_gains.d.z * rate_err.r +
151 
152 
153  stabilization_cmd[COMMAND_ROLL] =
154  (stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL])/16.;
155  stabilization_cmd[COMMAND_PITCH] =
156  (stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH])/16.;
157  stabilization_cmd[COMMAND_YAW] =
158  (stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW])/16.;
159 
160 }
struct Int32Rates body_rate
Rotational velocity in body frame.
Definition: ahrs.h:52
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:338
Attitude and Heading Reference System interface.
#define EULERS_ADD(_a, _b)
Definition: pprz_algebra.h:249
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:263
angular rates
float psi
in radians
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
void stabilization_attitude_ref_init(void)
float theta
in radians
euler angles
#define STABILIZATION_ATTITUDE_FLOAT_READ_RC(_sp, _inflight)
float p
in rad/s^2
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
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)
#define FLOAT_ANGLE_NORMALIZE(_a)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:619
float r
in rad/s^2
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:99
void stabilization_attitude_run(bool_t in_flight)
#define FLOAT_EULERS_ZERO(_e)
struct Int32Eulers ltp_to_body_euler
Rotation from LocalTangentPlane to body frame as Euler angles.
Definition: ahrs.h:50
struct FloatEulers stabilization_att_sum_err
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:569
#define EULERS_BOUND_CUBE(_v, _min, _max)
Definition: pprz_algebra.h:285
void stabilization_attitude_ref_update(void)
float q
in rad/s^2
float stabilization_att_ff_cmd[COMMANDS_NB]
struct FloatRates stab_att_ref_rate
with REF_RATE_FRAC
struct FloatRates stab_att_ref_accel
with REF_ACCEL_FRAC