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_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 
29 #include "generated/airframe.h"
30 
32 
33 #include "state.h"
34 
35 #include "subsystems/imu.h"
38 
39 #define F_UPDATE_RES 9
40 #define REF_DOT_FRAC 11
41 #define REF_FRAC 16
42 
43 #define MAX_SUM_ERR 4000000
44 
45 #ifndef STABILIZATION_RATE_DDGAIN_P
46 #define STABILIZATION_RATE_DDGAIN_P 0
47 #endif
48 #ifndef STABILIZATION_RATE_DDGAIN_Q
49 #define STABILIZATION_RATE_DDGAIN_Q 0
50 #endif
51 #ifndef STABILIZATION_RATE_DDGAIN_R
52 #define STABILIZATION_RATE_DDGAIN_R 0
53 #endif
54 
55 #ifndef STABILIZATION_RATE_IGAIN_P
56 #define STABILIZATION_RATE_IGAIN_P 0
57 #endif
58 
59 #ifndef STABILIZATION_RATE_IGAIN_Q
60 #define STABILIZATION_RATE_IGAIN_Q 0
61 #endif
62 
63 #ifndef STABILIZATION_RATE_IGAIN_R
64 #define STABILIZATION_RATE_IGAIN_R 0
65 #endif
66 
67 #if (STABILIZATION_RATE_GAIN_P < 0) || \
68  (STABILIZATION_RATE_GAIN_Q < 0) || \
69  (STABILIZATION_RATE_GAIN_R < 0) || \
70  (STABILIZATION_RATE_IGAIN_P < 0) || \
71  (STABILIZATION_RATE_IGAIN_Q < 0) || \
72  (STABILIZATION_RATE_IGAIN_R < 0)
73 #error "ALL control gains have to be positive!!!"
74 #endif
75 
76 #ifndef STABILIZATION_RATE_REF_TAU
77 #define STABILIZATION_RATE_REF_TAU 4
78 #endif
79 
80 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
81 #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
82 
90 
93 
94 
95 #ifndef STABILIZATION_RATE_DEADBAND_P
96 #define STABILIZATION_RATE_DEADBAND_P 0
97 #endif
98 #ifndef STABILIZATION_RATE_DEADBAND_Q
99 #define STABILIZATION_RATE_DEADBAND_Q 0
100 #endif
101 #ifndef STABILIZATION_RATE_DEADBAND_R
102 #define STABILIZATION_RATE_DEADBAND_R 200
103 #endif
104 
105 #define ROLL_RATE_DEADBAND_EXCEEDED() \
106  (radio_control.values[RADIO_ROLL] > STABILIZATION_RATE_DEADBAND_P || \
107  radio_control.values[RADIO_ROLL] < -STABILIZATION_RATE_DEADBAND_P)
108 
109 #define PITCH_RATE_DEADBAND_EXCEEDED() \
110  (radio_control.values[RADIO_PITCH] > STABILIZATION_RATE_DEADBAND_Q || \
111  radio_control.values[RADIO_PITCH] < -STABILIZATION_RATE_DEADBAND_Q)
112 
113 #define YAW_RATE_DEADBAND_EXCEEDED() \
114  (radio_control.values[RADIO_YAW] > STABILIZATION_RATE_DEADBAND_R || \
115  radio_control.values[RADIO_YAW] < -STABILIZATION_RATE_DEADBAND_R)
116 
117 #if PERIODIC_TELEMETRY
119 
120 static void send_rate(void) {
121  DOWNLINK_SEND_RATE_LOOP(DefaultChannel, DefaultDevice,
140  &stabilization_cmd[COMMAND_THRUST]);
141 }
142 #endif
143 
145 
147 
149  STABILIZATION_RATE_GAIN_P,
150  STABILIZATION_RATE_GAIN_Q,
151  STABILIZATION_RATE_GAIN_R);
160 
164 
165 #if PERIODIC_TELEMETRY
166  register_periodic_telemetry(DefaultPeriodic, "RATE_LOOP", send_rate);
167 #endif
168 }
169 
170 
172 
174  stabilization_rate_sp.p = (int32_t)radio_control.values[RADIO_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ;
175  else
177 
179  stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_PITCH] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ;
180  else
182 
184  stabilization_rate_sp.r = (int32_t)radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ;
185  else
187 
188  // Setpoint at ref resolution
190 }
191 
192 //Read rc with roll and yaw sitcks switched if the default orientation is vertical but airplane sticks are desired
194 
196  stabilization_rate_sp.r = (int32_t) -radio_control.values[RADIO_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ;
197  else
199 
201  stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_PITCH] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ;
202  else
204 
206  stabilization_rate_sp.p = (int32_t)radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ;
207  else
209 
210  // Setpoint at ref resolution
212 }
213 
217 }
218 
219 void stabilization_rate_run(bool_t in_flight) {
220 
221  /* reference */
222  struct Int32Rates _r;
225  /* integrate ref */
226  const struct Int32Rates _delta_ref = {
230  RATES_ADD(stabilization_rate_ref, _delta_ref);
231 
232  /* compute feed-forward command */
234 
235 
236  /* compute feed-back command */
237  /* error for feedback */
238  const struct Int32Rates _ref_scaled = {
242  struct Int32Rates _error;
243  struct Int32Rates* body_rate = stateGetBodyRates_i();
244  RATES_DIFF(_error, _ref_scaled, (*body_rate));
245  if (in_flight) {
246  /* update integrator */
249  }
250  else {
252  }
253 
254  /* PI */
257 
260 
263 
267 
268  /* sum to final command */
272 
273  /* bound the result */
274  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
275  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
276  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
277 
278 }
#define STABILIZATION_RATE_IGAIN_R
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:361
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 with INT32_RATE_FRAC
Periodic telemetry system header (includes downlink utility and generated code).
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:326
#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
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
#define THROTTLE_STICK_DOWN()
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer
#define RADIO_PITCH
Definition: spektrum_arch.h:45
#define RATES_SDIV(_ro, _ri, _s)
Definition: pprz_algebra.h:375
#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:333
struct RadioControl radio_control
Definition: radio_control.c:25
Inertial Measurement Unit interface.
angular rates
#define RADIO_YAW
Definition: spektrum_arch.h:48
#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()
Some helper functions to check RC sticks.
#define RATES_BOUND_CUBE(_v, _min, _max)
Definition: pprz_algebra.h:390
API to get/set the generic vehicle states.
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:319
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 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:42
#define ROLL_RATE_DEADBAND_EXCEEDED()
int32_t r
in rad/s 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:382