Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 
33 
34 #include "state.h"
35 
36 #include "subsystems/imu.h"
39 
40 #define MAX_SUM_ERR 40000
41 
42 #ifndef STABILIZATION_RATE_IGAIN_P
43 #define STABILIZATION_RATE_IGAIN_P 0
44 #endif
45 
46 #ifndef STABILIZATION_RATE_IGAIN_Q
47 #define STABILIZATION_RATE_IGAIN_Q 0
48 #endif
49 
50 #ifndef STABILIZATION_RATE_IGAIN_R
51 #define STABILIZATION_RATE_IGAIN_R 0
52 #endif
53 
54 #if (STABILIZATION_RATE_GAIN_P < 0) || \
55  (STABILIZATION_RATE_GAIN_Q < 0) || \
56  (STABILIZATION_RATE_GAIN_R < 0) || \
57  (STABILIZATION_RATE_IGAIN_P < 0) || \
58  (STABILIZATION_RATE_IGAIN_Q < 0) || \
59  (STABILIZATION_RATE_IGAIN_R < 0)
60 #error "ALL control gains have to be positive!!!"
61 #endif
62 
63 // divide by 2^_b and round instead of floor
64 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
65 #define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
66 
71 
73 
74 #ifndef STABILIZATION_RATE_DEADBAND_P
75 #define STABILIZATION_RATE_DEADBAND_P 0
76 #endif
77 #ifndef STABILIZATION_RATE_DEADBAND_Q
78 #define STABILIZATION_RATE_DEADBAND_Q 0
79 #endif
80 #ifndef STABILIZATION_RATE_DEADBAND_R
81 #define STABILIZATION_RATE_DEADBAND_R 200
82 #endif
83 
84 #define ROLL_RATE_DEADBAND_EXCEEDED() \
85  (radio_control.values[RADIO_ROLL] > STABILIZATION_RATE_DEADBAND_P || \
86  radio_control.values[RADIO_ROLL] < -STABILIZATION_RATE_DEADBAND_P)
87 
88 #define PITCH_RATE_DEADBAND_EXCEEDED() \
89  (radio_control.values[RADIO_PITCH] > STABILIZATION_RATE_DEADBAND_Q || \
90  radio_control.values[RADIO_PITCH] < -STABILIZATION_RATE_DEADBAND_Q)
91 
92 #define YAW_RATE_DEADBAND_EXCEEDED() \
93  (radio_control.values[RADIO_YAW] > STABILIZATION_RATE_DEADBAND_R || \
94  radio_control.values[RADIO_YAW] < -STABILIZATION_RATE_DEADBAND_R)
95 
96 #if PERIODIC_TELEMETRY
98 
99 static void send_rate(struct transport_tx *trans, struct link_device *dev)
100 {
101  pprz_msg_send_RATE_LOOP(trans, dev, AC_ID,
111  &stabilization_cmd[COMMAND_THRUST]);
112 }
113 #endif
114 
116 {
117 
119 
121  STABILIZATION_RATE_GAIN_P,
122  STABILIZATION_RATE_GAIN_Q,
123  STABILIZATION_RATE_GAIN_R);
128 
130 
131 #if PERIODIC_TELEMETRY
132  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RATE_LOOP, send_rate);
133 #endif
134 }
135 
136 
138 {
139 
141  stabilization_rate_sp.p = radio_control.values[RADIO_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ;
142  } else {
144  }
145 
147  stabilization_rate_sp.q = radio_control.values[RADIO_PITCH] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ;
148  } else {
150  }
151 
153  stabilization_rate_sp.r = radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ;
154  } else {
156  }
157 }
158 
159 //Read rc with roll and yaw sitcks switched if the default orientation is vertical but airplane sticks are desired
161 {
162 
164  stabilization_rate_sp.r = - radio_control.values[RADIO_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ;
165  } else {
167  }
168 
170  stabilization_rate_sp.q = radio_control.values[RADIO_PITCH] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ;
171  } else {
173  }
174 
176  stabilization_rate_sp.p = radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ;
177  } else {
179  }
180 }
181 
183 {
185 }
186 
187 void stabilization_rate_run(bool in_flight)
188 {
189  /* compute feed-back command */
190  struct FloatRates _error;
191  struct FloatRates *body_rate = stateGetBodyRates_f();
192  RATES_DIFF(_error, stabilization_rate_sp, (*body_rate));
193  if (in_flight) {
194  /* update integrator */
195  //divide the sum_err_increment to make sure it doesn't accumulate to the max too fast
196  struct FloatRates sum_err_increment;
197  RATES_SDIV(sum_err_increment, _error, PERIODIC_FREQUENCY);
198  RATES_ADD(stabilization_rate_sum_err, sum_err_increment);
200  } else {
202  }
203 
204  /* PI */
207 
210 
213 
217 
218  /* bound the result */
219  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
220  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
221  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
222 
223 }
radio_control.h
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
RADIO_ROLL
#define RADIO_ROLL
Definition: intermcu_ap.h:41
stabilization_rate_read_rc
void stabilization_rate_read_rc(void)
Definition: stabilization_rate.c:137
FLOAT_RATES_ZERO
#define FLOAT_RATES_ZERO(_r)
Definition: pprz_algebra_float.h:191
STABILIZATION_RATE_IGAIN_P
#define STABILIZATION_RATE_IGAIN_P
Definition: stabilization_rate.c:43
stabilization_rate_sum_err
struct FloatRates stabilization_rate_sum_err
Definition: stabilization_rate.c:70
stateGetBodyRates_f
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
stabilization_rate_fb_cmd
struct FloatRates stabilization_rate_fb_cmd
Definition: stabilization_rate.c:72
YAW_RATE_DEADBAND_EXCEEDED
#define YAW_RATE_DEADBAND_EXCEEDED()
Definition: stabilization_rate.c:92
THROTTLE_STICK_DOWN
#define THROTTLE_STICK_DOWN()
Definition: autopilot_rc_helpers.h:40
ROLL_RATE_DEADBAND_EXCEEDED
#define ROLL_RATE_DEADBAND_EXCEEDED()
Definition: stabilization_rate.c:84
STABILIZATION_RATE_IGAIN_Q
#define STABILIZATION_RATE_IGAIN_Q
Definition: stabilization_rate.c:47
telemetry.h
imu.h
RATES_ASSIGN
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:330
autopilot_rc_helpers.h
PITCH_RATE_DEADBAND_EXCEEDED
#define PITCH_RATE_DEADBAND_EXCEEDED()
Definition: stabilization_rate.c:88
RATES_DIFF
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
stabilization_rate_gain
struct FloatRates stabilization_rate_gain
Definition: stabilization_rate.c:68
RATES_ADD
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:344
stabilization_rate_enter
void stabilization_rate_enter(void)
Definition: stabilization_rate.c:182
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
stabilization_rate.h
stabilization_rate_init
void stabilization_rate_init(void)
Definition: stabilization_rate.c:115
RATES_SDIV
#define RATES_SDIV(_ro, _ri, _s)
Definition: pprz_algebra.h:386
stabilization_rate_sp
struct FloatRates stabilization_rate_sp
Definition: stabilization_rate.c:67
send_rate
static void send_rate(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_rate.c:99
MAX_SUM_ERR
#define MAX_SUM_ERR
Definition: stabilization_rate.c:40
stabilization_cmd
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
RATES_BOUND_CUBE
#define RATES_BOUND_CUBE(_v, _min, _max)
Definition: pprz_algebra.h:401
stabilization_rate_run
void stabilization_rate_run(bool in_flight)
Definition: stabilization_rate.c:187
stabilization_rate_read_rc_switched_sticks
void stabilization_rate_read_rc_switched_sticks(void)
Definition: stabilization_rate.c:160
stabilization_rate_igain
struct FloatRates stabilization_rate_igain
Definition: stabilization_rate.c:69
RADIO_YAW
#define RADIO_YAW
Definition: intermcu_ap.h:43
stabilization.h
state.h
RADIO_PITCH
#define RADIO_PITCH
Definition: intermcu_ap.h:42
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
radio_control
struct RadioControl radio_control
Definition: radio_control.c:30
STABILIZATION_RATE_IGAIN_R
#define STABILIZATION_RATE_IGAIN_R
Definition: stabilization_rate.c:51
RadioControl::values
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69