Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
guidance_v.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
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 #include "generated/airframe.h"
30 
35 
36 #include "state.h"
37 
38 #include "math/pprz_algebra_int.h"
39 
40 
41 /* error if some gains are negative */
42 #if (GUIDANCE_V_HOVER_KP < 0) || \
43  (GUIDANCE_V_HOVER_KD < 0) || \
44  (GUIDANCE_V_HOVER_KI < 0)
45 #error "ALL control gains must be positive!!!"
46 #endif
47 
48 
49 /* If only GUIDANCE_V_NOMINAL_HOVER_THROTTLE is defined,
50  * disable the adaptive throttle estimation by default.
51  * Otherwise enable adaptive estimation by default.
52  */
53 #ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
54 # ifndef GUIDANCE_V_ADAPT_THROTTLE_ENABLED
55 # define GUIDANCE_V_ADAPT_THROTTLE_ENABLED FALSE
56 # endif
57 #else
58 # define GUIDANCE_V_NOMINAL_HOVER_THROTTLE 0.4
59 # ifndef GUIDANCE_V_ADAPT_THROTTLE_ENABLED
60 # define GUIDANCE_V_ADAPT_THROTTLE_ENABLED TRUE
61 # endif
62 #endif
63 PRINT_CONFIG_VAR(GUIDANCE_V_NOMINAL_HOVER_THROTTLE)
64 PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_THROTTLE_ENABLED)
65 
66 
67 #ifndef GUIDANCE_V_CLIMB_RC_DEADBAND
68 #define GUIDANCE_V_CLIMB_RC_DEADBAND MAX_PPRZ/10
69 #endif
70 
71 #ifndef GUIDANCE_V_MAX_RC_CLIMB_SPEED
72 #define GUIDANCE_V_MAX_RC_CLIMB_SPEED GUIDANCE_V_REF_MIN_ZD
73 #endif
74 
75 #ifndef GUIDANCE_V_MAX_RC_DESCENT_SPEED
76 #define GUIDANCE_V_MAX_RC_DESCENT_SPEED GUIDANCE_V_REF_MAX_ZD
77 #endif
78 
79 #ifndef GUIDANCE_V_MIN_ERR_Z
80 #define GUIDANCE_V_MIN_ERR_Z POS_BFP_OF_REAL(-10.)
81 #endif
82 
83 #ifndef GUIDANCE_V_MAX_ERR_Z
84 #define GUIDANCE_V_MAX_ERR_Z POS_BFP_OF_REAL(10.)
85 #endif
86 
87 #ifndef GUIDANCE_V_MIN_ERR_ZD
88 #define GUIDANCE_V_MIN_ERR_ZD SPEED_BFP_OF_REAL(-10.)
89 #endif
90 
91 #ifndef GUIDANCE_V_MAX_ERR_ZD
92 #define GUIDANCE_V_MAX_ERR_ZD SPEED_BFP_OF_REAL(10.)
93 #endif
94 
95 #ifndef GUIDANCE_V_MAX_SUM_ERR
96 #define GUIDANCE_V_MAX_SUM_ERR 2000000
97 #endif
98 
99 #ifndef GUIDANCE_V_MAX_CMD
100 #define GUIDANCE_V_MAX_CMD 0.9*MAX_PPRZ
101 #endif
102 
107 
110 static bool desired_zd_updated;
111 
112 #define GUIDANCE_V_GUIDED_MODE_ZHOLD 0
113 #define GUIDANCE_V_GUIDED_MODE_CLIMB 1
114 #define GUIDANCE_V_GUIDED_MODE_THROTTLE 2
115 
117 
122 
128 
135 
139 
141 
143 
144 
146 
147 #if PERIODIC_TELEMETRY
149 
150 static void send_vert_loop(struct transport_tx *trans, struct link_device *dev)
151 {
152  pprz_msg_send_VERT_LOOP(trans, dev, AC_ID,
154  &(stateGetPositionNed_i()->z),
155  &(stateGetSpeedNed_i()->z),
156  &(stateGetAccelNed_i()->z),
159  &gv_adapt_X,
160  &gv_adapt_P,
166 }
167 
168 static void send_tune_vert(struct transport_tx *trans, struct link_device *dev)
169 {
170  pprz_msg_send_TUNE_VERT(trans, dev, AC_ID,
172  &(stateGetPositionNed_i()->z),
175 }
176 #endif
177 
178 void guidance_v_init(void)
179 {
180 
182 
183  guidance_v_kp = GUIDANCE_V_HOVER_KP;
184  guidance_v_kd = GUIDANCE_V_HOVER_KD;
185  guidance_v_ki = GUIDANCE_V_HOVER_KI;
186 
188 
191  desired_zd_updated = false;
193 
195 
196  gv_adapt_init();
197 
198 #if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
200 #endif
201 
202 #if PERIODIC_TELEMETRY
205 #endif
206 }
207 
208 
210 {
211 
212  /* used in RC_DIRECT directly and as saturation in CLIMB and HOVER */
214 
215  /* used in RC_CLIMB */
218 
219  static const int32_t climb_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_CLIMB_SPEED) /
221  static const int32_t descent_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_DESCENT_SPEED) /
223 
224  if (guidance_v_rc_zd_sp > 0) {
225  guidance_v_rc_zd_sp *= descent_scale;
226  } else {
227  guidance_v_rc_zd_sp *= climb_scale;
228  }
229 }
230 
232 {
233 
234  if (new_mode == guidance_v_mode) {
235  return;
236  }
237 
238  switch (new_mode) {
242  break;
243 
246  guidance_v_zd_sp = 0;
247  /* Falls through. */
248  case GUIDANCE_V_MODE_NAV:
251  break;
252 
253 #if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
256  break;
257 #endif
258 
260  break;
261 
262  default:
263  break;
264 
265  }
266 
267  guidance_v_mode = new_mode;
268 
269 }
270 
271 void guidance_v_notify_in_flight(bool in_flight)
272 {
273  if (in_flight) {
274  gv_adapt_init();
275  }
276 }
277 
278 void guidance_v_thrust_adapt(bool in_flight)
279 {
281 
282  if (in_flight) {
283  /* Only run adaptive throttle estimation if we are in flight and
284  * the desired vertical velocity (zd) was updated (i.e. we ran hover_loop before).
285  * This means that the estimation is not updated when using direct throttle commands.
286  *
287  * FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT, AKA SUPERVISION and co
288  */
289  if (desired_zd_updated) {
290  int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC;
291  gv_adapt_run(stateGetAccelNed_i()->z, vertical_thrust, guidance_v_zd_ref);
292  }
293  } else {
294  /* reset estimate while not in_flight */
295  gv_adapt_init();
296  }
297 }
298 
299 void guidance_v_run(bool in_flight)
300 {
301  guidance_v_thrust_adapt(in_flight);
302 
303  /* reset flag indicating if desired zd was updated */
304  desired_zd_updated = false;
305 
306  switch (guidance_v_mode) {
307 
309  guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only
310  stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t;
311  break;
312 
316  run_hover_loop(in_flight);
317  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
318  break;
319 
322  run_hover_loop(in_flight);
323 #if !NO_RC_THRUST_LIMIT
324  /* use rc limitation if available */
325  if (radio_control.status == RC_OK) {
327  } else
328 #endif
329  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
330  break;
331 
334  /* Falls through. */
336  guidance_v_guided_run(in_flight);
337  break;
338 
339 #if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
341  guidance_v_module_run(in_flight);
342  break;
343 #endif
344 
345  case GUIDANCE_V_MODE_NAV: {
346  guidance_v_from_nav(in_flight);
347  break;
348  }
349 
351  break;
352 
353  default:
354  break;
355  }
356 }
357 
358 
360 {
361  /* set current altitude as setpoint */
363 
364  /* reset guidance reference */
367 
368  /* reset speed setting */
369  guidance_v_zd_sp = 0;
370 }
371 
372 void guidance_v_set_ref(int32_t pos, int32_t speed, int32_t accel)
373 {
374  gv_set_ref(pos, speed, accel);
375  guidance_v_z_ref = pos;
376  guidance_v_zd_ref = speed;
377  guidance_v_zdd_ref = accel;
378 }
379 
380 
383 {
384  // cos(30°) = 0.8660254
385  static const int32_t max_bank_coef = BFP_OF_REAL(0.8660254f, INT32_TRIG_FRAC);
386 
387  struct Int32RMat *att = stateGetNedToBodyRMat_i();
388  /* thrust vector:
389  * int32_rmat_vmult(&thrust_vect, &att, &zaxis)
390  * same as last colum of rmat with INT32_TRIG_FRAC
391  * struct Int32Vect thrust_vect = {att.m[2], att.m[5], att.m[8]};
392  *
393  * Angle between two vectors v1 and v2:
394  * angle = acos(dot(v1, v2) / (norm(v1) * norm(v2)))
395  * since here both are already of unit length:
396  * angle = acos(dot(v1, v2))
397  * since we we want the cosine of the angle we simply need
398  * thrust_coeff = dot(v1, v2)
399  * also can be simplified considering: v1 is zaxis with (0,0,1)
400  * dot(v1, v2) = v1.z * v2.z = v2.z
401  */
402  int32_t coef = att->m[8];
403  if (coef < max_bank_coef) {
404  coef = max_bank_coef;
405  }
406  return coef;
407 }
408 
409 
410 #define FF_CMD_FRAC 18
411 
412 void run_hover_loop(bool in_flight)
413 {
414 
415  /* convert our reference to generic representation */
416  int64_t tmp = gv_z_ref >> (GV_Z_REF_FRAC - INT32_POS_FRAC);
417  guidance_v_z_ref = (int32_t)tmp;
420  /* set flag to indicate that desired zd was updated */
421  desired_zd_updated = true;
422  /* compute the error to our reference */
427 
428  if (in_flight) {
429  guidance_v_z_sum_err += err_z;
431  } else {
433  }
434 
435  /* our nominal command : (g + zdd)*m */
436  int32_t inv_m;
438  inv_m = gv_adapt_X >> (GV_ADAPT_X_FRAC - FF_CMD_FRAC);
439  } else {
440  /* use the fixed nominal throttle */
442  }
443 
444  const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) -
446 
447  guidance_v_ff_cmd = g_m_zdd / inv_m;
448  /* feed forward command */
450 
451 #if HYBRID_NAVIGATION
452  //FIXME: NOT USING FEEDFORWARD COMMAND BECAUSE OF QUADSHOT NAVIGATION
454 #endif
455 
456  /* bound the nominal command to GUIDANCE_V_MAX_CMD */
458 
459 
460  /* our error feed back command */
461  /* z-axis pointing down -> positive error means we need less thrust */
462  guidance_v_fb_cmd = ((-guidance_v_kp * err_z) >> 7) +
463  ((-guidance_v_kd * err_zd) >> 16) +
464  ((-guidance_v_ki * guidance_v_z_sum_err) >> 16);
465 
467 
468  /* bound the result */
469  Bound(guidance_v_delta_t, 0, MAX_PPRZ);
470 
471 }
472 
473 void guidance_v_from_nav(bool in_flight)
474 {
477  guidance_v_zd_sp = 0;
479  run_hover_loop(in_flight);
480  } else if (vertical_mode == VERTICAL_MODE_CLIMB) {
484  run_hover_loop(in_flight);
485  } else if (vertical_mode == VERTICAL_MODE_MANUAL) {
491  } else if (vertical_mode == VERTICAL_MODE_GUIDED) {
492  guidance_v_guided_run(in_flight);
493  }
494 #if HYBRID_NAVIGATION
496 #else
497 #if !NO_RC_THRUST_LIMIT
498  /* use rc limitation if available */
499  if (radio_control.status == RC_OK) {
501  } else
502 #endif
503  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
504 #endif
505 }
506 
508 {
509  /* set current altitude as setpoint */
511 
512  /* reset guidance reference */
515 }
516 
517 void guidance_v_guided_run(bool in_flight)
518 {
519  switch(guidance_v_guided_mode)
520  {
522  // Altitude Hold
523  guidance_v_zd_sp = 0;
525  run_hover_loop(in_flight);
526  break;
528  // Climb
530  run_hover_loop(in_flight);
531  break;
533  // Throttle
534  guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only
536  break;
537  default:
538  break;
539  }
540 #if !NO_RC_THRUST_LIMIT
541  /* use rc limitation if available */
542  if (radio_control.status == RC_OK) {
544  } else
545 #endif
546  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
547 }
548 
549 void guidance_v_set_z(float z)
550 {
551  /* disable vertical velocity setpoints */
553  /* set altitude setpoint */
555  /* reset speed setting */
556  guidance_v_zd_sp = 0;
557 }
558 
559 void guidance_v_set_vz(float vz)
560 {
561  /* enable vertical velocity setpoints */
563  /* set speed setting */
565 }
566 
567 void guidance_v_set_th(float th)
568 {
569  /* enable vertical velocity setpoints */
571 
572  /* reset guidance reference */
575  Bound(guidance_v_th_sp, 0, MAX_PPRZ);
576 }
577 
578 
nav_climb
float nav_climb
Definition: nav.c:59
radio_control.h
nav_flight_altitude
int32_t nav_flight_altitude
Definition: navigation.c:120
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
send_tune_vert
static void send_tune_vert(struct transport_tx *trans, struct link_device *dev)
Definition: guidance_v.c:168
vertical_mode
uint8_t vertical_mode
Definition: sim_ap.c:32
GUIDANCE_V_MAX_SUM_ERR
#define GUIDANCE_V_MAX_SUM_ERR
Definition: guidance_v.c:96
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
guidance_v_notify_in_flight
void guidance_v_notify_in_flight(bool in_flight)
Definition: guidance_v.c:271
stateGetPositionNed_f
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
guidance_v_rc_delta_t
int32_t guidance_v_rc_delta_t
Direct throttle from radio control.
Definition: guidance_v.c:121
gv_adapt_X
int32_t gv_adapt_X
State of the estimator.
Definition: guidance_v_adapt.c:99
GV_ADAPT_X_FRAC
#define GV_ADAPT_X_FRAC
Definition: guidance_v_adapt.h:40
desired_zd_updated
static bool desired_zd_updated
Definition: guidance_v.c:110
GUIDANCE_V_MIN_ERR_Z
#define GUIDANCE_V_MIN_ERR_Z
Definition: guidance_v.c:80
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
guidance_v_set_ref
void guidance_v_set_ref(int32_t pos, int32_t speed, int32_t accel)
Set guidance ref parameters.
Definition: guidance_v.c:372
GV_ZDD_REF_FRAC
#define GV_ZDD_REF_FRAC
number of bits for the fractional part of gv_zdd_ref
Definition: guidance_v_ref.h:57
guidance_v_module_init
void guidance_v_module_init(void)
Definition: ctrl_module_innerloop_demo.c:105
INT32_SPEED_FRAC
#define INT32_SPEED_FRAC
Definition: pprz_algebra_int.h:73
GUIDANCE_V_NOMINAL_HOVER_THROTTLE
#define GUIDANCE_V_NOMINAL_HOVER_THROTTLE
Definition: guidance_v.c:58
guidance_v_zdd_ref
int32_t guidance_v_zdd_ref
vertical acceleration reference in meter/s^2.
Definition: guidance_v.c:134
guidance_v_z_sp
int32_t guidance_v_z_sp
altitude setpoint in meters (input).
Definition: guidance_v.c:129
guidance_v_mode_changed
void guidance_v_mode_changed(uint8_t new_mode)
Definition: guidance_v.c:231
guidance_v_module_enter
void guidance_v_module_enter(void)
Definition: ctrl_module_innerloop_demo.c:111
guidance_v_read_rc
void guidance_v_read_rc(void)
Definition: guidance_v.c:209
GUIDANCE_V_MODE_CLIMB
#define GUIDANCE_V_MODE_CLIMB
Definition: guidance_v.h:38
GUIDANCE_V_MODE_MODULE
#define GUIDANCE_V_MODE_MODULE
Definition: guidance_v.h:41
GUIDANCE_V_GUIDED_MODE_ZHOLD
#define GUIDANCE_V_GUIDED_MODE_ZHOLD
Definition: guidance_v.c:112
SPEED_BFP_OF_REAL
#define SPEED_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:219
run_hover_loop
void run_hover_loop(bool in_flight)
Definition: guidance_v.c:412
guidance_module.h
gv_adapt_P
int32_t gv_adapt_P
Covariance.
Definition: guidance_v_adapt.c:100
guidance_v_fb_cmd
int32_t guidance_v_fb_cmd
feed-back command
Definition: guidance_v.c:105
stateGetPositionNed_i
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:665
guidance_v.h
VERTICAL_MODE_CLIMB
#define VERTICAL_MODE_CLIMB
Definition: navigation.h:82
gv_update_ref_from_zd_sp
void gv_update_ref_from_zd_sp(int32_t zd_sp, int32_t z_pos)
update vertical reference from speed setpoint.
Definition: guidance_v_ref.c:129
guidance_v_z_sum_err
int32_t guidance_v_z_sum_err
accumulator for I-gain
Definition: guidance_v.c:140
pprz_algebra_int.h
Paparazzi fixed point algebra.
NedCoor_i::z
int32_t z
Down.
Definition: pprz_geodetic_int.h:71
guidance_v_zd_ref
int32_t guidance_v_zd_ref
vertical speed reference in meter/s.
Definition: guidance_v.c:133
GUIDANCE_V_MAX_RC_DESCENT_SPEED
#define GUIDANCE_V_MAX_RC_DESCENT_SPEED
Definition: guidance_v.c:76
gv_zd_ref
int32_t gv_zd_ref
reference model vertical speed in meters/sec (output) fixed point representation with GV_ZD_REF_FRAC ...
Definition: guidance_v_ref.c:41
INT32_ACCEL_FRAC
#define INT32_ACCEL_FRAC
Definition: pprz_algebra_int.h:78
telemetry.h
guidance_v_z_enter
void guidance_v_z_enter(void)
Definition: guidance_v.c:359
stateGetNedToBodyRMat_i
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
send_vert_loop
static void send_vert_loop(struct transport_tx *trans, struct link_device *dev)
Definition: guidance_v.c:150
guidance_v_init
void guidance_v_init(void)
Definition: guidance_v.c:178
guidance_v_ff_cmd
int32_t guidance_v_ff_cmd
feed-forward command
Definition: guidance_v.c:104
guidance_v_set_z
void guidance_v_set_z(float z)
Set z position setpoint.
Definition: guidance_v.c:549
guidance_v_mode
uint8_t guidance_v_mode
Definition: guidance_v.c:103
GUIDANCE_V_MODE_RC_CLIMB
#define GUIDANCE_V_MODE_RC_CLIMB
Definition: guidance_v.h:37
stateGetSpeedNed_i
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:863
VERTICAL_MODE_MANUAL
#define VERTICAL_MODE_MANUAL
Definition: navigation.h:81
GUIDANCE_V_MODE_KILL
#define GUIDANCE_V_MODE_KILL
Definition: guidance_v.h:35
GUIDANCE_V_MODE_RC_DIRECT
#define GUIDANCE_V_MODE_RC_DIRECT
Definition: guidance_v.h:36
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
guidance_v_zd_sp
int32_t guidance_v_zd_sp
vertical speed setpoint in meter/s (input).
Definition: guidance_v.c:130
GuidanceVSetRef
#define GuidanceVSetRef
Definition: guidance_v.h:120
guidance_v_adapt_throttle_enabled
bool guidance_v_adapt_throttle_enabled
Use adaptive throttle command estimation.
Definition: guidance_v.c:109
Int32RMat::m
int32_t m[3 *3]
Definition: pprz_algebra_int.h:160
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
guidance_v_set_vz
void guidance_v_set_vz(float vz)
Set z velocity setpoint.
Definition: guidance_v.c:559
gv_adapt_Xmeas
int32_t gv_adapt_Xmeas
Measurement.
Definition: guidance_v_adapt.c:101
RadioControl::status
uint8_t status
Definition: radio_control.h:61
guidance_v_th_sp
int32_t guidance_v_th_sp
Definition: guidance_v.c:131
guidance_hybrid_vertical
void guidance_hybrid_vertical(void)
Description.
Definition: guidance_hybrid.c:378
guidance_v_run
void guidance_v_run(bool in_flight)
Definition: guidance_v.c:299
gv_z_ref
int64_t gv_z_ref
reference model altitude in meters (output) fixed point representation with GV_Z_REF_FRAC Q37....
Definition: guidance_v_ref.c:47
guidance_v_guided_enter
void guidance_v_guided_enter(void)
Enter GUIDED mode control.
Definition: guidance_v.c:507
BFP_OF_REAL
#define BFP_OF_REAL(_vr, _frac)
Definition: pprz_algebra_int.h:206
GUIDANCE_V_GUIDED_MODE_CLIMB
#define GUIDANCE_V_GUIDED_MODE_CLIMB
Definition: guidance_v.c:113
GUIDANCE_V_MODE_HOVER
#define GUIDANCE_V_MODE_HOVER
Definition: guidance_v.h:39
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
gv_adapt_run
void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref)
Adaptation function.
Definition: guidance_v_adapt.c:133
GUIDANCE_V_MAX_ERR_ZD
#define GUIDANCE_V_MAX_ERR_ZD
Definition: guidance_v.c:92
gv_adapt_init
void gv_adapt_init(void)
Definition: guidance_v_adapt.c:120
GUIDANCE_V_GUIDED_MODE_THROTTLE
#define GUIDANCE_V_GUIDED_MODE_THROTTLE
Definition: guidance_v.c:114
guidance_v_nominal_throttle
float guidance_v_nominal_throttle
nominal throttle for hover.
Definition: guidance_v.c:108
GUIDANCE_V_MODE_FLIP
#define GUIDANCE_V_MODE_FLIP
Definition: guidance_v.h:42
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
GUIDANCE_V_MAX_ERR_Z
#define GUIDANCE_V_MAX_ERR_Z
Definition: guidance_v.c:84
GUIDANCE_V_MIN_ERR_ZD
#define GUIDANCE_V_MIN_ERR_ZD
Definition: guidance_v.c:88
GV_Z_REF_FRAC
#define GV_Z_REF_FRAC
number of bits for the fractional part of gv_z_ref
Definition: guidance_v_ref.h:75
GUIDANCE_V_MAX_RC_CLIMB_SPEED
#define GUIDANCE_V_MAX_RC_CLIMB_SPEED
Definition: guidance_v.c:72
gv_zdd_ref
int32_t gv_zdd_ref
reference model vertical accel in meters/s^2 (output) fixed point representation with GV_ZDD_REF_FRAC...
Definition: guidance_v_ref.c:35
guidance_v_delta_t
int32_t guidance_v_delta_t
thrust command.
Definition: guidance_v.c:106
VERTICAL_MODE_GUIDED
#define VERTICAL_MODE_GUIDED
Definition: navigation.h:84
guidance_v_module_run
void guidance_v_module_run(UNUSED bool in_flight)
Definition: ctrl_module_innerloop_demo.c:116
VERTICAL_MODE_ALT
#define VERTICAL_MODE_ALT
Definition: navigation.h:83
GUIDANCE_V_MODE_NAV
#define GUIDANCE_V_MODE_NAV
Definition: guidance_v.h:40
RC_OK
#define RC_OK
Definition: radio_control.h:49
guidance_v_thrust_adapt
void guidance_v_thrust_adapt(bool in_flight)
Definition: guidance_v.c:278
guidance_v_ki
int32_t guidance_v_ki
vertical control I-gain
Definition: guidance_v.c:138
guidance_v_set_th
void guidance_v_set_th(float th)
Set throttle setpoint.
Definition: guidance_v.c:567
guidance_hybrid.h
GUIDANCE_V_CLIMB_RC_DEADBAND
#define GUIDANCE_V_CLIMB_RC_DEADBAND
Definition: guidance_v.c:68
navigation.h
gv_update_ref_from_z_sp
void gv_update_ref_from_z_sp(int32_t z_sp)
Definition: guidance_v_ref.c:95
GUIDANCE_V_MODE_GUIDED
#define GUIDANCE_V_MODE_GUIDED
Definition: guidance_v.h:43
guidance_v_thrust_coeff
int32_t guidance_v_thrust_coeff
Definition: guidance_v.c:142
stabilization_cmd
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
GUIDANCE_V_MAX_CMD
#define GUIDANCE_V_MAX_CMD
Definition: guidance_v.c:100
GV_ZD_REF_FRAC
#define GV_ZD_REF_FRAC
number of bits for the fractional part of gv_zd_ref
Definition: guidance_v_ref.h:66
gv_set_ref
void gv_set_ref(int32_t alt, int32_t speed, int32_t accel)
Definition: guidance_v_ref.c:87
stateGetAccelNed_i
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:1020
guidance_v_kd
int32_t guidance_v_kd
vertical control D-gain
Definition: guidance_v.c:137
POS_BFP_OF_REAL
#define POS_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:217
guidance_v_kp
int32_t guidance_v_kp
vertical control P-gain
Definition: guidance_v.c:136
FF_CMD_FRAC
#define FF_CMD_FRAC
Definition: guidance_v.c:410
stabilization.h
state.h
INT32_TRIG_FRAC
#define INT32_TRIG_FRAC
Definition: pprz_algebra_int.h:154
get_vertical_thrust_coeff
static int32_t get_vertical_thrust_coeff(void)
get the cosine of the angle between thrust vector and gravity vector
Definition: guidance_v.c:382
guidance_v_guided_run
void guidance_v_guided_run(bool in_flight)
Run GUIDED mode control.
Definition: guidance_v.c:517
guidance_v_guided_mode
int guidance_v_guided_mode
Definition: guidance_v.c:116
GUIDANCE_V_ADAPT_THROTTLE_ENABLED
#define GUIDANCE_V_ADAPT_THROTTLE_ENABLED
Definition: guidance_v.c:60
INT32_POS_FRAC
#define INT32_POS_FRAC
Definition: pprz_algebra_int.h:68
guidance_v_from_nav
void guidance_v_from_nav(bool in_flight)
Set guidance setpoint from NAV and run hover loop.
Definition: guidance_v.c:473
nav_throttle
uint32_t nav_throttle
direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL
Definition: navigation.c:119
guidance_v_rc_zd_sp
int32_t guidance_v_rc_zd_sp
Vertical speed setpoint from radio control.
Definition: guidance_v.c:127
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
radio_control
struct RadioControl radio_control
Definition: radio_control.c:33
Min
#define Min(x, y)
Definition: esc_dshot.c:85
RadioControl::values
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:67
guidance_v_z_ref
int32_t guidance_v_z_ref
altitude reference in meters.
Definition: guidance_v.c:132