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_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 "subsystems/ahrs.h"
25 
27 
28 #include "generated/airframe.h"
29 
31 
32 /* warn if some gains are still negative */
33 #if (STABILIZATION_ATTITUDE_PHI_PGAIN < 0) || \
34  (STABILIZATION_ATTITUDE_THETA_PGAIN < 0) || \
35  (STABILIZATION_ATTITUDE_PSI_PGAIN < 0) || \
36  (STABILIZATION_ATTITUDE_PHI_DGAIN < 0) || \
37  (STABILIZATION_ATTITUDE_THETA_DGAIN < 0) || \
38  (STABILIZATION_ATTITUDE_PSI_DGAIN < 0) || \
39  (STABILIZATION_ATTITUDE_PHI_IGAIN < 0) || \
40  (STABILIZATION_ATTITUDE_THETA_IGAIN < 0) || \
41  (STABILIZATION_ATTITUDE_PSI_IGAIN < 0)
42 #warning "ALL control gains are now positive!!!"
43 #endif
44 
46 
49 
51 
53 
54 
56  STABILIZATION_ATTITUDE_PHI_PGAIN,
57  STABILIZATION_ATTITUDE_THETA_PGAIN,
58  STABILIZATION_ATTITUDE_PSI_PGAIN);
59 
61  STABILIZATION_ATTITUDE_PHI_DGAIN,
62  STABILIZATION_ATTITUDE_THETA_DGAIN,
63  STABILIZATION_ATTITUDE_PSI_DGAIN);
64 
66  STABILIZATION_ATTITUDE_PHI_IGAIN,
67  STABILIZATION_ATTITUDE_THETA_IGAIN,
68  STABILIZATION_ATTITUDE_PSI_IGAIN);
69 
71  STABILIZATION_ATTITUDE_PHI_DDGAIN,
72  STABILIZATION_ATTITUDE_THETA_DDGAIN,
73  STABILIZATION_ATTITUDE_PSI_DDGAIN);
74 
75 
77 
78 }
79 
80 
81 void stabilization_attitude_read_rc(bool_t in_flight) {
82 
84 
85 }
86 
87 
89 
93 
94 }
95 
96 
97 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
98 #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
99 
100 #define MAX_SUM_ERR 4000000
101 
102 void stabilization_attitude_run(bool_t in_flight) {
103 
104 
105  /* update reference */
107 
108  /* compute feedforward command */
109  stabilization_att_ff_cmd[COMMAND_ROLL] =
111  stabilization_att_ff_cmd[COMMAND_PITCH] =
113  stabilization_att_ff_cmd[COMMAND_YAW] =
115 
116  /* compute feedback command */
117  /* attitude error */
118  const struct Int32Eulers att_ref_scaled = {
122  struct Int32Eulers att_err;
123  EULERS_DIFF(att_err, att_ref_scaled, ahrs.ltp_to_body_euler);
124  INT32_ANGLE_NORMALIZE(att_err.psi);
125 
126  if (in_flight) {
127  /* update integrator */
130  }
131  else {
133  }
134 
135  /* rate error */
136  const struct Int32Rates rate_ref_scaled = {
140  struct Int32Rates rate_err;
141  RATES_DIFF(rate_err, rate_ref_scaled, ahrs.body_rate);
142 
143  /* PID */
144  stabilization_att_fb_cmd[COMMAND_ROLL] =
145  stabilization_gains.p.x * att_err.phi +
146  stabilization_gains.d.x * rate_err.p +
148 
149  stabilization_att_fb_cmd[COMMAND_PITCH] =
150  stabilization_gains.p.y * att_err.theta +
151  stabilization_gains.d.y * rate_err.q +
153 
154  stabilization_att_fb_cmd[COMMAND_YAW] =
155  stabilization_gains.p.z * att_err.psi +
156  stabilization_gains.d.z * rate_err.r +
158 
159 
160  /* with P gain of 100, att_err of 180deg (3.14 rad)
161  * fb cmd: 100 * 3.14 * 2^12 / 2^CMD_SHIFT = 628
162  * max possible command is 9600
163  */
164 #define CMD_SHIFT 11
165 
166  /* sum feedforward and feedback */
167  stabilization_cmd[COMMAND_ROLL] =
169 
170  stabilization_cmd[COMMAND_PITCH] =
172 
173  stabilization_cmd[COMMAND_YAW] =
175 
176  /* bound the result */
177  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
178  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
179  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
180 
181 }
struct Int32Rates body_rate
Rotational velocity in body frame.
Definition: ahrs.h:52
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:338
void stabilization_attitude_enter(void)
static void reset_psi_ref_from_body(void)
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
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.
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
int32_t theta
in rad with INT32_ANGLE_FRAC
void stabilization_attitude_ref_init(void)
#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
float p
in rad/s^2
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
struct FloatEulers stab_att_ref_euler
with REF_ANGLE_FRAC
#define INT32_ANGLE_FRAC
void stabilization_attitude_init(void)
float phi
in radians
static void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight)
#define REF_RATE_FRAC
#define OFFSET_AND_ROUND2(_a, _b)
struct Int32Eulers stabilization_att_sum_err
#define CMD_SHIFT
#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:99
struct Int32Eulers ltp_to_body_euler
Rotation from LocalTangentPlane to body frame as Euler angles.
Definition: ahrs.h:50
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:285
void stabilization_attitude_ref_update(void)
float q
in rad/s^2
#define MAX_PPRZ
Definition: paparazzi.h:8
int32_t r
in rad/s^2 with INT32_RATE_FRAC
struct FloatRates stab_att_ref_rate
with REF_RATE_FRAC
struct FloatRates stab_att_ref_accel
with REF_ACCEL_FRAC
euler angles