Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
guidance_v_n.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 ENAC
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 
30 #include "state.h"
32 #include "generated/airframe.h"
33 #include "autopilot.h"
34 
35 /* mode */
37 
38 /* outer loop */
43 
44 /* inner loop */
48 
49 #ifndef V_CTL_AUTO_THROTTLE_DGAIN
50 #define V_CTL_AUTO_THROTTLE_DGAIN 0.
51 #endif
52 
53 /* "auto throttle" inner loop parameters */
63 #define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 0.4
66 
67 #ifndef V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN
68 #define V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN 0.
69 #endif
70 
71 /* "auto pitch" inner loop parameters */
76 #define V_CTL_AUTO_PITCH_MAX_SUM_ERR (RadOfDeg(10.))
77 
78 #ifndef V_CTL_AUTO_PITCH_DGAIN
79 #define V_CTL_AUTO_PITCH_DGAIN 0.
80 #endif
81 #ifndef V_CTL_AUTO_PITCH_IGAIN
82 #define V_CTL_AUTO_PITCH_IGAIN 0.
83 #endif
84 
89 #ifndef V_CTL_PITCH_TRIM
90 #define V_CTL_PITCH_TRIM 0.
91 #endif
93 
94 // Set higher than 2*V_CTL_ALTITUDE_MAX_CLIMB to disable
95 #ifndef V_CTL_AUTO_CLIMB_LIMIT
96 #define V_CTL_AUTO_CLIMB_LIMIT (0.5/4.0) // m/s/s
97 #endif
98 
100 
101 #if USE_AIRSPEED
116 #define V_CTL_AUTO_AIRSPEED_PITCH_MAX_SUM_ERR (RadOfDeg(15.))
117 #define V_CTL_AUTO_AIRSPEED_THROTTLE_MAX_SUM_ERR 0.2
118 #define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100
119 #endif
120 
121 void v_ctl_init(void)
122 {
123  /* mode */
126 
127  /* outer loop */
130  v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN;
132 
133  /* inner loops */
137 
140 
141  /* "auto throttle" inner loop parameters */
142  v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
144  v_ctl_auto_throttle_min_cruise_throttle = V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE;
145  v_ctl_auto_throttle_max_cruise_throttle = V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE;
146  v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
147  v_ctl_auto_throttle_pgain = V_CTL_AUTO_THROTTLE_PGAIN;
148  v_ctl_auto_throttle_igain = V_CTL_AUTO_THROTTLE_IGAIN;
151  v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
153 
154  /* "auto pitch" inner loop parameters */
155  v_ctl_auto_pitch_pgain = V_CTL_AUTO_PITCH_PGAIN;
159 
160 #if USE_AIRSPEED
161  v_ctl_auto_airspeed_setpoint = V_CTL_AUTO_AIRSPEED_SETPOINT;
162  v_ctl_auto_airspeed_controlled = V_CTL_AUTO_AIRSPEED_SETPOINT;
163  v_ctl_auto_airspeed_throttle_pgain = V_CTL_AUTO_AIRSPEED_THROTTLE_PGAIN;
164  v_ctl_auto_airspeed_throttle_dgain = V_CTL_AUTO_AIRSPEED_THROTTLE_DGAIN;
165  v_ctl_auto_airspeed_throttle_igain = V_CTL_AUTO_AIRSPEED_THROTTLE_IGAIN;
167  v_ctl_auto_airspeed_pitch_pgain = V_CTL_AUTO_AIRSPEED_PITCH_PGAIN;
168  v_ctl_auto_airspeed_pitch_dgain = V_CTL_AUTO_AIRSPEED_PITCH_DGAIN;
169  v_ctl_auto_airspeed_pitch_igain = V_CTL_AUTO_AIRSPEED_PITCH_IGAIN;
171 
172  v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
173  v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
174  v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN;
176 #endif
177 
178  controlled_throttle = 0.;
180 }
181 
183 {
186  }
187 #if CTRL_VERTICAL_LANDING
190  } else {
191 #endif
195  } else {
198  } /* v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB */
199  } /* v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE */
200 #if CTRL_VERTICAL_LANDING
201  } /* v_ctl_mode == V_CTL_MODE_LANDING */
202 #endif
203 
204 #if defined V_CTL_THROTTLE_IDLE
205  Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE * MAX_PPRZ), MAX_PPRZ);
206 #endif
207 
208 #ifdef V_CTL_POWER_CTL_BAT_NOMINAL
209  if (electrical.vsupply > 0.) {
210  v_ctl_throttle_setpoint *= V_CTL_POWER_CTL_BAT_NOMINAL / electrical.vsupply;
212  }
213 #endif
214 
217  }
218 }
219 
225 // Don't use lead controller unless you know what you're doing
226 #define LEAD_CTRL_TAU 0.8
227 #define LEAD_CTRL_A 1.
228 #define LEAD_CTRL_Te (1./60.)
229 
231 {
232  static float v_ctl_climb_setpoint_last = 0.;
233  //static float last_lead_input = 0.;
234 
235  // Altitude error
238 
239  // Lead controller
240  //v_ctl_climb_setpoint = ((LEAD_CTRL_TAU*LEAD_CTRL_A + LEAD_CTRL_Te)*climb_sp + LEAD_CTRL_TAU*(v_ctl_climb_setpoint - LEAD_CTRL_A*last_lead_input)) / (LEAD_CTRL_TAU + LEAD_CTRL_Te);
241  //last_lead_input = pitch_sp;
242 
243  // Limit rate of change of climb setpoint
244  float diff_climb = v_ctl_climb_setpoint - v_ctl_climb_setpoint_last;
245  BoundAbs(diff_climb, V_CTL_AUTO_CLIMB_LIMIT);
246  v_ctl_climb_setpoint = v_ctl_climb_setpoint_last + diff_climb;
247 
248  // Limit climb setpoint
250  v_ctl_climb_setpoint_last = v_ctl_climb_setpoint;
251 }
252 
253 static inline void v_ctl_set_pitch(void)
254 {
255  static float last_err = 0.;
256 
257  if (autopilot_get_mode() == AP_MODE_MANUAL || autopilot.launch == false) {
259  }
260 
261  // Compute errors
262  float err = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z;
263  float d_err = err - last_err;
264  last_err = err;
265 
266  if (v_ctl_auto_pitch_igain > 0.) {
267  v_ctl_auto_pitch_sum_err += err * (1. / 60.);
269  }
270 
271  // PI loop + feedforward ctl
275  + v_ctl_auto_pitch_pgain * err
276  + v_ctl_auto_pitch_dgain * d_err
278 
279 }
280 
281 static inline void v_ctl_set_throttle(void)
282 {
283  static float last_err = 0.;
284 
285  if (autopilot_get_mode() == AP_MODE_MANUAL || autopilot.launch == false) {
287  }
288 
289  // Compute errors
290  float err = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z;
291  float d_err = err - last_err;
292  last_err = err;
293 
294  if (v_ctl_auto_throttle_igain > 0.) {
295  v_ctl_auto_throttle_sum_err += err * (1. / 60.);
297  }
298 
299  // PID loop + feedforward ctl
303  + v_ctl_auto_throttle_dgain * d_err
305 
306 }
307 
308 #if USE_AIRSPEED
309 #define AIRSPEED_LOOP_PERIOD (1./60.)
310 
311 // Airspeed control loop (input: [airspeed controlled, climb_setpoint], output: [throttle controlled, pitch setpoint])
312 static inline void v_ctl_set_airspeed(void)
313 {
314  static float last_err_vz = 0.;
315  static float last_err_as = 0.;
316 
317  // Bound airspeed setpoint
318  Bound(v_ctl_auto_airspeed_setpoint, V_CTL_AIRSPEED_MIN, V_CTL_AIRSPEED_MAX);
319 
320  // Compute errors
321  float err_vz = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z;
322  float d_err_vz = (err_vz - last_err_vz) * AIRSPEED_LOOP_PERIOD;
323  last_err_vz = err_vz;
324  if (v_ctl_auto_throttle_igain > 0.) {
325  v_ctl_auto_throttle_sum_err += err_vz * AIRSPEED_LOOP_PERIOD;
327  }
328  if (v_ctl_auto_pitch_igain > 0.) {
329  v_ctl_auto_pitch_sum_err += err_vz * AIRSPEED_LOOP_PERIOD;
331  }
332 
333  float err_airspeed = v_ctl_auto_airspeed_setpoint - stateGetAirspeed_f();
334  float d_err_airspeed = (err_airspeed - last_err_as) * AIRSPEED_LOOP_PERIOD;
335  last_err_as = err_airspeed;
337  v_ctl_auto_airspeed_throttle_sum_err += err_airspeed * AIRSPEED_LOOP_PERIOD;
339  V_CTL_AUTO_AIRSPEED_THROTTLE_MAX_SUM_ERR / v_ctl_auto_airspeed_throttle_igain);
340  }
342  v_ctl_auto_airspeed_pitch_sum_err += err_airspeed * AIRSPEED_LOOP_PERIOD;
343  BoundAbs(v_ctl_auto_airspeed_pitch_sum_err, V_CTL_AUTO_AIRSPEED_PITCH_MAX_SUM_ERR / v_ctl_auto_airspeed_pitch_igain);
344  }
345 
346 
347  // Reset integrators in manual or before flight
348  if (autopilot_get_mode() == AP_MODE_MANUAL || autopilot.launch == false) {
353  }
354 
355  // Pitch loop
359  + v_ctl_auto_pitch_pgain * err_vz
360  + v_ctl_auto_pitch_dgain * d_err_vz
362  - v_ctl_auto_airspeed_pitch_pgain * err_airspeed
363  - v_ctl_auto_airspeed_pitch_dgain * d_err_airspeed
365 
366  // Throttle loop
369  + v_ctl_auto_throttle_pgain * err_vz
370  + v_ctl_auto_throttle_dgain * d_err_vz
372  + v_ctl_auto_airspeed_throttle_pgain * err_airspeed
373  + v_ctl_auto_airspeed_throttle_dgain * d_err_airspeed
375 
376 }
377 
378 static inline void v_ctl_set_groundspeed(void)
379 {
380  // Ground speed control loop (input: groundspeed error, output: airspeed controlled)
382  v_ctl_auto_groundspeed_sum_err += err_groundspeed;
386 
387 }
388 #endif
389 
391 {
392 
393  switch (v_ctl_speed_mode) {
395  // Set pitch
396  v_ctl_set_pitch();
397  // Set throttle
399  break;
400 #if USE_AIRSPEED
402  v_ctl_set_airspeed();
403  break;
405  v_ctl_set_groundspeed();
406  v_ctl_set_airspeed();
407  break;
408 #endif
409  default:
411  break;
412  }
413 
414  // Set Pitch output
415  Bound(v_ctl_pitch_setpoint, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
416  // Set Throttle output
418 
419 }
420 
421 #ifdef V_CTL_THROTTLE_SLEW_LIMITER
422 #define V_CTL_THROTTLE_SLEW (1./CONTROL_FREQUENCY/(V_CTL_THROTTLE_SLEW_LIMITER))
423 #endif
424 
425 #ifndef V_CTL_THROTTLE_SLEW
426 #define V_CTL_THROTTLE_SLEW (1.)
427 #endif
432 {
434  BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW * MAX_PPRZ));
435  v_ctl_throttle_slewed += diff_throttle;
436 }
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:222
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:49
Core autopilot interface common to all firmwares.
bool launch
request launch
Definition: autopilot.h:71
bool kill_throttle
allow autopilot to use throttle
Definition: autopilot.h:69
uint16_t flight_time
flight time in seconds
Definition: autopilot.h:65
struct Electrical electrical
Definition: electrical.c:92
float vsupply
supply voltage in V
Definition: electrical.h:45
float v_ctl_auto_groundspeed_sum_err
Definition: energy_ctrl.c:128
float v_ctl_auto_airspeed_controlled
Definition: energy_ctrl.c:123
float v_ctl_auto_groundspeed_igain
Definition: energy_ctrl.c:127
#define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR
Definition: energy_ctrl.c:129
#define V_CTL_ALTITUDE_MAX_CLIMB
Definition: energy_ctrl.c:143
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:121
float v_ctl_auto_groundspeed_pgain
Definition: energy_ctrl.c:126
float v_ctl_auto_groundspeed_setpoint
in meters per second
Definition: energy_ctrl.c:125
#define V_CTL_SPEED_THROTTLE
Definition: energy_ctrl.h:36
#define V_CTL_SPEED_GROUNDSPEED
Definition: energy_ctrl.h:38
#define V_CTL_SPEED_AIRSPEED
Definition: energy_ctrl.h:37
#define AP_MODE_MANUAL
AP modes.
Vertical control for fixed wing vehicles.
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
void v_ctl_landing_loop(void)
Definition: guidance_v.c:357
#define V_CTL_MODE_AUTO_THROTTLE
#define V_CTL_MODE_MANUAL
#define V_CTL_AUTO_THROTTLE_STANDARD
#define V_CTL_MODE_LANDING
#define V_CTL_MODE_AUTO_ALT
#define V_CTL_CLIMB_MODE_AUTO_THROTTLE
#define V_CTL_MODE_AUTO_CLIMB
#define V_CTL_PITCH_TRIM
Definition: guidance_v_n.c:90
#define V_CTL_AUTO_CLIMB_LIMIT
Definition: guidance_v_n.c:96
float v_ctl_auto_throttle_pgain
Definition: guidance_v_n.c:59
float v_ctl_auto_pitch_igain
Definition: guidance_v_n.c:74
static void v_ctl_set_pitch(void)
Definition: guidance_v_n.c:253
pprz_t v_ctl_throttle_setpoint
Definition: guidance_v_n.c:86
#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR
Definition: guidance_v_n.c:63
float v_ctl_auto_throttle_climb_throttle_increment
Definition: guidance_v_n.c:58
#define V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN
Definition: guidance_v_n.c:68
void v_ctl_climb_loop(void)
Auto-throttle inner loop.
Definition: guidance_v_n.c:390
float v_ctl_auto_pitch_pgain
Definition: guidance_v_n.c:72
float v_ctl_auto_throttle_min_cruise_throttle
Definition: guidance_v_n.c:56
static void v_ctl_set_throttle(void)
Definition: guidance_v_n.c:281
uint8_t v_ctl_climb_mode
Definition: guidance_v_n.c:46
float v_ctl_auto_pitch_sum_err
Definition: guidance_v_n.c:75
uint8_t v_ctl_auto_throttle_submode
Definition: guidance_v_n.c:47
#define V_CTL_AUTO_PITCH_MAX_SUM_ERR
Definition: guidance_v_n.c:76
float v_ctl_auto_throttle_max_cruise_throttle
Definition: guidance_v_n.c:57
float v_ctl_auto_throttle_igain
Definition: guidance_v_n.c:60
float v_ctl_altitude_pgain
Definition: guidance_v_n.c:41
float v_ctl_pitch_setpoint
Definition: guidance_v_n.c:88
void v_ctl_guidance_loop(void)
General guidance logic This will call the proper control loops according to the sub-modes.
Definition: guidance_v_n.c:182
void v_ctl_throttle_slew(void)
Computes slewed throttle from throttle setpoint called at 20Hz.
Definition: guidance_v_n.c:431
float controlled_throttle
Definition: guidance_v_n.c:85
float v_ctl_auto_throttle_sum_err
Definition: guidance_v_n.c:62
float v_ctl_altitude_setpoint
in meters above MSL
Definition: guidance_v_n.c:39
#define V_CTL_AUTO_PITCH_IGAIN
Definition: guidance_v_n.c:82
pprz_t v_ctl_throttle_slewed
Definition: guidance_v_n.c:87
float v_ctl_auto_throttle_nominal_cruise_throttle
Definition: guidance_v_n.c:55
float v_ctl_auto_throttle_pitch_of_vz_dgain
Definition: guidance_v_n.c:65
float v_ctl_auto_pitch_dgain
Definition: guidance_v_n.c:73
#define V_CTL_AUTO_PITCH_DGAIN
Definition: guidance_v_n.c:79
uint8_t v_ctl_speed_mode
Definition: guidance_v_n.c:99
float v_ctl_auto_throttle_pitch_of_vz_pgain
Definition: guidance_v_n.c:64
void v_ctl_altitude_loop(void)
outer loop
Definition: guidance_v_n.c:230
#define V_CTL_THROTTLE_SLEW
Definition: guidance_v_n.c:426
float v_ctl_auto_throttle_cruise_throttle
Definition: guidance_v_n.c:54
void v_ctl_init(void)
Definition: guidance_v_n.c:121
float v_ctl_altitude_pre_climb
Path Angle.
Definition: guidance_v_n.c:40
float v_ctl_pitch_trim
Definition: guidance_v_n.c:92
float v_ctl_altitude_error
in meters, (setpoint - alt) -> positive = too low
Definition: guidance_v_n.c:42
float v_ctl_auto_throttle_dgain
Definition: guidance_v_n.c:61
#define V_CTL_AUTO_THROTTLE_DGAIN
Definition: guidance_v_n.c:50
float v_ctl_climb_setpoint
Definition: guidance_v_n.c:45
uint8_t v_ctl_mode
Definition: guidance_v_n.c:36
"New" vertical control for fixed wing vehicles.
float v_ctl_auto_airspeed_pitch_sum_err
float v_ctl_auto_airspeed_pitch_pgain
float v_ctl_auto_airspeed_pitch_igain
float v_ctl_auto_airspeed_throttle_dgain
float v_ctl_auto_airspeed_pitch_dgain
float v_ctl_auto_airspeed_throttle_sum_err
float v_ctl_auto_airspeed_throttle_igain
float v_ctl_auto_airspeed_throttle_pgain
pprz_t nav_throttle_setpoint
Definition: nav.c:309
float nav_pitch
Definition: nav.c:310
Fixedwing Navigation library.
#define TRIM_UPPRZ(pprz)
Definition: paparazzi.h:14
int16_t pprz_t
Definition: paparazzi.h:6
#define MAX_PPRZ
Definition: paparazzi.h:8
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
float z
in meters
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98