Paparazzi UAS  v4.0.4_stable-3-gf39211a
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) 2006 Pascal Brisset, Antoine Drouin, Michel Gorraz
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 
29 #include "estimator.h"
30 #include "subsystems/nav.h"
31 #include "generated/airframe.h"
33 
34 /* mode */
36 
37 /* outer loop */
44 
45 /* inner loop */
49 
50 #ifndef V_CTL_AUTO_THROTTLE_DGAIN
51 #define V_CTL_AUTO_THROTTLE_DGAIN 0.
52 #endif
53 
54 /* "auto throttle" inner loop parameters */
64 #define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 150
67 
68 #ifndef V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN
69 #define V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN 0.
70 #endif
71 
72 /* agressive tuning */
73 #ifdef TUNE_AGRESSIVE_CLIMB
74 float agr_climb_throttle;
75 float agr_climb_pitch;
76 float agr_climb_nav_ratio;
77 float agr_descent_throttle;
78 float agr_descent_pitch;
79 float agr_descent_nav_ratio;
80 #endif
81 
82 /* "auto pitch" inner loop parameters */
86 #define V_CTL_AUTO_PITCH_MAX_SUM_ERR 100
87 
90 
91 inline static void v_ctl_climb_auto_throttle_loop( void );
92 #ifdef V_CTL_AUTO_PITCH_PGAIN
93 inline static void v_ctl_climb_auto_pitch_loop( void );
94 #endif
95 
96 #if USE_AIRSPEED
97 float v_ctl_auto_airspeed_setpoint;
98 float v_ctl_auto_airspeed_controlled;
99 float v_ctl_auto_airspeed_pgain;
100 float v_ctl_auto_airspeed_igain;
101 float v_ctl_auto_airspeed_sum_err;
102 float v_ctl_auto_groundspeed_setpoint;
103 float v_ctl_auto_groundspeed_pgain;
104 float v_ctl_auto_groundspeed_igain;
105 float v_ctl_auto_groundspeed_sum_err;
106 #define V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR 200
107 #define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100
108 #define V_CTL_AUTO_CLIMB_LIMIT 0.5/4.0 // m/s/s
109 #define V_CTL_AUTO_AGR_CLIMB_GAIN 2.0 // altitude gain multiplier while in aggressive climb mode
110 #endif
111 
112 #pragma message "CAUTION! ALL control gains have to be positive now!"
113 
114 #ifndef V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION
115 #define V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION 1.0f
116 #endif
117 
118 
119 void v_ctl_init( void ) {
120  /* mode */
122 
123  /* outer loop */
126  v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN;
129  v_ctl_altitude_max_climb = V_CTL_ALTITUDE_MAX_CLIMB;
130 
131  /* inner loops */
135 
136  /* "auto throttle" inner loop parameters */
137  v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
138  v_ctl_auto_throttle_min_cruise_throttle = V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE;
139  v_ctl_auto_throttle_max_cruise_throttle = V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE;
141  v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
142  v_ctl_auto_throttle_pgain = V_CTL_AUTO_THROTTLE_PGAIN;
143  v_ctl_auto_throttle_igain = V_CTL_AUTO_THROTTLE_IGAIN;
146  v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
148 
149 #ifdef V_CTL_AUTO_PITCH_PGAIN
150  /* "auto pitch" inner loop parameters */
151  v_ctl_auto_pitch_pgain = V_CTL_AUTO_PITCH_PGAIN;
154 #endif
155 
156 #if USE_AIRSPEED
157  v_ctl_auto_airspeed_setpoint = V_CTL_AUTO_AIRSPEED_SETPOINT;
158  v_ctl_auto_airspeed_controlled = V_CTL_AUTO_AIRSPEED_SETPOINT;
159  v_ctl_auto_airspeed_pgain = V_CTL_AUTO_AIRSPEED_PGAIN;
160  v_ctl_auto_airspeed_igain = V_CTL_AUTO_AIRSPEED_IGAIN;
161  v_ctl_auto_airspeed_sum_err = 0.;
162 
163  v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
164  v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
165  v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN;
166  v_ctl_auto_groundspeed_sum_err = 0.;
167 #endif
168 
170 
171 /*agressive tuning*/
172 #ifdef TUNE_AGRESSIVE_CLIMB
173  agr_climb_throttle = AGR_CLIMB_THROTTLE;
174  #undef AGR_CLIMB_THROTTLE
175  #define AGR_CLIMB_THROTTLE agr_climb_throttle
176  agr_climb_pitch = AGR_CLIMB_PITCH;
177  #undef AGR_CLIMB_PITCH
178  #define AGR_CLIMB_PITCH agr_climb_pitch
179  agr_climb_nav_ratio = AGR_CLIMB_NAV_RATIO;
180  #undef AGR_CLIMB_NAV_RATIO
181  #define AGR_CLIMB_NAV_RATIO agr_climb_nav_ratio
182  agr_descent_throttle = AGR_DESCENT_THROTTLE;
183  #undef AGR_DESCENT_THROTTLE
184  #define AGR_DESCENT_THROTTLE agr_descent_throttle
185  agr_descent_pitch = AGR_DESCENT_PITCH;
186  #undef AGR_DESCENT_PITCH
187  #define AGR_DESCENT_PITCH agr_descent_pitch
188  agr_descent_nav_ratio = AGR_DESCENT_NAV_RATIO;
189  #undef AGR_DESCENT_NAV_RATIO
190  #define AGR_DESCENT_NAV_RATIO agr_descent_nav_ratio
191 #endif
192 }
193 
198 void v_ctl_altitude_loop( void ) {
199  float altitude_pgain_boost = 1.0;
200 
201 #if USE_AIRSPEED && defined(AGR_CLIMB)
202  // Aggressive climb mode (boost gain of altitude loop)
204  float dist = fabs(v_ctl_altitude_error);
205  altitude_pgain_boost = 1.0 + (V_CTL_AUTO_AGR_CLIMB_GAIN-1.0)*(dist-AGR_BLEND_END)/(AGR_BLEND_START-AGR_BLEND_END);
206  Bound(altitude_pgain_boost, 1.0, V_CTL_AUTO_AGR_CLIMB_GAIN);
207  }
208 #endif
209 
214 
215 #ifdef AGR_CLIMB
217  float dist = fabs(v_ctl_altitude_error);
218  if (dist < AGR_BLEND_END) {
220  }
221  else if (dist > AGR_BLEND_START) {
223  }
224  else {
226  }
227  }
228 #endif
229 }
230 
231 void v_ctl_climb_loop ( void ) {
232  switch (v_ctl_climb_mode) {
234  default:
236  break;
237 #ifdef V_CTL_AUTO_PITCH_PGAIN
239  v_ctl_climb_auto_pitch_loop();
240  break;
241 #endif
242  }
243 }
244 
250 #if !USE_AIRSPEED
251 
252 inline static void v_ctl_climb_auto_throttle_loop(void) {
253  static float last_err;
254 
255  float f_throttle = 0;
257  float d_err = err - last_err;
258  last_err = err;
260  + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint
263  + v_ctl_auto_throttle_dgain * d_err);
264 
265  /* pitch pre-command */
266  float v_ctl_pitch_of_vz = (v_ctl_climb_setpoint + d_err * v_ctl_auto_throttle_pitch_of_vz_dgain) * v_ctl_auto_throttle_pitch_of_vz_pgain;
267 
268 #if defined AGR_CLIMB
269  switch (v_ctl_auto_throttle_submode) {
271  if (v_ctl_climb_setpoint > 0) { /* Climbing */
272  f_throttle = AGR_CLIMB_THROTTLE;
273  nav_pitch = AGR_CLIMB_PITCH;
274  }
275  else { /* Going down */
276  f_throttle = AGR_DESCENT_THROTTLE;
277  nav_pitch = AGR_DESCENT_PITCH;
278  }
279  break;
280 
282  float ratio = (fabs(v_ctl_altitude_error) - AGR_BLEND_END)
283  / (AGR_BLEND_START - AGR_BLEND_END);
284  f_throttle = (1-ratio) * controlled_throttle;
285  nav_pitch = (1-ratio) * v_ctl_pitch_of_vz;
286  v_ctl_auto_throttle_sum_err += (1-ratio) * err;
288  /* positive error -> too low */
289  if (v_ctl_altitude_error > 0) {
290  f_throttle += ratio * AGR_CLIMB_THROTTLE;
291  nav_pitch += ratio * AGR_CLIMB_PITCH;
292  } else {
293  f_throttle += ratio * AGR_DESCENT_THROTTLE;
294  nav_pitch += ratio * AGR_DESCENT_PITCH;
295  }
296  break;
297  }
298 
300  default:
301 #endif
302  f_throttle = controlled_throttle;
305  nav_pitch += v_ctl_pitch_of_vz;
306 #if defined AGR_CLIMB
307  break;
308  } /* switch submode */
309 #endif
310 
312 }
313 
314 #else // USE_AIRSPEED
315 
316 inline static void v_ctl_climb_auto_throttle_loop(void) {
317  float f_throttle = 0;
318  float controlled_throttle;
319  float v_ctl_pitch_of_vz;
320 
321  // Limit rate of change of climb setpoint (to ensure that airspeed loop can catch-up)
322  static float v_ctl_climb_setpoint_last = 0;
323  float diff_climb = v_ctl_climb_setpoint - v_ctl_climb_setpoint_last;
324  Bound(diff_climb, -V_CTL_AUTO_CLIMB_LIMIT, V_CTL_AUTO_CLIMB_LIMIT);
325  v_ctl_climb_setpoint = v_ctl_climb_setpoint_last + diff_climb;
326  v_ctl_climb_setpoint_last = v_ctl_climb_setpoint;
327 
328  // Pitch control (input: rate of climb error, output: pitch setpoint)
332  v_ctl_pitch_of_vz = -v_ctl_auto_pitch_pgain *
334 
335  // Ground speed control loop (input: groundspeed error, output: airspeed controlled)
336  float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - estimator_hspeed_mod);
337  v_ctl_auto_groundspeed_sum_err += err_groundspeed;
338  BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR);
339  v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) * v_ctl_auto_groundspeed_pgain;
340 
341  // Do not allow controlled airspeed below the setpoint
342  if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint) {
343  v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint;
344  v_ctl_auto_groundspeed_sum_err = v_ctl_auto_airspeed_controlled/(v_ctl_auto_groundspeed_pgain*v_ctl_auto_groundspeed_igain); // reset integrator of ground speed loop
345  }
346 
347  // Airspeed control loop (input: airspeed controlled, output: throttle controlled)
348  float err_airspeed = (v_ctl_auto_airspeed_controlled - estimator_airspeed);
349  v_ctl_auto_airspeed_sum_err += err_airspeed;
350  BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR);
351  controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * v_ctl_auto_airspeed_igain) * v_ctl_auto_airspeed_pgain;
352 
353  // Done, set outputs
354  Bound(controlled_throttle, 0, v_ctl_auto_throttle_max_cruise_throttle);
355  f_throttle = controlled_throttle;
356  nav_pitch = v_ctl_pitch_of_vz;
358  Bound(nav_pitch, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
359 }
360 
361 #endif // USE_AIRSPEED
362 
363 
368 #ifdef V_CTL_AUTO_PITCH_PGAIN
369 inline static void v_ctl_climb_auto_pitch_loop(void) {
376  Bound(nav_pitch, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
377 }
378 #endif
379 
380 #ifdef V_CTL_THROTTLE_SLEW_LIMITER
381 #define V_CTL_THROTTLE_SLEW (1./CONTROL_RATE/(V_CTL_THROTTLE_SLEW_LIMITER))
382 #endif
383 
384 #ifndef V_CTL_THROTTLE_SLEW
385 #define V_CTL_THROTTLE_SLEW 1.
386 #endif
387 
390 void v_ctl_throttle_slew( void ) {
392  BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ));
393  v_ctl_throttle_slewed += diff_throttle;
394 }
#define V_CTL_AUTO_THROTTLE_DGAIN
Definition: guidance_v.c:51
#define V_CTL_AUTO_PITCH_MAX_SUM_ERR
Definition: guidance_v.c:86
#define V_CTL_AUTO_THROTTLE_BLENDED
Definition: guidance_v.h:59
float estimator_hspeed_mod
module of horizontal ground speed in m/s
Definition: estimator.c:64
#define V_CTL_CLIMB_MODE_AUTO_THROTTLE
Definition: guidance_v.h:53
float v_ctl_auto_throttle_pgain
Definition: guidance_v.c:60
pprz_t v_ctl_throttle_slewed
Definition: guidance_v.c:89
float v_ctl_altitude_max_climb
Definition: guidance_v.c:43
float estimator_z_dot
Definition: estimator.c:46
float v_ctl_auto_throttle_sum_err
Definition: guidance_v.c:63
uint8_t v_ctl_climb_mode
Definition: guidance_v.c:47
float v_ctl_auto_throttle_igain
Definition: guidance_v.c:61
int16_t pprz_t
Definition: paparazzi.h:6
float controlled_throttle
Definition: guidance_v_n.c:85
void v_ctl_altitude_loop(void)
outer loop
Definition: guidance_v.c:198
float v_ctl_altitude_pre_climb
Definition: guidance_v.c:39
float v_ctl_auto_throttle_climb_throttle_increment
Definition: guidance_v.c:59
float v_ctl_auto_throttle_pitch_of_vz_dgain
Definition: guidance_v.c:66
float v_ctl_auto_throttle_nominal_cruise_throttle
Definition: guidance_v.c:56
#define V_CTL_CLIMB_MODE_AUTO_PITCH
Definition: guidance_v.h:54
float v_ctl_auto_throttle_max_cruise_throttle
Definition: guidance_v.c:58
#define V_CTL_AUTO_THROTTLE_AGRESSIVE
Definition: guidance_v.h:58
uint8_t v_ctl_auto_throttle_submode
Definition: guidance_v.c:48
static void v_ctl_climb_auto_throttle_loop(void)
auto throttle inner loop
Definition: guidance_v.c:252
#define TRIM_UPPRZ(pprz)
Definition: paparazzi.h:14
float v_ctl_auto_pitch_pgain
Definition: guidance_v.c:83
Vertical control for fixed wing vehicles.
void v_ctl_throttle_slew(void)
Computes slewed throttle from throttle setpoint called at 20Hz.
Definition: guidance_v.c:390
#define V_CTL_MODE_MANUAL
Definition: guidance_v.h:35
float v_ctl_altitude_error
in meters, (setpoint - alt) -> positive = too low
Definition: guidance_v.c:41
#define V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN
Definition: guidance_v.c:69
float estimator_airspeed
m/s
Definition: estimator.c:69
#define V_CTL_AUTO_PITCH_IGAIN
Definition: guidance_v_n.c:82
float v_ctl_auto_pitch_sum_err
Definition: guidance_v.c:85
float v_ctl_auto_throttle_min_cruise_throttle
Definition: guidance_v.c:57
float v_ctl_auto_pitch_igain
Definition: guidance_v.c:84
pprz_t v_ctl_throttle_setpoint
Definition: guidance_v.c:88
#define V_CTL_AUTO_CLIMB_LIMIT
Definition: guidance_v_n.c:90
float v_ctl_altitude_pgain
Definition: guidance_v.c:40
float v_ctl_altitude_setpoint
in meters above MSL
Definition: guidance_v.c:38
float v_ctl_auto_throttle_cruise_throttle
Definition: guidance_v.c:55
unsigned char uint8_t
Definition: types.h:14
float estimator_z
altitude above MSL in meters
Definition: estimator.c:44
void v_ctl_init(void)
Definition: guidance_v.c:119
#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR
Definition: guidance_v.c:64
#define V_CTL_AUTO_THROTTLE_STANDARD
Definition: guidance_v.h:57
State estimation, fusioning sensors.
void v_ctl_climb_loop(void)
Definition: guidance_v.c:231
float v_ctl_climb_setpoint
Definition: guidance_v.c:46
float v_ctl_auto_throttle_pitch_of_vz_pgain
Definition: guidance_v.c:65
#define V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION
Definition: guidance_v.c:115
uint8_t v_ctl_mode
Definition: guidance_v.c:35
#define MAX_PPRZ
Definition: paparazzi.h:8
#define V_CTL_THROTTLE_SLEW
auto pitch inner loop
Definition: guidance_v.c:385
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
float v_ctl_auto_throttle_dgain
Definition: guidance_v.c:62
float v_ctl_altitude_pre_climb_correction
Definition: guidance_v.c:42