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_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 
28 #include "generated/airframe.h"
29 
32 
33 #include "std.h"
34 #include "paparazzi.h"
35 #include "math/pprz_algebra_int.h"
36 #include "state.h"
37 
39 
40 /* warn if some gains are still negative */
41 #if (STABILIZATION_ATTITUDE_PHI_PGAIN < 0) || \
42  (STABILIZATION_ATTITUDE_THETA_PGAIN < 0) || \
43  (STABILIZATION_ATTITUDE_PSI_PGAIN < 0) || \
44  (STABILIZATION_ATTITUDE_PHI_DGAIN < 0) || \
45  (STABILIZATION_ATTITUDE_THETA_DGAIN < 0) || \
46  (STABILIZATION_ATTITUDE_PSI_DGAIN < 0) || \
47  (STABILIZATION_ATTITUDE_PHI_IGAIN < 0) || \
48  (STABILIZATION_ATTITUDE_THETA_IGAIN < 0) || \
49  (STABILIZATION_ATTITUDE_PSI_IGAIN < 0)
50 #error "ALL control gains have to be positive!!!"
51 #endif
52 
54 
57 
58 static inline void reset_psi_ref_from_body(void) {
59  //sp has been set from body using stabilization_attitude_get_yaw_i, use that value
61  stab_att_ref_rate.r = 0;
63 }
64 
65 #if PERIODIC_TELEMETRY
67 
68 static void send_att(void) {
69  struct Int32Rates* body_rate = stateGetBodyRates_i();
70  struct Int32Eulers* att = stateGetNedToBodyEulers_i();
71  DOWNLINK_SEND_STAB_ATTITUDE_INT(DefaultChannel, DefaultDevice,
72  &(body_rate->p), &(body_rate->q), &(body_rate->r),
73  &(att->phi), &(att->theta), &(att->psi),
80  &stabilization_att_fb_cmd[COMMAND_ROLL],
81  &stabilization_att_fb_cmd[COMMAND_PITCH],
82  &stabilization_att_fb_cmd[COMMAND_YAW],
83  &stabilization_att_ff_cmd[COMMAND_ROLL],
84  &stabilization_att_ff_cmd[COMMAND_PITCH],
85  &stabilization_att_ff_cmd[COMMAND_YAW],
86  &stabilization_cmd[COMMAND_ROLL],
87  &stabilization_cmd[COMMAND_PITCH],
88  &stabilization_cmd[COMMAND_YAW]);
89 }
90 
91 static void send_att_ref(void) {
92  DOWNLINK_SEND_STAB_ATTITUDE_REF_INT(DefaultChannel, DefaultDevice,
105 }
106 #endif
107 
109 
111 
112 
114  STABILIZATION_ATTITUDE_PHI_PGAIN,
115  STABILIZATION_ATTITUDE_THETA_PGAIN,
116  STABILIZATION_ATTITUDE_PSI_PGAIN);
117 
119  STABILIZATION_ATTITUDE_PHI_DGAIN,
120  STABILIZATION_ATTITUDE_THETA_DGAIN,
121  STABILIZATION_ATTITUDE_PSI_DGAIN);
122 
124  STABILIZATION_ATTITUDE_PHI_IGAIN,
125  STABILIZATION_ATTITUDE_THETA_IGAIN,
126  STABILIZATION_ATTITUDE_PSI_IGAIN);
127 
129  STABILIZATION_ATTITUDE_PHI_DDGAIN,
130  STABILIZATION_ATTITUDE_THETA_DDGAIN,
131  STABILIZATION_ATTITUDE_PSI_DDGAIN);
132 
133 
135 
136 #if PERIODIC_TELEMETRY
137  register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att);
138  register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref);
139 #endif
140 }
141 
142 void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
143  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
144 }
145 
150 }
151 
156 }
157 
159  memcpy(&stab_att_sp_euler, rpy, sizeof(struct Int32Eulers));
160 }
161 
163  /* Rotate horizontal commands to body frame by psi */
165  int32_t s_psi, c_psi;
166  PPRZ_ITRIG_SIN(s_psi, psi);
167  PPRZ_ITRIG_COS(c_psi, psi);
168  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
169  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
171 }
172 
173 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
174 #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
175 
176 #define MAX_SUM_ERR 4000000
177 
178 void stabilization_attitude_run(bool_t in_flight) {
179 
180  /* update reference */
182 
183  /* compute feedforward command */
184  stabilization_att_ff_cmd[COMMAND_ROLL] =
186  stabilization_att_ff_cmd[COMMAND_PITCH] =
188  stabilization_att_ff_cmd[COMMAND_YAW] =
190 
191  /* compute feedback command */
192  /* attitude error */
193  const struct Int32Eulers att_ref_scaled = {
197  struct Int32Eulers att_err;
198  struct Int32Eulers* ltp_to_body_euler = stateGetNedToBodyEulers_i();
199  EULERS_DIFF(att_err, att_ref_scaled, (*ltp_to_body_euler));
200  INT32_ANGLE_NORMALIZE(att_err.psi);
201 
202  if (in_flight) {
203  /* update integrator */
206  }
207  else {
209  }
210 
211  /* rate error */
212  const struct Int32Rates rate_ref_scaled = {
216  struct Int32Rates rate_err;
217  struct Int32Rates* body_rate = stateGetBodyRates_i();
218  RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate));
219 
220  /* PID */
221  stabilization_att_fb_cmd[COMMAND_ROLL] =
222  stabilization_gains.p.x * att_err.phi +
223  stabilization_gains.d.x * rate_err.p +
225 
226  stabilization_att_fb_cmd[COMMAND_PITCH] =
227  stabilization_gains.p.y * att_err.theta +
228  stabilization_gains.d.y * rate_err.q +
230 
231  stabilization_att_fb_cmd[COMMAND_YAW] =
232  stabilization_gains.p.z * att_err.psi +
233  stabilization_gains.d.z * rate_err.r +
235 
236 
237  /* with P gain of 100, att_err of 180deg (3.14 rad)
238  * fb cmd: 100 * 3.14 * 2^12 / 2^CMD_SHIFT = 628
239  * max possible command is 9600
240  */
241 #define CMD_SHIFT 11
242 
243  /* sum feedforward and feedback */
244  stabilization_cmd[COMMAND_ROLL] =
246 
247  stabilization_cmd[COMMAND_PITCH] =
249 
250  stabilization_cmd[COMMAND_YAW] =
252 
253  /* bound the result */
254  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
255  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
256  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
257 
258 }
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:361
void stabilization_attitude_enter(void)
#define EULERS_ADD(_a, _b)
Definition: pprz_algebra.h:270
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:284
int32_t p
in rad/s with INT32_RATE_FRAC
Periodic telemetry system header (includes downlink utility and generated code).
float psi
in radians
#define OFFSET_AND_ROUND(_a, _b)
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
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
float theta
in radians
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
Read attitude setpoint from RC as euler angles.
#define INT32_TRIG_FRAC
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
void stabilization_attitude_set_failsafe_setpoint(void)
float p
in rad/s
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:40
static void reset_psi_ref_from_body(void)
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
Definition: state.h:1071
float heading
Definition: ahrs_infrared.c:40
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
#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
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:107
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
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 with INT32_RATE_FRAC
#define EULERS_BOUND_CUBE(_v, _min, _max)
Definition: pprz_algebra.h:306
float q
in rad/s
#define MAX_PPRZ
Definition: paparazzi.h:8
int32_t r
in rad/s 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
struct FloatRates stab_att_ref_accel
with REF_ACCEL_FRAC
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:50
Paparazzi fixed point algebra.
euler angles