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
guidance_v.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 
27 #define GUIDANCE_V_C
29 
30 
33 // #include "booz_fms.h" FIXME
35 
36 #include "state.h"
37 
38 #include "math/pprz_algebra_int.h"
39 
40 #include "generated/airframe.h"
41 
42 
43 /* warn if some gains are still negative */
44 #if (GUIDANCE_V_HOVER_KP < 0) || \
45  (GUIDANCE_V_HOVER_KD < 0) || \
46  (GUIDANCE_V_HOVER_KI < 0)
47 #warning "ALL control gains are now positive!!!"
48 #endif
49 
50 #if defined GUIDANCE_V_INV_M
51 #warning "GUIDANCE_V_INV_M has been removed. If you don't want to use adaptive hover, please define GUIDANCE_V_NOMINAL_HOVER_THROTTLE"
52 #endif
53 
58 
60 
61 
66 
72 
78 
82 
84 
85 
86 #define GuidanceVSetRef(_pos, _speed, _accel) { \
87  gv_set_ref(_pos, _speed, _accel); \
88  guidance_v_z_ref = _pos; \
89  guidance_v_zd_ref = _speed; \
90  guidance_v_zdd_ref = _accel; \
91  }
92 
93 
94 __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flight);
95 
96 
97 void guidance_v_init(void) {
98 
100 
101  guidance_v_kp = GUIDANCE_V_HOVER_KP;
102  guidance_v_kd = GUIDANCE_V_HOVER_KD;
103  guidance_v_ki = GUIDANCE_V_HOVER_KI;
104 
106 
107 #ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
108  guidance_v_nominal_throttle = GUIDANCE_V_NOMINAL_HOVER_THROTTLE;
109 #endif
110 
111  gv_adapt_init();
112 }
113 
114 
115 void guidance_v_read_rc(void) {
116 
117  /* used in RC_DIRECT directly and as saturation in CLIMB and HOVER */
119 
120  /* used in RC_CLIMB */
121  guidance_v_rc_zd_sp = ((MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]) * GUIDANCE_V_RC_CLIMB_COEF;
122  DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_RC_CLIMB_DEAD_BAND);
123 
124 }
125 
127 
128  if (new_mode == guidance_v_mode)
129  return;
130 
131  switch (new_mode) {
133  guidance_v_z_sp = stateGetPositionNed_i()->z; // set current altitude as setpoint
136  break;
137 
140  guidance_v_zd_sp = 0;
141  case GUIDANCE_V_MODE_NAV:
144  break;
145 
146  default:
147  break;
148 
149  }
150 
151  guidance_v_mode = new_mode;
152 
153 }
154 
155 void guidance_v_notify_in_flight( bool_t in_flight) {
156  if (in_flight) {
157  gv_adapt_init();
158  }
159 }
160 
161 
162 void guidance_v_run(bool_t in_flight) {
163 
164  // FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT
165  // AKA SUPERVISION and co
166  if (in_flight) {
168  }
169 
170  switch (guidance_v_mode) {
171 
173  guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only
174  stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t;
175  break;
176 
180  run_hover_loop(in_flight);
181  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
182  break;
183 
185 #if USE_FMS
186  if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_CLIMB) {
187  guidance_v_zd_sp = fms.input.v_sp.climb;
188  }
189 #endif
191  run_hover_loop(in_flight);
192 #if NO_RC_THRUST_LIMIT
193  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
194 #else
195  // saturate max authority with RC stick
197 #endif
198  break;
199 
201 #if USE_FMS
202  if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_HOVER)
203  guidance_v_z_sp = fms.input.v_sp.height;
204 #endif
206  run_hover_loop(in_flight);
207 #if NO_RC_THRUST_LIMIT
208  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
209 #else
210  // saturate max authority with RC stick
212 #endif
213  break;
214 
215  case GUIDANCE_V_MODE_NAV:
216  {
220  run_hover_loop(in_flight);
221  }
222  else if (vertical_mode == VERTICAL_MODE_CLIMB) {
226  run_hover_loop(in_flight);
227  }
228  else if (vertical_mode == VERTICAL_MODE_MANUAL) {
229  guidance_v_z_sp = -nav_flight_altitude; // For display only
231  }
232 #if NO_RC_THRUST_LIMIT
233  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
234 #else
235  /* use rc limitation if available */
236  if (radio_control.status == RC_OK)
238  else
239  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
240 #endif
241  break;
242  }
243  default:
244  break;
245  }
246 }
247 
248 
249 #define FF_CMD_FRAC 18
250 
251 #define MAX_BANK_COEF (BFP_OF_REAL(RadOfDeg(30.),INT32_TRIG_FRAC))
252 
253 __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flight) {
254 
255  /* convert our reference to generic representation */
257  guidance_v_z_ref = (int32_t)tmp;
260  /* compute the error to our reference */
261  int32_t err_z = guidance_v_z_ref - stateGetPositionNed_i()->z;
262  Bound(err_z, GUIDANCE_V_MIN_ERR_Z, GUIDANCE_V_MAX_ERR_Z);
264  Bound(err_zd, GUIDANCE_V_MIN_ERR_ZD, GUIDANCE_V_MAX_ERR_ZD);
265 
266  if (in_flight) {
267  guidance_v_z_sum_err += err_z;
268  Bound(guidance_v_z_sum_err, -GUIDANCE_V_MAX_SUM_ERR, GUIDANCE_V_MAX_SUM_ERR);
269  }
270  else
272 
273  /* our nominal command : (g + zdd)*m */
274 #ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
276 #else
277  const int32_t inv_m = gv_adapt_X>>(GV_ADAPT_X_FRAC - FF_CMD_FRAC);
278 #endif
279  const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) -
280  (guidance_v_zdd_ref<<(FF_CMD_FRAC - INT32_ACCEL_FRAC));
281 
282  guidance_v_ff_cmd = g_m_zdd / inv_m;
283  int32_t cphi,ctheta,cphitheta;
284  struct Int32Eulers* att_euler = stateGetNedToBodyEulers_i();
285  PPRZ_ITRIG_COS(cphi, att_euler->phi);
286  PPRZ_ITRIG_COS(ctheta, att_euler->theta);
287  cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC;
288  if (cphitheta < MAX_BANK_COEF) cphitheta = MAX_BANK_COEF;
289  /* feed forward command */
291 
292  /* bound the nominal command to 0.9*MAX_PPRZ */
293  Bound(guidance_v_ff_cmd, 0, 8640);
294 
295 
296  /* our error feed back command */
297  /* z-axis pointing down -> positive error means we need less thrust */
298  guidance_v_fb_cmd = ((-guidance_v_kp * err_z) >> 7) +
299  ((-guidance_v_kd * err_zd) >> 16) +
300  ((-guidance_v_ki * guidance_v_z_sum_err) >> 16);
301 
303 
304  /* bound the result */
305  Bound(guidance_v_delta_t, 0, MAX_PPRZ);
306 
307 }
static void gv_adapt_init(void)
int32_t guidance_v_z_sp
altitude setpoint in meters (input).
Definition: guidance_v.c:73
int32_t phi
in rad with INT32_ANGLE_FRAC
#define GV_ZD_REF_FRAC
number of bits for the fractional part of gv_zd_ref
int32_t guidance_v_kd
vertical control D-gain
Definition: guidance_v.c:80
#define GUIDANCE_V_MODE_NAV
Definition: guidance_v.h:42
#define Min(x, y)
#define GV_Z_REF_FRAC
number of bits for the fractional part of gv_z_ref
int32_t guidance_v_fb_cmd
feed-back command
Definition: guidance_v.c:56
#define INT32_POS_FRAC
int32_t guidance_v_delta_t
thrust command.
Definition: guidance_v.c:57
void guidance_v_run(bool_t in_flight)
Definition: guidance_v.c:162
int32_t guidance_v_kp
vertical control P-gain
Definition: guidance_v.c:79
void guidance_v_init(void)
Definition: guidance_v.c:97
int32_t gv_adapt_X
State of the estimator.
void guidance_v_notify_in_flight(bool_t in_flight)
Definition: guidance_v.c:155
void guidance_v_mode_changed(uint8_t new_mode)
Definition: guidance_v.c:126
float guidance_v_nominal_throttle
nominal throttle for hover.
Definition: guidance_v.c:59
Vertical guidance for rotorcrafts.
int32_t theta
in rad with INT32_ANGLE_FRAC
static void run_hover_loop(bool_t in_flight)
Definition: guidance_v.c:253
#define GV_ZDD_REF_FRAC
number of bits for the fractional part of gv_zdd_ref
signed long long int64_t
Definition: types.h:21
void gv_update_ref_from_zd_sp(int32_t zd_sp)
int32_t guidance_v_z_sum_err
accumulator for I-gain
Definition: guidance_v.c:83
#define INT32_TRIG_FRAC
#define GUIDANCE_V_MODE_KILL
Definition: guidance_v.h:37
int32_t guidance_v_rc_zd_sp
Vertical speed setpoint from radio control.
Definition: guidance_v.c:71
void gv_update_ref_from_z_sp(int32_t z_sp)
int32_t z
Down.
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:798
int32_t guidance_v_ki
vertical control I-gain
Definition: guidance_v.c:81
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:55
#define GUIDANCE_V_MODE_CLIMB
Definition: guidance_v.h:40
int32_t gv_zd_ref
reference model vertical speed in meters/sec (output) fixed point representation with GV_ZD_REF_FRAC ...
#define BFP_OF_REAL(_vr, _frac)
int32_t guidance_v_zdd_ref
vertical acceleration reference in meter/s^2.
Definition: guidance_v.c:77
#define GUIDANCE_V_MODE_HOVER
Definition: guidance_v.h:41
struct RadioControl radio_control
Definition: radio_control.c:25
int32_t guidance_v_z_ref
altitude reference in meters.
Definition: guidance_v.c:75
static void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref)
Adaptation function.
int32_t guidance_v_zd_ref
vertical speed reference in meter/s.
Definition: guidance_v.c:76
#define GuidanceVSetRef(_pos, _speed, _accel)
Definition: guidance_v.c:86
signed long int32_t
Definition: types.h:19
#define RC_OK
Definition: radio_control.h:45
uint8_t status
Definition: radio_control.h:50
uint8_t guidance_v_mode
Definition: guidance_v.c:54
#define RADIO_THROTTLE
Definition: spektrum_arch.h:38
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
#define MAX_BANK_COEF
Definition: guidance_v.c:251
#define FF_CMD_FRAC
Definition: guidance_v.c:249
int64_t gv_z_ref
reference model altitude in meters (output) fixed point representation with GV_Z_REF_FRAC Q37...
General stabilization interface for rotorcrafts.
void guidance_v_read_rc(void)
Definition: guidance_v.c:115
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:28
int32_t guidance_v_zd_sp
vertical speed setpoint in meter/s (input).
Definition: guidance_v.c:74
int32_t guidance_v_rc_delta_t
Direct throttle from radio control.
Definition: guidance_v.c:65
int32_t guidance_v_ff_cmd
feed-forward command
Definition: guidance_v.c:55
#define INT32_SPEED_FRAC
#define GUIDANCE_V_MODE_RC_CLIMB
Definition: guidance_v.h:39
#define MAX_PPRZ
Definition: paparazzi.h:8
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:630
#define GUIDANCE_V_MODE_RC_DIRECT
Definition: guidance_v.h:38
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1012
uint8_t vertical_mode
Definition: sim_ap.c:37
#define INT32_ACCEL_FRAC
int32_t gv_zdd_ref
reference model vertical accel in meters/s^2 (output) fixed point representation with GV_ZDD_REF_FRAC...
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:924
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:50
Paparazzi fixed point algebra.
#define GV_ADAPT_X_FRAC
euler angles