Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros 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
28 #define GUIDANCE_V_USE_REF 1
30 
31 
34 #include "subsystems/ahrs.h"
35 // #include "booz_fms.h" FIXME
37 
38 #include "subsystems/ins.h"
39 #include "math/pprz_algebra_int.h"
40 
41 #include "generated/airframe.h"
42 
43 
44 /* warn if some gains are still negative */
45 #if (GUIDANCE_V_HOVER_KP < 0) || \
46  (GUIDANCE_V_HOVER_KD < 0) || \
47  (GUIDANCE_V_HOVER_KI < 0)
48 #warning "ALL control gains are now positive!!!"
49 #endif
50 
51 #if defined GUIDANCE_V_INV_M
52 #warning "GUIDANCE_V_INV_M has been removed. If you don't want to use adaptive hover, please define GUIDANCE_V_NOMINAL_HOVER_THROTTLE"
53 #endif
54 
55 #define GUIDANCE_V_GAIN_SCALER 48
56 
61 
63 
64 
69 
75 
81 
85 
87 
88 
89 #define GuidanceVSetRef(_pos, _speed, _accel) { \
90  gv_set_ref(_pos, _speed, _accel); \
91  guidance_v_z_ref = _pos; \
92  guidance_v_zd_ref = _speed; \
93  guidance_v_zdd_ref = _accel; \
94  }
95 
96 
97 __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flight);
98 
99 
100 void guidance_v_init(void) {
101 
103 
104  guidance_v_kp = GUIDANCE_V_HOVER_KP;
105  guidance_v_kd = GUIDANCE_V_HOVER_KD;
106  guidance_v_ki = GUIDANCE_V_HOVER_KI;
107 
109 
110 #ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
111  guidance_v_nominal_throttle = GUIDANCE_V_NOMINAL_HOVER_THROTTLE;
112 #endif
113 
114  gv_adapt_init();
115 }
116 
117 
118 void guidance_v_read_rc(void) {
119 
120  /* used in RC_DIRECT directly and as saturation in CLIMB and HOVER */
122 
123  /* used in RC_CLIMB */
124  guidance_v_rc_zd_sp = ((MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]) * GUIDANCE_V_RC_CLIMB_COEF;
125  DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_RC_CLIMB_DEAD_BAND);
126 
127 }
128 
130 
131  if (new_mode == guidance_v_mode)
132  return;
133 
134  switch (new_mode) {
136  guidance_v_z_sp = ins_ltp_pos.z; // set current altitude as setpoint
139  break;
140 
143  guidance_v_zd_sp = 0;
144  case GUIDANCE_V_MODE_NAV:
147  break;
148 
149  default:
150  break;
151 
152  }
153 
154  guidance_v_mode = new_mode;
155 
156 }
157 
158 void guidance_v_notify_in_flight( bool_t in_flight) {
159  if (in_flight) {
160  gv_adapt_init();
161  }
162 }
163 
164 
165 void guidance_v_run(bool_t in_flight) {
166 
167  // FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT
168  // AKA SUPERVISION and co
169  if (in_flight) {
171  }
172 
173  switch (guidance_v_mode) {
174 
176  guidance_v_z_sp = ins_ltp_pos.z; // for display only
177  stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t;
178  break;
179 
183  run_hover_loop(in_flight);
184  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
185  break;
186 
188 #if USE_FMS
189  if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_CLIMB) {
190  guidance_v_zd_sp = fms.input.v_sp.climb;
191  }
192 #endif
194  run_hover_loop(in_flight);
195 #if NO_RC_THRUST_LIMIT
196  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
197 #else
198  // saturate max authority with RC stick
200 #endif
201  break;
202 
204 #if USE_FMS
205  if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_HOVER)
206  guidance_v_z_sp = fms.input.v_sp.height;
207 #endif
209  run_hover_loop(in_flight);
210 #if NO_RC_THRUST_LIMIT
211  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
212 #else
213  // saturate max authority with RC stick
215 #endif
216  break;
217 
218  case GUIDANCE_V_MODE_NAV:
219  {
223  run_hover_loop(in_flight);
224  }
225  else if (vertical_mode == VERTICAL_MODE_CLIMB) {
229  run_hover_loop(in_flight);
230  }
231  else if (vertical_mode == VERTICAL_MODE_MANUAL) {
232  guidance_v_z_sp = -nav_flight_altitude; // For display only
234  }
235 #if NO_RC_THRUST_LIMIT
236  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
237 #else
238  /* use rc limitation if available */
239  if (radio_control.status == RC_OK)
241  else
242  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
243 #endif
244  break;
245  }
246  default:
247  break;
248  }
249 }
250 
251 
252 #define FF_CMD_FRAC 18
253 
254 #define MAX_BANK_COEF (BFP_OF_REAL(RadOfDeg(30.),INT32_TRIG_FRAC))
255 
256 __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flight) {
257 
258  /* convert our reference to generic representation */
260  guidance_v_z_ref = (int32_t)tmp;
263  /* compute the error to our reference */
264  int32_t err_z = guidance_v_z_ref - ins_ltp_pos.z;
265  Bound(err_z, GUIDANCE_V_MIN_ERR_Z, GUIDANCE_V_MAX_ERR_Z);
267  Bound(err_zd, GUIDANCE_V_MIN_ERR_ZD, GUIDANCE_V_MAX_ERR_ZD);
268 
269  if (in_flight) {
270  guidance_v_z_sum_err += err_z;
271  Bound(guidance_v_z_sum_err, -GUIDANCE_V_MAX_SUM_ERR, GUIDANCE_V_MAX_SUM_ERR);
272  }
273  else
275 
276  /* our nominal command : (g + zdd)*m */
277 #ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
279 #else
280  const int32_t inv_m = gv_adapt_X>>(GV_ADAPT_X_FRAC - FF_CMD_FRAC);
281 #endif
282  const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) -
283  (guidance_v_zdd_ref<<(FF_CMD_FRAC - INT32_ACCEL_FRAC));
284 
285  guidance_v_ff_cmd = g_m_zdd / inv_m;
286  int32_t cphi,ctheta,cphitheta;
289  cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC;
290  if (cphitheta < MAX_BANK_COEF) cphitheta = MAX_BANK_COEF;
291  /* feed forward command */
293 
294  /* bound the nominal command to 0.9*MAX_PPRZ */
295  Bound(guidance_v_ff_cmd, 0, 8640);
296 
297 
298  /* our error feed back command */
299  /* z-axis pointing down -> positive error means we need less thrust */
300  guidance_v_fb_cmd = ((-guidance_v_kp * err_z) >> 12) +
301  ((-guidance_v_kd * err_zd) >> 21) +
302  ((-guidance_v_ki * guidance_v_z_sum_err) >> 21);
303 
305 
306  /* bound the result */
307  Bound(guidance_v_delta_t, 0, MAX_PPRZ);
308 
309 }
static void gv_adapt_init(void)
int32_t gv_zdd_ref
reference model vertical accel in meters/s^2 (output) fixed point representation with GV_ZDD_REF_FRAC...
int32_t guidance_v_z_sp
altitude setpoint in meters (input).
Definition: guidance_v.c:76
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
Attitude and Heading Reference System interface.
int32_t guidance_v_kd
vertical control D-gain
Definition: guidance_v.c:83
#define GUIDANCE_V_MODE_NAV
Definition: guidance_v.h:53
#define Min(x, y)
#define GV_Z_REF_FRAC
number of bits for the fractional part of gv_z_ref
__attribute__((always_inline))
Definition: guidance_v.c:97
int32_t gv_zd_ref
reference model vertical speed in meters/sec (output) fixed point representation with GV_ZD_REF_FRAC ...
int32_t guidance_v_fb_cmd
feed-back command
Definition: guidance_v.c:59
#define INT32_POS_FRAC
int32_t guidance_v_delta_t
thrust command.
Definition: guidance_v.c:60
void guidance_v_run(bool_t in_flight)
Definition: guidance_v.c:165
int32_t guidance_v_kp
vertical control P-gain
Definition: guidance_v.c:82
int32_t gv_adapt_X
State of the estimator.
void guidance_v_notify_in_flight(bool_t in_flight)
Definition: guidance_v.c:158
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
#define GUIDANCE_V_GAIN_SCALER
Definition: guidance_v.c:55
struct NedCoor_i ins_ltp_accel
Definition: ins.c:79
void guidance_v_mode_changed(uint8_t new_mode)
Definition: guidance_v.c:129
float guidance_v_nominal_throttle
nominal throttle for hover.
Definition: guidance_v.c:62
Vertical guidance for rotorcrafts.
Device independent INS code.
int32_t theta
in rad with INT32_ANGLE_FRAC
#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
int32_t guidance_v_z_sum_err
accumulator for I-gain
Definition: guidance_v.c:86
#define INT32_TRIG_FRAC
struct NedCoor_i ins_ltp_pos
Definition: ins.c:77
#define GUIDANCE_V_MODE_KILL
Definition: guidance_v.h:48
static void gv_update_ref_from_z_sp(int32_t z_sp)
int32_t guidance_v_rc_zd_sp
Vertical speed setpoint from radio control.
Definition: guidance_v.c:74
void guidance_v_init(void)
int32_t guidance_v_ki
vertical control I-gain
Definition: guidance_v.c:84
#define GUIDANCE_V_MODE_CLIMB
Definition: guidance_v.h:51
struct NedCoor_i ins_ltp_speed
Definition: ins.c:78
#define BFP_OF_REAL(_vr, _frac)
int32_t guidance_v_zdd_ref
vertical acceleration reference in meter/s^2.
Definition: guidance_v.c:80
#define GUIDANCE_V_MODE_HOVER
Definition: guidance_v.h:52
struct RadioControl radio_control
Definition: radio_control.c:27
int32_t guidance_v_z_ref
altitude reference in meters.
Definition: guidance_v.c:78
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:79
int64_t gv_z_ref
reference model altitude in meters (output) fixed point representation with GV_Z_REF_FRAC Q37...
#define GuidanceVSetRef(_pos, _speed, _accel)
Definition: guidance_v.c:89
signed long int32_t
Definition: types.h:19
uint8_t guidance_v_mode
Definition: guidance_v.c:57
#define RADIO_THROTTLE
Definition: spektrum_arch.h:40
static void gv_update_ref_from_zd_sp(int32_t zd_sp)
struct Int32Eulers ltp_to_body_euler
Rotation from LocalTangentPlane to body frame as Euler angles.
Definition: ahrs.h:50
unsigned char uint8_t
Definition: types.h:14
#define MAX_BANK_COEF
Definition: guidance_v.c:254
#define FF_CMD_FRAC
Definition: guidance_v.c:252
General stabilization interface for rotorcrafts.
void guidance_v_read_rc(void)
Definition: guidance_v.c:118
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:77
int32_t guidance_v_rc_delta_t
Direct throttle from radio control.
Definition: guidance_v.c:68
int32_t guidance_v_ff_cmd
feed-forward command
Definition: guidance_v.c:58
#define INT32_SPEED_FRAC
#define GUIDANCE_V_MODE_RC_CLIMB
Definition: guidance_v.h:50
#define MAX_PPRZ
Definition: paparazzi.h:8
#define GUIDANCE_V_MODE_RC_DIRECT
Definition: guidance_v.h:49
uint8_t vertical_mode
Definition: sim_ap.c:38
#define INT32_ACCEL_FRAC
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:52
Paparazzi fixed point algebra.
#define GV_ADAPT_X_FRAC