Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules 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 "state.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 
91 #ifndef V_CTL_PITCH_TRIM
92 #define V_CTL_PITCH_TRIM 0.
93 #endif
95 
96 inline static void v_ctl_climb_auto_throttle_loop(void);
97 #ifdef V_CTL_AUTO_PITCH_PGAIN
98 inline static void v_ctl_climb_auto_pitch_loop(void);
99 #endif
100 
101 #if USE_AIRSPEED
104 float v_ctl_auto_airspeed_pgain;
105 float v_ctl_auto_airspeed_igain;
106 float v_ctl_auto_airspeed_sum_err;
111 #define V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR 200
112 #define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100
113 #define V_CTL_AUTO_CLIMB_LIMIT 0.5/4.0 // m/s/s
114 #define V_CTL_AUTO_AGR_CLIMB_GAIN 2.0 // altitude gain multiplier while in aggressive climb mode
115 #endif
116 
117 #ifndef V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION
118 #define V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION 1.0f
119 #endif
120 
121 
122 void v_ctl_init(void)
123 {
124  /* mode */
126 
127  /* outer loop */
130  v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN;
134 
135  /* inner loops */
139 
142 
143  /* "auto throttle" inner loop parameters */
144  v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
145  v_ctl_auto_throttle_min_cruise_throttle = V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE;
146  v_ctl_auto_throttle_max_cruise_throttle = V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE;
148  v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
149  v_ctl_auto_throttle_pgain = V_CTL_AUTO_THROTTLE_PGAIN;
150  v_ctl_auto_throttle_igain = V_CTL_AUTO_THROTTLE_IGAIN;
153  v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
155 
156 #ifdef V_CTL_AUTO_PITCH_PGAIN
157  /* "auto pitch" inner loop parameters */
158  v_ctl_auto_pitch_pgain = V_CTL_AUTO_PITCH_PGAIN;
161 #endif
162 
163 #if USE_AIRSPEED
164  v_ctl_auto_airspeed_setpoint = V_CTL_AUTO_AIRSPEED_SETPOINT;
165  v_ctl_auto_airspeed_controlled = V_CTL_AUTO_AIRSPEED_SETPOINT;
166  v_ctl_auto_airspeed_pgain = V_CTL_AUTO_AIRSPEED_PGAIN;
167  v_ctl_auto_airspeed_igain = V_CTL_AUTO_AIRSPEED_IGAIN;
168  v_ctl_auto_airspeed_sum_err = 0.;
169 
170  v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
171  v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
172  v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN;
174 #endif
175 
177 
178  /*agressive tuning*/
179 #ifdef TUNE_AGRESSIVE_CLIMB
180  agr_climb_throttle = AGR_CLIMB_THROTTLE;
181 #undef AGR_CLIMB_THROTTLE
182 #define AGR_CLIMB_THROTTLE agr_climb_throttle
183  agr_climb_pitch = AGR_CLIMB_PITCH;
184 #undef AGR_CLIMB_PITCH
185 #define AGR_CLIMB_PITCH agr_climb_pitch
186  agr_climb_nav_ratio = AGR_CLIMB_NAV_RATIO;
187 #undef AGR_CLIMB_NAV_RATIO
188 #define AGR_CLIMB_NAV_RATIO agr_climb_nav_ratio
189  agr_descent_throttle = AGR_DESCENT_THROTTLE;
190 #undef AGR_DESCENT_THROTTLE
191 #define AGR_DESCENT_THROTTLE agr_descent_throttle
192  agr_descent_pitch = AGR_DESCENT_PITCH;
193 #undef AGR_DESCENT_PITCH
194 #define AGR_DESCENT_PITCH agr_descent_pitch
195  agr_descent_nav_ratio = AGR_DESCENT_NAV_RATIO;
196 #undef AGR_DESCENT_NAV_RATIO
197 #define AGR_DESCENT_NAV_RATIO agr_descent_nav_ratio
198 #endif
199 }
200 
206 {
207  float altitude_pgain_boost = 1.0;
208 
209 #if USE_AIRSPEED && defined(AGR_CLIMB)
210  // Aggressive climb mode (boost gain of altitude loop)
212  float dist = fabs(v_ctl_altitude_error);
213  altitude_pgain_boost = 1.0 + (V_CTL_AUTO_AGR_CLIMB_GAIN - 1.0) * (dist - AGR_BLEND_END) /
214  (AGR_BLEND_START - AGR_BLEND_END);
215  Bound(altitude_pgain_boost, 1.0, V_CTL_AUTO_AGR_CLIMB_GAIN);
216  }
217 #endif
218 
223 
224 #ifdef AGR_CLIMB
226  float dist = fabs(v_ctl_altitude_error);
227  if (dist < AGR_BLEND_END) {
229  } else if (dist > AGR_BLEND_START) {
231  } else {
233  }
234  }
235 #endif
236 }
237 
239 {
240  switch (v_ctl_climb_mode) {
242  default:
244  break;
245 #ifdef V_CTL_AUTO_PITCH_PGAIN
246 #pragma message "AUTO PITCH Enabled!"
248  v_ctl_climb_auto_pitch_loop();
249  break;
250 #endif
251  }
252 }
253 
259 #if !USE_AIRSPEED
260 
261 inline static void v_ctl_climb_auto_throttle_loop(void)
262 {
263  static float last_err;
264 
265  float f_throttle = 0;
266  float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint;
267  float d_err = err - last_err;
268  last_err = err;
273  + v_ctl_auto_throttle_dgain * d_err);
274 
275  /* pitch pre-command */
276  float v_ctl_pitch_of_vz = (v_ctl_climb_setpoint + d_err * v_ctl_auto_throttle_pitch_of_vz_dgain) *
278 
279 #if defined AGR_CLIMB
280  switch (v_ctl_auto_throttle_submode) {
282  if (v_ctl_climb_setpoint > 0) { /* Climbing */
283  f_throttle = AGR_CLIMB_THROTTLE;
284  v_ctl_pitch_setpoint = AGR_CLIMB_PITCH;
285  } else { /* Going down */
286  f_throttle = AGR_DESCENT_THROTTLE;
287  v_ctl_pitch_setpoint = AGR_DESCENT_PITCH;
288  }
289  break;
290 
292  float ratio = (fabs(v_ctl_altitude_error) - AGR_BLEND_END)
293  / (AGR_BLEND_START - AGR_BLEND_END);
294  f_throttle = (1 - ratio) * controlled_throttle;
295  v_ctl_pitch_setpoint = (1 - ratio) * (v_ctl_pitch_of_vz + v_ctl_pitch_trim);
296  v_ctl_auto_throttle_sum_err += (1 - ratio) * err;
298  /* positive error -> too low */
299  if (v_ctl_altitude_error > 0) {
300  f_throttle += ratio * AGR_CLIMB_THROTTLE;
301  v_ctl_pitch_setpoint += ratio * AGR_CLIMB_PITCH;
302  } else {
303  f_throttle += ratio * AGR_DESCENT_THROTTLE;
304  v_ctl_pitch_setpoint += ratio * AGR_DESCENT_PITCH;
305  }
306  break;
307  }
308 
310  default:
311 #endif
312  f_throttle = controlled_throttle;
315  v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + v_ctl_pitch_trim + nav_pitch;
316 #if defined AGR_CLIMB
317  break;
318  } /* switch submode */
319 #endif
320 
322 }
323 
324 #else // USE_AIRSPEED
325 
326 inline static void v_ctl_climb_auto_throttle_loop(void)
327 {
328  float f_throttle = 0;
329  float controlled_throttle;
330  float v_ctl_pitch_of_vz;
331 
332  // Limit rate of change of climb setpoint (to ensure that airspeed loop can catch-up)
333  static float v_ctl_climb_setpoint_last = 0;
334  float diff_climb = v_ctl_climb_setpoint - v_ctl_climb_setpoint_last;
335  Bound(diff_climb, -V_CTL_AUTO_CLIMB_LIMIT, V_CTL_AUTO_CLIMB_LIMIT);
336  v_ctl_climb_setpoint = v_ctl_climb_setpoint_last + diff_climb;
337  v_ctl_climb_setpoint_last = v_ctl_climb_setpoint;
338 
339  // Pitch control (input: rate of climb error, output: pitch setpoint)
340  float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint;
343  v_ctl_pitch_of_vz = -v_ctl_auto_pitch_pgain *
345 
346  // Ground speed control loop (input: groundspeed error, output: airspeed controlled)
348  v_ctl_auto_groundspeed_sum_err += err_groundspeed;
352 
353  // Do not allow controlled airspeed below the setpoint
357  v_ctl_auto_groundspeed_igain); // reset integrator of ground speed loop
358  }
359 
360  // Airspeed control loop (input: airspeed controlled, output: throttle controlled)
361  float err_airspeed = (v_ctl_auto_airspeed_controlled - stateGetAirspeed_f());
362  v_ctl_auto_airspeed_sum_err += err_airspeed;
363  BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR);
364  controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * v_ctl_auto_airspeed_igain) *
365  v_ctl_auto_airspeed_pgain;
366 
367  // Done, set outputs
368  Bound(controlled_throttle, 0, v_ctl_auto_throttle_max_cruise_throttle);
369  f_throttle = controlled_throttle;
370  v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + v_ctl_pitch_trim;
372  Bound(v_ctl_pitch_setpoint, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
373 }
374 
375 #endif // USE_AIRSPEED
376 
377 
382 #ifdef V_CTL_AUTO_PITCH_PGAIN
383 inline static void v_ctl_climb_auto_pitch_loop(void)
384 {
385  float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint;
391  Bound(v_ctl_pitch_setpoint, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
392 }
393 #endif
394 
395 #ifdef V_CTL_THROTTLE_SLEW_LIMITER
396 #define V_CTL_THROTTLE_SLEW (1./CONTROL_FREQUENCY/(V_CTL_THROTTLE_SLEW_LIMITER))
397 #endif
398 
399 #ifndef V_CTL_THROTTLE_SLEW
400 #define V_CTL_THROTTLE_SLEW 1.
401 #endif
402 
406 {
408  BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW * MAX_PPRZ));
409  v_ctl_throttle_slewed += diff_throttle;
410 }
#define V_CTL_AUTO_THROTTLE_DGAIN
Definition: guidance_v.c:51
#define V_CTL_AUTO_PITCH_MAX_SUM_ERR
Definition: guidance_v.c:86
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:912
float v_ctl_auto_throttle_pgain
Definition: guidance_v.c:60
pprz_t v_ctl_throttle_slewed
Definition: guidance_v.c:89
float alt
in meters above WGS84 reference ellipsoid
float v_ctl_altitude_max_climb
Definition: guidance_v.c:43
float v_ctl_auto_groundspeed_setpoint
in meters per second
Definition: energy_ctrl.c:125
float v_ctl_auto_groundspeed_igain
Definition: energy_ctrl.c:127
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
#define V_CTL_ALTITUDE_MAX_CLIMB
Definition: energy_ctrl.c:145
float controlled_throttle
Definition: guidance_v_n.c:85
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1310
void v_ctl_altitude_loop(void)
outer loop
Definition: guidance_v.c:205
float v_ctl_altitude_pre_climb
Path Angle.
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
float v_ctl_auto_throttle_max_cruise_throttle
Definition: guidance_v.c:58
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:121
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:261
float v_ctl_pitch_trim
Definition: guidance_v.c:94
#define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR
Definition: energy_ctrl.c:129
#define V_CTL_AUTO_THROTTLE_STANDARD
#define TRIM_UPPRZ(pprz)
Definition: paparazzi.h:14
float v_ctl_auto_groundspeed_sum_err
Definition: energy_ctrl.c:128
float v_ctl_auto_pitch_pgain
Definition: guidance_v.c:83
#define V_CTL_CLIMB_MODE_AUTO_THROTTLE
Vertical control for fixed wing vehicles.
#define V_CTL_PITCH_TRIM
Definition: guidance_v.c:92
void v_ctl_throttle_slew(void)
Computes slewed throttle from throttle setpoint called at 20Hz.
Definition: guidance_v.c:405
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
#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
float v_ctl_pitch_setpoint
Definition: guidance_v.c:90
pprz_t v_ctl_throttle_setpoint
Definition: guidance_v.c:88
#define V_CTL_AUTO_CLIMB_LIMIT
Definition: guidance_v_n.c:96
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
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:675
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
void v_ctl_init(void)
Definition: guidance_v.c:122
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:894
#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR
Definition: guidance_v.c:64
float z
in meters
void v_ctl_climb_loop(void)
Auto-throttle inner loop.
Definition: guidance_v.c:238
float v_ctl_climb_setpoint
Definition: guidance_v.c:46
#define V_CTL_AUTO_THROTTLE_BLENDED
#define V_CTL_AUTO_THROTTLE_AGRESSIVE
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:118
uint8_t v_ctl_mode
Definition: guidance_v.c:35
#define MAX_PPRZ
Definition: paparazzi.h:8
#define V_CTL_CLIMB_MODE_AUTO_PITCH
#define V_CTL_MODE_MANUAL
float v_ctl_auto_groundspeed_pgain
Definition: energy_ctrl.c:126
#define V_CTL_THROTTLE_SLEW
auto pitch inner loop
Definition: guidance_v.c:400
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
float v_ctl_auto_airspeed_controlled
Definition: energy_ctrl.c:123
float v_ctl_auto_throttle_dgain
Definition: guidance_v.c:62
float v_ctl_altitude_pre_climb_correction
Definition: guidance_v.c:42
Fixedwing autopilot modes.