Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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"
32 #include "autopilot.h"
33 #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" //> allow for roll control during landing final flare
34 
35 /* mode */
37 
38 /* outer loop */
45 
46 /* inner loop */
50 
51 #ifndef V_CTL_AUTO_THROTTLE_DGAIN
52 #define V_CTL_AUTO_THROTTLE_DGAIN 0.
53 #endif
54 
55 /* "auto throttle" inner loop parameters */
65 #define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 150
68 
69 #ifndef V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN
70 #define V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN 0.
71 #endif
72 
73 /* agressive tuning */
74 #ifdef TUNE_AGRESSIVE_CLIMB
75 float agr_climb_throttle;
76 float agr_climb_pitch;
77 float agr_climb_nav_ratio;
78 float agr_descent_throttle;
79 float agr_descent_pitch;
80 float agr_descent_nav_ratio;
81 #endif
82 
83 /* "auto pitch" inner loop parameters */
87 #define V_CTL_AUTO_PITCH_MAX_SUM_ERR 100
88 
92 #ifndef V_CTL_PITCH_TRIM
93 #define V_CTL_PITCH_TRIM 0.
94 #endif
96 
97 inline static void v_ctl_climb_auto_throttle_loop(void);
98 #ifdef V_CTL_AUTO_PITCH_PGAIN
99 inline static void v_ctl_climb_auto_pitch_loop(void);
100 #endif
101 
102 #if USE_AIRSPEED
105 float v_ctl_auto_airspeed_pgain;
106 float v_ctl_auto_airspeed_igain;
107 float v_ctl_auto_airspeed_sum_err;
112 #define V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR 200
113 #define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100
114 #define V_CTL_AUTO_CLIMB_LIMIT 0.5/4.0 // m/s/s
115 #define V_CTL_AUTO_AGR_CLIMB_GAIN 2.0 // altitude gain multiplier while in aggressive climb mode
116 #endif
117 
118 #ifndef V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION
119 #define V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION 1.0f
120 #endif
121 
122 #if CTRL_VERTICAL_LANDING
123 #ifndef V_CTL_LANDING_THROTTLE_PGAIN
124 #error "V_CTL_LANDING_THROTTLE_PGAIN undefined, please define it in your airfame config file"
125 #endif
126 #ifndef V_CTL_LANDING_THROTTLE_IGAIN
127 #error "V_CTL_LANDING_THROTTLE_IGAIN undefined, please define it in your airfame config file"
128 #endif
129 #ifndef V_CTL_LANDING_THROTTLE_MAX
130 INFO("V_CTL_LANDING_THROTTLE_MAX undefined, using V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE instead")
131 #define V_CTL_LANDING_THROTTLE_MAX V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE
132 #endif
133 #ifndef V_CTL_LANDING_DESIRED_SPEED
134 #error "V_CTL_LANDING_DESIRED_SPEED undefined, please define it in your airfame config file"
135 #endif
136 
137 
138 #ifndef V_CTL_LANDING_PITCH_PGAIN
139 INFO("V_CTL_LANDING_PITCH_PGAIN undefined, using V_CTL_AUTO_PITCH_PGAIN instead")
140 #define V_CTL_LANDING_PITCH_PGAIN V_CTL_AUTO_PITCH_PGAIN
141 #endif
142 #ifndef V_CTL_LANDING_PITCH_IGAIN
143 INFO("V_CTL_LANDING_PITCH_IGAIN undefined, using V_CTL_AUTO_PITCH_IGAIN instead")
144 #define V_CTL_LANDING_PITCH_IGAIN V_CTL_AUTO_PITCH_IGAIN
145 #endif
146 #ifndef V_CTL_LANDING_PITCH_LIMITS
147 INFO("V_CTL_LANDING_PITCH_LIMITS undefined, using V_CTL_AUTO_PITCH_MAX_PITCH instead")
148 #define V_CTL_LANDING_PITCH_LIMITS V_CTL_AUTO_PITCH_MAX_PITCH
149 #endif
150 #ifndef V_CTL_LANDING_PITCH_FLARE
151 #error "V_CTL_LANDING_PITCH_FLARE undefined, please define it in your airfame config file"
152 #endif
153 #ifndef V_CTL_LANDING_ALT_THROTTLE_KILL
154 #error "V_CTL_LANDING_ALT_THROTTLE_KILL undefined, please define it in your airfame config file"
155 #endif
156 #ifndef V_CTL_LANDING_ALT_FLARE
157 #error "V_CTL_LANDING_ALT_FLARE undefined, please define it in your airfame config file"
158 #endif
159 
160 float v_ctl_landing_throttle_pgain;
161 float v_ctl_landing_throttle_igain;
162 float v_ctl_landing_throttle_max;
163 float v_ctl_landing_desired_speed;
164 float v_ctl_landing_pitch_pgain;
165 float v_ctl_landing_pitch_igain;
166 float v_ctl_landing_pitch_limits;
167 float v_ctl_landing_pitch_flare;
168 float v_ctl_landing_alt_throttle_kill; //> must be greater than alt_flare
169 float v_ctl_landing_alt_flare;
170 #endif /* CTRL_VERTICAL_LANDING */
171 
172 void v_ctl_init(void)
173 {
174  /* mode */
176 
177 #if CTRL_VERTICAL_LANDING
178  /* improved landing routine */
179  v_ctl_landing_throttle_pgain = V_CTL_LANDING_THROTTLE_PGAIN;
180  v_ctl_landing_throttle_igain = V_CTL_LANDING_THROTTLE_IGAIN;
181  v_ctl_landing_throttle_max = V_CTL_LANDING_THROTTLE_MAX;
182  v_ctl_landing_desired_speed = V_CTL_LANDING_DESIRED_SPEED;
183  v_ctl_landing_pitch_pgain = V_CTL_LANDING_PITCH_PGAIN;
184  v_ctl_landing_pitch_igain = V_CTL_LANDING_PITCH_IGAIN;
185  v_ctl_landing_pitch_limits = V_CTL_LANDING_PITCH_LIMITS;
186  v_ctl_landing_pitch_flare = V_CTL_LANDING_PITCH_FLARE;
187  v_ctl_landing_alt_throttle_kill = V_CTL_LANDING_ALT_THROTTLE_KILL;
188  v_ctl_landing_alt_flare = V_CTL_LANDING_ALT_FLARE;
189 #endif /* CTRL_VERTICAL_LANDING */
190 
191  /* outer loop */
194  v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN;
198 
199  /* inner loops */
203 
206 
207  /* "auto throttle" inner loop parameters */
208  v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
209  v_ctl_auto_throttle_min_cruise_throttle = V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE;
210  v_ctl_auto_throttle_max_cruise_throttle = V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE;
212  v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
213  v_ctl_auto_throttle_pgain = V_CTL_AUTO_THROTTLE_PGAIN;
214  v_ctl_auto_throttle_igain = V_CTL_AUTO_THROTTLE_IGAIN;
217  v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
219 
220 #ifdef V_CTL_AUTO_PITCH_PGAIN
221  /* "auto pitch" inner loop parameters */
222  v_ctl_auto_pitch_pgain = V_CTL_AUTO_PITCH_PGAIN;
225 #endif
226 
227 #if USE_AIRSPEED
228  v_ctl_auto_airspeed_setpoint = V_CTL_AUTO_AIRSPEED_SETPOINT;
229  v_ctl_auto_airspeed_controlled = V_CTL_AUTO_AIRSPEED_SETPOINT;
230  v_ctl_auto_airspeed_pgain = V_CTL_AUTO_AIRSPEED_PGAIN;
231  v_ctl_auto_airspeed_igain = V_CTL_AUTO_AIRSPEED_IGAIN;
232  v_ctl_auto_airspeed_sum_err = 0.;
233 
234  v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
235  v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
236  v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN;
238 #endif
239 
241 
242  /*agressive tuning*/
243 #ifdef TUNE_AGRESSIVE_CLIMB
244  agr_climb_throttle = AGR_CLIMB_THROTTLE;
245 #undef AGR_CLIMB_THROTTLE
246 #define AGR_CLIMB_THROTTLE agr_climb_throttle
247  agr_climb_pitch = AGR_CLIMB_PITCH;
248 #undef AGR_CLIMB_PITCH
249 #define AGR_CLIMB_PITCH agr_climb_pitch
250  agr_climb_nav_ratio = AGR_CLIMB_NAV_RATIO;
251 #undef AGR_CLIMB_NAV_RATIO
252 #define AGR_CLIMB_NAV_RATIO agr_climb_nav_ratio
253  agr_descent_throttle = AGR_DESCENT_THROTTLE;
254 #undef AGR_DESCENT_THROTTLE
255 #define AGR_DESCENT_THROTTLE agr_descent_throttle
256  agr_descent_pitch = AGR_DESCENT_PITCH;
257 #undef AGR_DESCENT_PITCH
258 #define AGR_DESCENT_PITCH agr_descent_pitch
259  agr_descent_nav_ratio = AGR_DESCENT_NAV_RATIO;
260 #undef AGR_DESCENT_NAV_RATIO
261 #define AGR_DESCENT_NAV_RATIO agr_descent_nav_ratio
262 #endif
263 }
264 
266 {
269  }
270 #if CTRL_VERTICAL_LANDING
273  } else {
274 #endif
278  } else {
281  } /* v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB */
282  } /* v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE */
283 #if CTRL_VERTICAL_LANDING
284  } /* v_ctl_mode == V_CTL_MODE_LANDING */
285 #endif
286 
287 #if defined V_CTL_THROTTLE_IDLE
288  Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE * MAX_PPRZ), MAX_PPRZ);
289 #endif
290 
291 #ifdef V_CTL_POWER_CTL_BAT_NOMINAL
292  if (electrical.vsupply > 0.) {
293  v_ctl_throttle_setpoint *= V_CTL_POWER_CTL_BAT_NOMINAL / electrical.vsupply;
295  }
296 #endif
297 
300  }
301 }
302 
303 
309 {
310  float altitude_pgain_boost = 1.0;
311 
312 #if USE_AIRSPEED && defined(AGR_CLIMB)
313  // Aggressive climb mode (boost gain of altitude loop)
315  float dist = fabs(v_ctl_altitude_error);
316  altitude_pgain_boost = 1.0 + (V_CTL_AUTO_AGR_CLIMB_GAIN - 1.0) * (dist - AGR_BLEND_END) /
317  (AGR_BLEND_START - AGR_BLEND_END);
318  Bound(altitude_pgain_boost, 1.0, V_CTL_AUTO_AGR_CLIMB_GAIN);
319  }
320 #endif
321 
326 
327 #ifdef AGR_CLIMB
329  float dist = fabs(v_ctl_altitude_error);
330  if (dist < AGR_BLEND_END) {
332  } else if (dist > AGR_BLEND_START) {
334  } else {
336  }
337  }
338 #endif
339 }
340 
342 {
343  switch (v_ctl_climb_mode) {
345  default:
347  break;
348 #ifdef V_CTL_AUTO_PITCH_PGAIN
349 #pragma message "AUTO PITCH Enabled!"
351  v_ctl_climb_auto_pitch_loop();
352  break;
353 #endif
354  }
355 }
356 
358 {
359 #if CTRL_VERTICAL_LANDING
360  static float land_speed_i_err;
361  static float land_alt_i_err;
362  static float kill_alt;
363  float land_speed_err = v_ctl_landing_desired_speed - stateGetHorizontalSpeedNorm_f();
364  float land_alt_err = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt;
365 
367  && (kill_alt - v_ctl_altitude_setpoint)
368  > (v_ctl_landing_alt_throttle_kill - v_ctl_landing_alt_flare)) {
369  v_ctl_throttle_setpoint = 0.0; // Throttle is already in KILL (command redundancy)
370  nav_pitch = v_ctl_landing_pitch_flare; // desired final flare pitch
371  lateral_mode = LATERAL_MODE_ROLL; //override course correction during flare - eliminate possibility of catching wing tip due to course correction
372  h_ctl_roll_setpoint = 0.0; // command zero roll during flare
373  } else {
374  // set throttle based on altitude error
375  v_ctl_throttle_setpoint = land_alt_err * v_ctl_landing_throttle_pgain
376  + land_alt_i_err * v_ctl_landing_throttle_igain;
377  Bound(v_ctl_throttle_setpoint, 0, v_ctl_landing_throttle_max * MAX_PPRZ); // cut off throttle cmd at specified MAX
378 
379  land_alt_i_err += land_alt_err / CONTROL_FREQUENCY; // integrator land_alt_err, divide by control frequency
380  BoundAbs(land_alt_i_err, 50); // integrator sat limits
381 
382  // set pitch based on ground speed error
383  nav_pitch -= land_speed_err * v_ctl_landing_pitch_pgain / 1000
384  + land_speed_i_err * v_ctl_landing_pitch_igain / 1000; // 1000 is a multiplier to get more reasonable gains for ctl_basic
385  Bound(nav_pitch, -v_ctl_landing_pitch_limits, v_ctl_landing_pitch_limits); //max pitch authority for landing
386 
387  land_speed_i_err += land_speed_err / CONTROL_FREQUENCY; // integrator land_speed_err, divide by control frequency
388  BoundAbs(land_speed_i_err, .2); // integrator sat limits
389 
390  // update kill_alt until final kill throttle is initiated - allows for mode switch to first part of if statement above
391  // eliminates the need for knowing the altitude of TD
392  if (!autopilot_throttle_killed()) {
393  kill_alt = v_ctl_altitude_setpoint; //
394  if (land_alt_err > 0.0) {
395  nav_pitch = 0.01; // if below desired alt close to ground command level pitch
396  }
397  }
398  }
399 #endif /* CTRL_VERTICAL_LANDING */
400 }
401 
407 #if !USE_AIRSPEED
408 
409 inline static void v_ctl_climb_auto_throttle_loop(void)
410 {
411  static float last_err;
412 
413  float f_throttle = 0;
414  float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint;
415  float d_err = err - last_err;
416  last_err = err;
421  + v_ctl_auto_throttle_dgain * d_err);
422 
423  /* pitch pre-command */
424  float v_ctl_pitch_of_vz = (v_ctl_climb_setpoint + d_err * v_ctl_auto_throttle_pitch_of_vz_dgain) *
426 
427 #if defined AGR_CLIMB
428  switch (v_ctl_auto_throttle_submode) {
430  if (v_ctl_climb_setpoint > 0) { /* Climbing */
431  f_throttle = AGR_CLIMB_THROTTLE;
432  v_ctl_pitch_setpoint = AGR_CLIMB_PITCH;
433  } else { /* Going down */
434  f_throttle = AGR_DESCENT_THROTTLE;
435  v_ctl_pitch_setpoint = AGR_DESCENT_PITCH;
436  }
437  break;
438 
440  float ratio = (fabs(v_ctl_altitude_error) - AGR_BLEND_END)
441  / (AGR_BLEND_START - AGR_BLEND_END);
442  f_throttle = (1 - ratio) * controlled_throttle;
443  v_ctl_pitch_setpoint = (1 - ratio) * (v_ctl_pitch_of_vz + v_ctl_pitch_trim);
444  v_ctl_auto_throttle_sum_err += (1 - ratio) * err;
446  /* positive error -> too low */
447  if (v_ctl_altitude_error > 0) {
448  f_throttle += ratio * AGR_CLIMB_THROTTLE;
449  v_ctl_pitch_setpoint += ratio * AGR_CLIMB_PITCH;
450  } else {
451  f_throttle += ratio * AGR_DESCENT_THROTTLE;
452  v_ctl_pitch_setpoint += ratio * AGR_DESCENT_PITCH;
453  }
454  break;
455  }
456 
458  default:
459 #endif
460  f_throttle = controlled_throttle;
463  v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + v_ctl_pitch_trim + nav_pitch;
464 #if defined AGR_CLIMB
465  break;
466  } /* switch submode */
467 #endif
468 
470 }
471 
472 #else // USE_AIRSPEED
473 
474 inline static void v_ctl_climb_auto_throttle_loop(void)
475 {
476  float f_throttle = 0;
477  float controlled_throttle;
478  float v_ctl_pitch_of_vz;
479 
480  // Limit rate of change of climb setpoint (to ensure that airspeed loop can catch-up)
481  static float v_ctl_climb_setpoint_last = 0;
482  float diff_climb = v_ctl_climb_setpoint - v_ctl_climb_setpoint_last;
483  Bound(diff_climb, -V_CTL_AUTO_CLIMB_LIMIT, V_CTL_AUTO_CLIMB_LIMIT);
484  v_ctl_climb_setpoint = v_ctl_climb_setpoint_last + diff_climb;
485  v_ctl_climb_setpoint_last = v_ctl_climb_setpoint;
486 
487  // Pitch control (input: rate of climb error, output: pitch setpoint)
488  float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint;
491  v_ctl_pitch_of_vz = -v_ctl_auto_pitch_pgain *
493 
494  // Ground speed control loop (input: groundspeed error, output: airspeed controlled)
496  v_ctl_auto_groundspeed_sum_err += err_groundspeed;
500 
501  // Do not allow controlled airspeed below the setpoint
505  v_ctl_auto_groundspeed_igain); // reset integrator of ground speed loop
506  }
507 
508  // Airspeed control loop (input: airspeed controlled, output: throttle controlled)
509  float err_airspeed = (v_ctl_auto_airspeed_controlled - stateGetAirspeed_f());
510  v_ctl_auto_airspeed_sum_err += err_airspeed;
511  BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR);
512  controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * v_ctl_auto_airspeed_igain) *
513  v_ctl_auto_airspeed_pgain;
514 
515  // Done, set outputs
517  f_throttle = controlled_throttle;
518  v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + v_ctl_pitch_trim;
520  Bound(v_ctl_pitch_setpoint, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
521 }
522 
523 #endif // USE_AIRSPEED
524 
525 
530 #ifdef V_CTL_AUTO_PITCH_PGAIN
531 inline static void v_ctl_climb_auto_pitch_loop(void)
532 {
533  float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint;
539  Bound(v_ctl_pitch_setpoint, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
540 }
541 #endif
542 
543 #ifdef V_CTL_THROTTLE_SLEW_LIMITER
544 #define V_CTL_THROTTLE_SLEW (1./CONTROL_FREQUENCY/(V_CTL_THROTTLE_SLEW_LIMITER))
545 #endif
546 
547 #ifndef V_CTL_THROTTLE_SLEW
548 #define V_CTL_THROTTLE_SLEW 1.
549 #endif
554 {
556  BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW * MAX_PPRZ));
557  v_ctl_throttle_slewed += diff_throttle;
558 }
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:49
bool autopilot_throttle_killed(void)
get kill status
Definition: autopilot.c:313
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
uint8_t lateral_mode
#define LATERAL_MODE_ROLL
#define CONTROL_FREQUENCY
#define V_CTL_PITCH_TRIM
Definition: guidance_v.c:93
float v_ctl_altitude_pre_climb_correction
Definition: guidance_v.c:43
void v_ctl_landing_loop(void)
Definition: guidance_v.c:357
float v_ctl_auto_throttle_pgain
Definition: guidance_v.c:61
float v_ctl_auto_pitch_igain
Definition: guidance_v.c:85
pprz_t v_ctl_throttle_setpoint
Definition: guidance_v.c:89
#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR
Definition: guidance_v.c:65
float v_ctl_auto_throttle_climb_throttle_increment
Definition: guidance_v.c:60
#define V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN
Definition: guidance_v.c:70
void v_ctl_climb_loop(void)
Auto-throttle inner loop.
Definition: guidance_v.c:341
float v_ctl_auto_pitch_pgain
Definition: guidance_v.c:84
float v_ctl_auto_throttle_min_cruise_throttle
Definition: guidance_v.c:58
uint8_t v_ctl_climb_mode
Definition: guidance_v.c:48
float v_ctl_auto_pitch_sum_err
Definition: guidance_v.c:86
uint8_t v_ctl_auto_throttle_submode
Definition: guidance_v.c:49
#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_altitude_pgain
Definition: guidance_v.c:41
float v_ctl_pitch_setpoint
Definition: guidance_v.c:91
void v_ctl_guidance_loop(void)
General guidance logic This will call the proper control loops according to the sub-modes.
Definition: guidance_v.c:265
void v_ctl_throttle_slew(void)
Computes slewed throttle from throttle setpoint called at 20Hz.
Definition: guidance_v.c:553
float v_ctl_altitude_max_climb
Definition: guidance_v.c:44
float v_ctl_auto_throttle_sum_err
Definition: guidance_v.c:64
float v_ctl_altitude_setpoint
in meters above MSL
Definition: guidance_v.c:39
pprz_t v_ctl_throttle_slewed
Definition: guidance_v.c:90
float v_ctl_auto_throttle_nominal_cruise_throttle
Definition: guidance_v.c:57
float v_ctl_auto_throttle_pitch_of_vz_dgain
Definition: guidance_v.c:67
float v_ctl_auto_throttle_pitch_of_vz_pgain
Definition: guidance_v.c:66
void v_ctl_altitude_loop(void)
outer loop
Definition: guidance_v.c:308
#define V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION
Definition: guidance_v.c:119
#define V_CTL_THROTTLE_SLEW
auto pitch inner loop
Definition: guidance_v.c:548
float v_ctl_auto_throttle_cruise_throttle
Definition: guidance_v.c:56
void v_ctl_init(void)
Definition: guidance_v.c:172
float v_ctl_altitude_pre_climb
Path Angle.
Definition: guidance_v.c:40
float v_ctl_pitch_trim
Definition: guidance_v.c:95
float v_ctl_altitude_error
in meters, (setpoint - alt) -> positive = too low
Definition: guidance_v.c:42
static void v_ctl_climb_auto_throttle_loop(void)
auto throttle inner loop
Definition: guidance_v.c:409
float v_ctl_auto_throttle_dgain
Definition: guidance_v.c:63
#define V_CTL_AUTO_THROTTLE_DGAIN
Definition: guidance_v.c:52
float v_ctl_climb_setpoint
Definition: guidance_v.c:47
uint8_t v_ctl_mode
Definition: guidance_v.c:36
Vertical control for fixed wing vehicles.
Fixed wing horizontal control.
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
#define V_CTL_MODE_AUTO_THROTTLE
#define V_CTL_MODE_MANUAL
#define V_CTL_AUTO_THROTTLE_AGRESSIVE
#define V_CTL_CLIMB_MODE_AUTO_PITCH
#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_THROTTLE_BLENDED
#define V_CTL_AUTO_CLIMB_LIMIT
Definition: guidance_v_n.c:96
float controlled_throttle
Definition: guidance_v_n.c:85
#define V_CTL_AUTO_PITCH_IGAIN
Definition: guidance_v_n.c:82
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
float h_ctl_roll_setpoint
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98