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_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 
23 #include "state.h"
25 
27 #include "paparazzi.h"
28 
29 #include "generated/airframe.h"
30 
32 
33 /* warn if some gains are still negative */
34 #if (STABILIZATION_ATTITUDE_PHI_PGAIN < 0) || \
35  (STABILIZATION_ATTITUDE_THETA_PGAIN < 0) || \
36  (STABILIZATION_ATTITUDE_PSI_PGAIN < 0) || \
37  (STABILIZATION_ATTITUDE_PHI_DGAIN < 0) || \
38  (STABILIZATION_ATTITUDE_THETA_DGAIN < 0) || \
39  (STABILIZATION_ATTITUDE_PSI_DGAIN < 0) || \
40  (STABILIZATION_ATTITUDE_PHI_IGAIN < 0) || \
41  (STABILIZATION_ATTITUDE_THETA_IGAIN < 0) || \
42  (STABILIZATION_ATTITUDE_PSI_IGAIN < 0)
43 #warning "ALL control gains are now positive!!!"
44 #endif
45 
47 
50 
51 static inline void reset_psi_ref_from_body(void) {
52  //sp has been set from body using stabilization_attitude_get_yaw_i, use that value
54  stab_att_ref_rate.r = 0;
56 }
57 
59 
61 
62 
64  STABILIZATION_ATTITUDE_PHI_PGAIN,
65  STABILIZATION_ATTITUDE_THETA_PGAIN,
66  STABILIZATION_ATTITUDE_PSI_PGAIN);
67 
69  STABILIZATION_ATTITUDE_PHI_DGAIN,
70  STABILIZATION_ATTITUDE_THETA_DGAIN,
71  STABILIZATION_ATTITUDE_PSI_DGAIN);
72 
74  STABILIZATION_ATTITUDE_PHI_IGAIN,
75  STABILIZATION_ATTITUDE_THETA_IGAIN,
76  STABILIZATION_ATTITUDE_PSI_IGAIN);
77 
79  STABILIZATION_ATTITUDE_PHI_DDGAIN,
80  STABILIZATION_ATTITUDE_THETA_DDGAIN,
81  STABILIZATION_ATTITUDE_PSI_DDGAIN);
82 
83 
85 
86 }
87 
88 
89 void stabilization_attitude_read_rc(bool_t in_flight) {
91 }
92 
97 }
98 
103 }
104 
106  memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers));
107 }
108 
109 
110 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
111 #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
112 
113 #define MAX_SUM_ERR 4000000
114 
115 void stabilization_attitude_run(bool_t in_flight) {
116 
117  /* update reference */
119 
120  /* compute feedforward command */
121  stabilization_att_ff_cmd[COMMAND_ROLL] =
123  stabilization_att_ff_cmd[COMMAND_PITCH] =
125  stabilization_att_ff_cmd[COMMAND_YAW] =
127 
128  /* compute feedback command */
129  /* attitude error */
130  const struct Int32Eulers att_ref_scaled = {
134  struct Int32Eulers att_err;
135  struct Int32Eulers* ltp_to_body_euler = stateGetNedToBodyEulers_i();
136  EULERS_DIFF(att_err, att_ref_scaled, (*ltp_to_body_euler));
137  INT32_ANGLE_NORMALIZE(att_err.psi);
138 
139  if (in_flight) {
140  /* update integrator */
143  }
144  else {
146  }
147 
148  /* rate error */
149  const struct Int32Rates rate_ref_scaled = {
153  struct Int32Rates rate_err;
154  struct Int32Rates* body_rate = stateGetBodyRates_i();
155  RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate));
156 
157  /* PID */
158  stabilization_att_fb_cmd[COMMAND_ROLL] =
159  stabilization_gains.p.x * att_err.phi +
160  stabilization_gains.d.x * rate_err.p +
162 
163  stabilization_att_fb_cmd[COMMAND_PITCH] =
164  stabilization_gains.p.y * att_err.theta +
165  stabilization_gains.d.y * rate_err.q +
167 
168  stabilization_att_fb_cmd[COMMAND_YAW] =
169  stabilization_gains.p.z * att_err.psi +
170  stabilization_gains.d.z * rate_err.r +
172 
173 
174  /* with P gain of 100, att_err of 180deg (3.14 rad)
175  * fb cmd: 100 * 3.14 * 2^12 / 2^CMD_SHIFT = 628
176  * max possible command is 9600
177  */
178 #define CMD_SHIFT 11
179 
180  /* sum feedforward and feedback */
181  stabilization_cmd[COMMAND_ROLL] =
183 
184  stabilization_cmd[COMMAND_PITCH] =
186 
187  stabilization_cmd[COMMAND_YAW] =
189 
190  /* bound the result */
191  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
192  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
193  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
194 
195 }
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t stabilization_att_fb_cmd[COMMANDS_NB]
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:336
void stabilization_attitude_enter(void)
#define EULERS_ADD(_a, _b)
Definition: pprz_algebra.h:247
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:261
int32_t p
in rad/s^2 with INT32_RATE_FRAC
float psi
in radians
#define OFFSET_AND_ROUND(_a, _b)
Read an attitude setpoint from the RC.
int32_t theta
in rad with INT32_ANGLE_FRAC
#define INT_EULERS_ZERO(_e)
#define INT32_RATE_FRAC
struct Int32AttitudeGains stabilization_gains
void stabilization_attitude_read_rc(bool_t in_flight)
float theta
in radians
void stabilization_attitude_set_failsafe_setpoint(void)
float p
in rad/s^2
static void reset_psi_ref_from_body(void)
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
Definition: state.h:1071
struct FloatEulers stab_att_ref_euler
with REF_ANGLE_FRAC
#define INT32_ANGLE_FRAC
void stabilization_attitude_init(void)
float phi
in radians
#define REF_RATE_FRAC
#define OFFSET_AND_ROUND2(_a, _b)
struct Int32Eulers stabilization_att_sum_err
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight)
Read attitude setpoint from RC as euler angles.
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
#define CMD_SHIFT
void stabilization_attitude_ref_init(void)
void stabilization_attitude_ref_update()
#define REF_ANGLE_FRAC
#define INT32_ANGLE_NORMALIZE(_a)
angular rates
float r
in rad/s^2
int32_t stabilization_att_ff_cmd[COMMANDS_NB]
signed long int32_t
Definition: types.h:19
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:97
API to get/set the generic vehicle states.
void stabilization_attitude_run(bool_t in_flight)
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
int32_t q
in rad/s^2 with INT32_RATE_FRAC
#define EULERS_BOUND_CUBE(_v, _min, _max)
Definition: pprz_algebra.h:283
float q
in rad/s^2
#define MAX_PPRZ
Definition: paparazzi.h:8
int32_t r
in rad/s^2 with INT32_RATE_FRAC
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1012
struct FloatRates stab_att_ref_rate
with REF_RATE_FRAC
void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler)
struct FloatRates stab_att_ref_accel
with REF_ACCEL_FRAC
euler angles