Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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"
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 
90 // Set higher than 2*V_CTL_ALTITUDE_MAX_CLIMB to disable
91 #ifndef V_CTL_AUTO_CLIMB_LIMIT
92 #define V_CTL_AUTO_CLIMB_LIMIT (0.5/4.0) // m/s/s
93 #endif
94 
96 
97 #if USE_AIRSPEED
112 #define V_CTL_AUTO_AIRSPEED_PITCH_MAX_SUM_ERR (RadOfDeg(15.))
113 #define V_CTL_AUTO_AIRSPEED_THROTTLE_MAX_SUM_ERR 0.2
114 #define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100
115 #endif
116 
117 void v_ctl_init( void ) {
118  /* mode */
121 
122  /* outer loop */
125  v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN;
127 
128  /* inner loops */
132 
133  /* "auto throttle" inner loop parameters */
134  v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
136  v_ctl_auto_throttle_min_cruise_throttle = V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE;
137  v_ctl_auto_throttle_max_cruise_throttle = V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE;
138  v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
139  v_ctl_auto_throttle_pgain = V_CTL_AUTO_THROTTLE_PGAIN;
140  v_ctl_auto_throttle_igain = V_CTL_AUTO_THROTTLE_IGAIN;
143  v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
145 
146  /* "auto pitch" inner loop parameters */
147  v_ctl_auto_pitch_pgain = V_CTL_AUTO_PITCH_PGAIN;
151 
152 #if USE_AIRSPEED
153  v_ctl_auto_airspeed_setpoint = V_CTL_AUTO_AIRSPEED_SETPOINT;
154  v_ctl_auto_airspeed_controlled = V_CTL_AUTO_AIRSPEED_SETPOINT;
155  v_ctl_auto_airspeed_throttle_pgain = V_CTL_AUTO_AIRSPEED_THROTTLE_PGAIN;
156  v_ctl_auto_airspeed_throttle_dgain = V_CTL_AUTO_AIRSPEED_THROTTLE_DGAIN;
157  v_ctl_auto_airspeed_throttle_igain = V_CTL_AUTO_AIRSPEED_THROTTLE_IGAIN;
159  v_ctl_auto_airspeed_pitch_pgain = V_CTL_AUTO_AIRSPEED_PITCH_PGAIN;
160  v_ctl_auto_airspeed_pitch_dgain = V_CTL_AUTO_AIRSPEED_PITCH_DGAIN;
161  v_ctl_auto_airspeed_pitch_igain = V_CTL_AUTO_AIRSPEED_PITCH_IGAIN;
163 
164  v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
165  v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
166  v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN;
168 #endif
169 
170  controlled_throttle = 0.;
172 }
173 
179 // Don't use lead controller unless you know what you're doing
180 #define LEAD_CTRL_TAU 0.8
181 #define LEAD_CTRL_A 1.
182 #define LEAD_CTRL_Te (1./60.)
183 
184 void v_ctl_altitude_loop( void ) {
185  static float v_ctl_climb_setpoint_last = 0.;
186  //static float last_lead_input = 0.;
187 
188  // Altitude error
191 
192  // Lead controller
193  //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);
194  //last_lead_input = pitch_sp;
195 
196  // Limit rate of change of climb setpoint
197  float diff_climb = v_ctl_climb_setpoint - v_ctl_climb_setpoint_last;
198  BoundAbs(diff_climb, V_CTL_AUTO_CLIMB_LIMIT);
199  v_ctl_climb_setpoint = v_ctl_climb_setpoint_last + diff_climb;
200 
201  // Limit climb setpoint
203  v_ctl_climb_setpoint_last = v_ctl_climb_setpoint;
204 }
205 
206 static inline void v_ctl_set_pitch ( void ) {
207  static float last_err = 0.;
208 
209  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0)
211 
212  // Compute errors
213  float err = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z;
214  float d_err = err - last_err;
215  last_err = err;
216 
217  if (v_ctl_auto_pitch_igain > 0.) {
218  v_ctl_auto_pitch_sum_err += err*(1./60.);
220  }
221 
222  // PI loop + feedforward ctl
225  + v_ctl_auto_pitch_pgain * err
226  + v_ctl_auto_pitch_dgain * d_err
228 
229 }
230 
231 static inline void v_ctl_set_throttle( void ) {
232  static float last_err = 0.;
233 
234  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0)
236 
237  // Compute errors
238  float err = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z;
239  float d_err = err - last_err;
240  last_err = err;
241 
242  if (v_ctl_auto_throttle_igain > 0.) {
243  v_ctl_auto_throttle_sum_err += err*(1./60.);
245  }
246 
247  // PID loop + feedforward ctl
251  + v_ctl_auto_throttle_dgain * d_err
253 
254 }
255 
256 #if USE_AIRSPEED
257 #define AIRSPEED_LOOP_PERIOD (1./60.)
258 
259 // Airspeed control loop (input: [airspeed controlled, climb_setpoint], output: [throttle controlled, pitch setpoint])
260 static inline void v_ctl_set_airspeed( void ) {
261  static float last_err_vz = 0.;
262  static float last_err_as = 0.;
263 
264  // Bound airspeed setpoint
265  Bound(v_ctl_auto_airspeed_setpoint, V_CTL_AIRSPEED_MIN, V_CTL_AIRSPEED_MAX);
266 
267  // Compute errors
268  float err_vz = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z;
269  float d_err_vz = (err_vz - last_err_vz)*AIRSPEED_LOOP_PERIOD;
270  last_err_vz = err_vz;
271  if (v_ctl_auto_throttle_igain > 0.) {
272  v_ctl_auto_throttle_sum_err += err_vz*AIRSPEED_LOOP_PERIOD;
274  }
275  if (v_ctl_auto_pitch_igain > 0.) {
276  v_ctl_auto_pitch_sum_err += err_vz*AIRSPEED_LOOP_PERIOD;
278  }
279 
280  float err_airspeed = v_ctl_auto_airspeed_setpoint - *stateGetAirspeed_f();
281  float d_err_airspeed = (err_airspeed - last_err_as)*AIRSPEED_LOOP_PERIOD;
282  last_err_as = err_airspeed;
284  v_ctl_auto_airspeed_throttle_sum_err += err_airspeed*AIRSPEED_LOOP_PERIOD;
285  BoundAbs(v_ctl_auto_airspeed_throttle_sum_err, V_CTL_AUTO_AIRSPEED_THROTTLE_MAX_SUM_ERR / v_ctl_auto_airspeed_throttle_igain);
286  }
288  v_ctl_auto_airspeed_pitch_sum_err += err_airspeed*AIRSPEED_LOOP_PERIOD;
289  BoundAbs(v_ctl_auto_airspeed_pitch_sum_err, V_CTL_AUTO_AIRSPEED_PITCH_MAX_SUM_ERR / v_ctl_auto_airspeed_pitch_igain);
290  }
291 
292 
293  // Reset integrators in manual or before flight
294  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
299  }
300 
301  // Pitch loop
304  + v_ctl_auto_pitch_pgain * err_vz
305  + v_ctl_auto_pitch_dgain * d_err_vz
307  - v_ctl_auto_airspeed_pitch_pgain * err_airspeed
308  - v_ctl_auto_airspeed_pitch_dgain * d_err_airspeed
310 
311  // Throttle loop
314  + v_ctl_auto_throttle_pgain * err_vz
315  + v_ctl_auto_throttle_dgain * d_err_vz
317  + v_ctl_auto_airspeed_throttle_pgain * err_airspeed
318  + v_ctl_auto_airspeed_throttle_dgain * d_err_airspeed
320 
321 }
322 
323 static inline void v_ctl_set_groundspeed( void ) {
324  // Ground speed control loop (input: groundspeed error, output: airspeed controlled)
326  v_ctl_auto_groundspeed_sum_err += err_groundspeed;
329 
330 }
331 #endif
332 
333 void v_ctl_climb_loop ( void ) {
334 
335  switch (v_ctl_speed_mode) {
337  // Set pitch
338  v_ctl_set_pitch();
339  // Set throttle
341  break;
342 #if USE_AIRSPEED
344  v_ctl_set_airspeed();
345  break;
347  v_ctl_set_groundspeed();
348  v_ctl_set_airspeed();
349  break;
350 #endif
351  default:
353  break;
354  }
355 
356  // Set Pitch output
357  Bound(v_ctl_pitch_setpoint, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
358  // Set Throttle output
360 
361 }
362 
363 #ifdef V_CTL_THROTTLE_SLEW_LIMITER
364 #define V_CTL_THROTTLE_SLEW (1./CONTROL_FREQUENCY/(V_CTL_THROTTLE_SLEW_LIMITER))
365 #endif
366 
367 #ifndef V_CTL_THROTTLE_SLEW
368 #define V_CTL_THROTTLE_SLEW (1.)
369 #endif
370 
373 void v_ctl_throttle_slew( void ) {
375  BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ));
376  v_ctl_throttle_slewed += diff_throttle;
377 }
float v_ctl_auto_pitch_dgain
Definition: guidance_v_n.c:73
void v_ctl_altitude_loop(void)
outer loop
Definition: guidance_v_n.c:184
float v_ctl_auto_throttle_nominal_cruise_throttle
Definition: guidance_v_n.c:55
float v_ctl_auto_airspeed_pitch_igain
static void v_ctl_set_pitch(void)
Definition: guidance_v_n.c:206
bool_t launch
Definition: sim_ap.c:40
float v_ctl_auto_pitch_pgain
Definition: guidance_v_n.c:72
float v_ctl_auto_airspeed_pitch_sum_err
float v_ctl_auto_groundspeed_setpoint
in meters per second
Definition: energy_ctrl.c:128
float v_ctl_auto_groundspeed_igain
Definition: energy_ctrl.c:130
void v_ctl_climb_loop(void)
Auto-throttle inner loop.
Definition: guidance_v_n.c:333
uint8_t pprz_mode
Definition: autopilot.c:40
int16_t pprz_t
Definition: paparazzi.h:6
static float * stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:854
#define V_CTL_ALTITUDE_MAX_CLIMB
Definition: energy_ctrl.c:141
"New" vertical control for fixed wing vehicles.
float z
in meters
float controlled_throttle
Definition: guidance_v_n.c:85
float v_ctl_auto_pitch_sum_err
Definition: guidance_v_n.c:75
float v_ctl_auto_throttle_max_cruise_throttle
Definition: guidance_v_n.c:57
uint8_t v_ctl_climb_mode
Definition: guidance_v_n.c:46
#define V_CTL_AUTO_PITCH_MAX_SUM_ERR
Definition: guidance_v_n.c:76
#define V_CTL_THROTTLE_SLEW
Definition: guidance_v_n.c:368
float v_ctl_altitude_setpoint
in meters above MSL
Definition: guidance_v_n.c:39
float v_ctl_altitude_error
in meters, (setpoint - alt) -> positive = too low
Definition: guidance_v_n.c:42
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:124
static float * stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1199
#define V_CTL_SPEED_AIRSPEED
Definition: guidance_v_n.h:32
float v_ctl_altitude_pre_climb
Path Angle.
Definition: guidance_v_n.c:40
float v_ctl_climb_setpoint
Definition: guidance_v_n.c:45
float v_ctl_auto_airspeed_throttle_sum_err
#define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR
Definition: energy_ctrl.c:132
#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:131
#define V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN
Definition: guidance_v_n.c:68
uint8_t v_ctl_speed_mode
Definition: guidance_v_n.c:95
#define V_CTL_CLIMB_MODE_AUTO_THROTTLE
Vertical control for fixed wing vehicles.
float v_ctl_auto_throttle_sum_err
Definition: guidance_v_n.c:62
float v_ctl_altitude_pgain
Definition: guidance_v_n.c:41
#define V_CTL_AUTO_THROTTLE_DGAIN
Definition: guidance_v_n.c:50
float v_ctl_auto_throttle_igain
Definition: guidance_v_n.c:60
float v_ctl_auto_airspeed_throttle_pgain
float v_ctl_auto_airspeed_throttle_dgain
#define V_CTL_AUTO_PITCH_IGAIN
Definition: guidance_v_n.c:82
uint8_t v_ctl_mode
Definition: guidance_v_n.c:36
static void v_ctl_set_throttle(void)
Definition: guidance_v_n.c:231
#define V_CTL_SPEED_GROUNDSPEED
Definition: guidance_v_n.h:33
float v_ctl_pitch_setpoint
Definition: guidance_v_n.c:88
#define V_CTL_AUTO_CLIMB_LIMIT
Definition: guidance_v_n.c:92
float v_ctl_auto_airspeed_throttle_igain
float v_ctl_auto_throttle_climb_throttle_increment
Definition: guidance_v_n.c:58
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:651
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
float v_ctl_auto_throttle_pitch_of_vz_dgain
Definition: guidance_v_n.c:65
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:840
float v_ctl_auto_throttle_pitch_of_vz_pgain
Definition: guidance_v_n.c:64
#define V_CTL_SPEED_THROTTLE
Definition: guidance_v_n.h:31
#define V_CTL_AUTO_PITCH_DGAIN
Definition: guidance_v_n.c:79
uint8_t v_ctl_auto_throttle_submode
Definition: guidance_v_n.c:47
float v_ctl_auto_throttle_cruise_throttle
Definition: guidance_v_n.c:54
void v_ctl_throttle_slew(void)
Computes slewed throttle from throttle setpoint called at 20Hz.
Definition: guidance_v_n.c:373
void v_ctl_init(void)
Definition: guidance_v_n.c:117
float v_ctl_auto_airspeed_pitch_pgain
#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR
Definition: guidance_v_n.c:63
#define MAX_PPRZ
Definition: paparazzi.h:8
#define V_CTL_MODE_MANUAL
#define PPRZ_MODE_MANUAL
AP modes.
Definition: autopilot.h:50
float v_ctl_auto_airspeed_pitch_dgain
float v_ctl_auto_throttle_pgain
Definition: guidance_v_n.c:59
float v_ctl_auto_groundspeed_pgain
Definition: energy_ctrl.c:129
pprz_t v_ctl_throttle_slewed
Definition: guidance_v_n.c:87
float v_ctl_auto_pitch_igain
Definition: guidance_v_n.c:74
float alt
in meters above WGS84 reference ellipsoid
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
float v_ctl_auto_airspeed_controlled
Definition: energy_ctrl.c:126
pprz_t v_ctl_throttle_setpoint
Definition: guidance_v_n.c:86
float v_ctl_auto_throttle_dgain
Definition: guidance_v_n.c:61
Fixedwing autopilot modes.
float v_ctl_auto_throttle_min_cruise_throttle
Definition: guidance_v_n.c:56