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_rate.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3  * Copyright (C) 2010 Felix Ruess <felix.ruess@gmail.com>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
30 
31 #include "state.h"
32 
33 #include "subsystems/imu.h"
35 #include "generated/airframe.h"
36 
37 #define F_UPDATE_RES 9
38 #define REF_DOT_FRAC 11
39 #define REF_FRAC 16
40 
41 #define MAX_SUM_ERR 4000000
42 
43 #if (STABILIZATION_RATE_GAIN_P < 0) || \
44  (STABILIZATION_RATE_GAIN_Q < 0) || \
45  (STABILIZATION_RATE_GAIN_R < 0)
46 #warning "ALL control gains are now positive!!!"
47 #endif
48 
49 #ifndef STABILIZATION_RATE_DDGAIN_P
50 #define STABILIZATION_RATE_DDGAIN_P 0
51 #endif
52 #ifndef STABILIZATION_RATE_DDGAIN_Q
53 #define STABILIZATION_RATE_DDGAIN_Q 0
54 #endif
55 #ifndef STABILIZATION_RATE_DDGAIN_R
56 #define STABILIZATION_RATE_DDGAIN_R 0
57 #endif
58 
59 #ifndef STABILIZATION_RATE_IGAIN_P
60 #define STABILIZATION_RATE_IGAIN_P 0
61 #else
62 #if (STABILIZATION_RATE_IGAIN_P < 0)
63 #warning "ALL control gains are now positive!!!"
64 #endif
65 #endif
66 #ifndef STABILIZATION_RATE_IGAIN_Q
67 #define STABILIZATION_RATE_IGAIN_Q 0
68 #else
69 #if (STABILIZATION_RATE_IGAIN_Q < 0)
70 #warning "ALL control gains are now positive!!!"
71 #endif
72 #endif
73 #ifndef STABILIZATION_RATE_IGAIN_R
74 #define STABILIZATION_RATE_IGAIN_R 0
75 #else
76 #if (STABILIZATION_RATE_IGAIN_R < 0)
77 #warning "ALL control gains are now positive!!!"
78 #endif
79 #endif
80 
81 #ifndef STABILIZATION_RATE_REF_TAU
82 #define STABILIZATION_RATE_REF_TAU 4
83 #endif
84 
85 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
86 #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
87 
95 
98 
99 
100 #ifndef STABILIZATION_RATE_DEADBAND_P
101 #define STABILIZATION_RATE_DEADBAND_P 0
102 #endif
103 #ifndef STABILIZATION_RATE_DEADBAND_Q
104 #define STABILIZATION_RATE_DEADBAND_Q 0
105 #endif
106 #ifndef STABILIZATION_RATE_DEADBAND_R
107 #define STABILIZATION_RATE_DEADBAND_R 200
108 #endif
109 
110 #define ROLL_RATE_DEADBAND_EXCEEDED() \
111  (radio_control.values[RADIO_ROLL] > STABILIZATION_RATE_DEADBAND_P || \
112  radio_control.values[RADIO_ROLL] < -STABILIZATION_RATE_DEADBAND_P)
113 
114 #define PITCH_RATE_DEADBAND_EXCEEDED() \
115  (radio_control.values[RADIO_PITCH] > STABILIZATION_RATE_DEADBAND_Q || \
116  radio_control.values[RADIO_PITCH] < -STABILIZATION_RATE_DEADBAND_Q)
117 
118 #define YAW_RATE_DEADBAND_EXCEEDED() \
119  (radio_control.values[RADIO_YAW] > STABILIZATION_RATE_DEADBAND_R || \
120  radio_control.values[RADIO_YAW] < -STABILIZATION_RATE_DEADBAND_R)
121 
122 
124 
126 
128  STABILIZATION_RATE_GAIN_P,
129  STABILIZATION_RATE_GAIN_Q,
130  STABILIZATION_RATE_GAIN_R);
139 
143 }
144 
145 
147 
149  stabilization_rate_sp.p = (int32_t)radio_control.values[RADIO_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ;
150  else
152 
154  stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_PITCH] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ;
155  else
157 
159  stabilization_rate_sp.r = (int32_t)radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ;
160  else
162 
163  // Setpoint at ref resolution
165 }
166 
167 //Read rc with roll and yaw sitcks switched if the default orientation is vertical but airplane sticks are desired
169 
171  stabilization_rate_sp.r = (int32_t) -radio_control.values[RADIO_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ;
172  else
174 
176  stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_PITCH] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ;
177  else
179 
181  stabilization_rate_sp.p = (int32_t)radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ;
182  else
184 
185  // Setpoint at ref resolution
187 }
188 
192 }
193 
194 void stabilization_rate_run(bool_t in_flight) {
195 
196  /* reference */
197  struct Int32Rates _r;
200  /* integrate ref */
201  const struct Int32Rates _delta_ref = {
205  RATES_ADD(stabilization_rate_ref, _delta_ref);
206 
207  /* compute feed-forward command */
209 
210 
211  /* compute feed-back command */
212  /* error for feedback */
213  const struct Int32Rates _ref_scaled = {
217  struct Int32Rates _error;
218  struct Int32Rates* body_rate = stateGetBodyRates_i();
219  RATES_DIFF(_error, _ref_scaled, (*body_rate));
220  if (in_flight) {
221  /* update integrator */
224  }
225  else {
227  }
228 
229  /* PI */
232 
235 
238 
242 
243  /* sum to final command */
247 
248  /* bound the result */
249  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
250  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
251  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
252 
253 }
#define STABILIZATION_RATE_IGAIN_R
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:336
struct Int32Rates stabilization_rate_ff_cmd
#define OFFSET_AND_ROUND(_a, _b)
void stabilization_rate_run(bool_t in_flight)
#define MAX_SUM_ERR
int32_t p
in rad/s^2 with INT32_RATE_FRAC
struct Int32Rates stabilization_rate_sum_err
void stabilization_rate_enter(void)
struct Int32Rates stabilization_rate_gain
#define REF_FRAC
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:301
#define OFFSET_AND_ROUND2(_a, _b)
#define YAW_RATE_DEADBAND_EXCEEDED()
#define INT_RATES_ZERO(_e)
#define INT32_RATE_FRAC
struct Int32Rates stabilization_rate_igain
#define RADIO_PITCH
Definition: spektrum_arch.h:40
#define RATES_SDIV(_ro, _ri, _s)
Definition: pprz_algebra.h:350
#define STABILIZATION_RATE_DDGAIN_Q
struct Int32Rates stabilization_rate_ddgain
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
Definition: state.h:1071
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:55
#define STABILIZATION_RATE_IGAIN_P
struct Int32Rates stabilization_rate_refdot
void stabilization_rate_read_rc(void)
void stabilization_rate_read_rc_switched_sticks(void)
#define STABILIZATION_RATE_DDGAIN_P
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:308
struct RadioControl radio_control
Definition: radio_control.c:25
Inertial Measurement Unit interface.
angular rates
#define RADIO_YAW
Definition: spektrum_arch.h:41
#define STABILIZATION_RATE_IGAIN_Q
#define F_UPDATE_RES
signed long int32_t
Definition: types.h:19
#define STABILIZATION_RATE_DDGAIN_R
#define PITCH_RATE_DEADBAND_EXCEEDED()
#define RATES_BOUND_CUBE(_v, _min, _max)
Definition: pprz_algebra.h:365
API to get/set the generic vehicle states.
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:294
General stabilization interface for rotorcrafts.
#define REF_DOT_FRAC
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:28
int32_t q
in rad/s^2 with INT32_RATE_FRAC
struct Int32Rates stabilization_rate_fb_cmd
struct Int32Rates stabilization_rate_ref
#define MAX_PPRZ
Definition: paparazzi.h:8
void stabilization_rate_init(void)
#define RADIO_ROLL
Definition: spektrum_arch.h:39
#define ROLL_RATE_DEADBAND_EXCEEDED()
int32_t r
in rad/s^2 with INT32_RATE_FRAC
struct Int32Rates stabilization_rate_sp
#define STABILIZATION_RATE_REF_TAU
#define INT_RATES_LSHIFT(_o, _i, _r)
#define RATES_EWMULT_RSHIFT(c, a, b, _s)
Definition: pprz_algebra.h:357