Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
27
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
121void 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
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
215 if (autopilot.kill_throttle || (!autopilot.flight_time && !autopilot.launch)) {
217 }
218}
219
224
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
253static 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
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
276 + v_ctl_auto_pitch_dgain * d_err
278
279}
280
281static 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
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
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])
312static 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
370 + v_ctl_auto_throttle_dgain * d_err_vz
373 + v_ctl_auto_airspeed_throttle_dgain * d_err_airspeed
375
376}
377
378static 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
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:229
struct pprz_autopilot autopilot
Global autopilot structure.
Definition autopilot.c:50
Core autopilot interface common to all firmwares.
struct Electrical electrical
Definition electrical.c:92
pprz_t v_ctl_throttle_setpoint
float v_ctl_auto_groundspeed_sum_err
float v_ctl_auto_throttle_climb_throttle_increment
float v_ctl_auto_airspeed_controlled
uint8_t v_ctl_climb_mode
Definition energy_ctrl.c:75
uint8_t v_ctl_auto_throttle_submode
Definition energy_ctrl.c:76
float v_ctl_auto_groundspeed_igain
float v_ctl_altitude_pgain
Definition energy_ctrl.c:90
float v_ctl_pitch_setpoint
float v_ctl_auto_throttle_sum_err
Definition energy_ctrl.c:77
float v_ctl_altitude_setpoint
in meters above MSL
Definition energy_ctrl.c:88
pprz_t v_ctl_throttle_slewed
#define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR
float v_ctl_auto_throttle_nominal_cruise_throttle
#define V_CTL_ALTITUDE_MAX_CLIMB
uint8_t v_ctl_speed_mode
float v_ctl_auto_throttle_pitch_of_vz_pgain
#define V_CTL_THROTTLE_SLEW
float v_ctl_auto_throttle_cruise_throttle
float v_ctl_altitude_pre_climb
Path Angle.
Definition energy_ctrl.c:89
float v_ctl_auto_airspeed_setpoint
in meters per second
float v_ctl_altitude_error
in meters, (setpoint - alt) -> positive = too low
Definition energy_ctrl.c:92
float v_ctl_auto_groundspeed_pgain
float v_ctl_auto_groundspeed_setpoint
in meters per second
float v_ctl_climb_setpoint
Definition energy_ctrl.c:98
uint8_t v_ctl_mode
Definition energy_ctrl.c:74
#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.
#define V_CTL_PITCH_TRIM
Definition guidance_v.c:93
float v_ctl_auto_throttle_pgain
Definition guidance_v.c:61
float v_ctl_auto_pitch_igain
Definition guidance_v.c:85
#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR
Definition guidance_v.c:65
#define V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN
Definition guidance_v.c:70
float v_ctl_auto_pitch_pgain
Definition guidance_v.c:84
float v_ctl_auto_throttle_min_cruise_throttle
Definition guidance_v.c:58
float v_ctl_auto_pitch_sum_err
Definition guidance_v.c:86
#define V_CTL_AUTO_PITCH_MAX_SUM_ERR
Definition guidance_v.c:87
float v_ctl_auto_throttle_max_cruise_throttle
Definition guidance_v.c:59
float v_ctl_auto_throttle_igain
Definition guidance_v.c:62
float v_ctl_auto_throttle_pitch_of_vz_dgain
Definition guidance_v.c:67
float v_ctl_pitch_trim
Definition guidance_v.c:95
float v_ctl_auto_throttle_dgain
Definition guidance_v.c:63
#define V_CTL_AUTO_THROTTLE_DGAIN
Definition guidance_v.c:52
Vertical control for fixed wing vehicles.
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition state.h:832
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition state.h:1087
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition state.h:1069
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition state.h:1615
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_AUTO_CLIMB_LIMIT
static void v_ctl_set_pitch(void)
void v_ctl_climb_loop(void)
Auto-throttle inner loop.
static void v_ctl_set_throttle(void)
void v_ctl_guidance_loop(void)
General guidance logic This will call the proper control loops according to the sub-modes.
void v_ctl_throttle_slew(void)
Computes slewed throttle from throttle setpoint called at 20Hz.
float controlled_throttle
#define V_CTL_AUTO_PITCH_IGAIN
float v_ctl_auto_pitch_dgain
#define V_CTL_AUTO_PITCH_DGAIN
void v_ctl_altitude_loop(void)
outer loop
void v_ctl_init(void)
"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.