Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
103 
106 static bool desired_zd_updated;
107 
108 #define GUIDANCE_V_GUIDED_MODE_ZHOLD 0
109 #define GUIDANCE_V_GUIDED_MODE_CLIMB 1
110 #define GUIDANCE_V_GUIDED_MODE_THROTTLE 2
111 
113 
118 
124 
131 
135 
137 
139 
140 
142 
143 #if PERIODIC_TELEMETRY
145 
146 static void send_vert_loop(struct transport_tx *trans, struct link_device *dev)
147 {
148  pprz_msg_send_VERT_LOOP(trans, dev, AC_ID,
149  &guidance_v_z_sp, &guidance_v_zd_sp,
150  &(stateGetPositionNed_i()->z),
151  &(stateGetSpeedNed_i()->z),
152  &(stateGetAccelNed_i()->z),
153  &guidance_v_z_ref, &guidance_v_zd_ref,
154  &guidance_v_zdd_ref,
155  &gv_adapt_X,
156  &gv_adapt_P,
158  &guidance_v_z_sum_err,
159  &guidance_v_ff_cmd,
160  &guidance_v_fb_cmd,
161  &guidance_v_delta_t);
162 }
163 
164 static void send_tune_vert(struct transport_tx *trans, struct link_device *dev)
165 {
166  pprz_msg_send_TUNE_VERT(trans, dev, AC_ID,
167  &guidance_v_z_sp,
168  &(stateGetPositionNed_i()->z),
169  &guidance_v_z_ref,
170  &guidance_v_zd_ref);
171 }
172 #endif
173 
174 void guidance_v_init(void)
175 {
176 
177  guidance_v_mode = GUIDANCE_V_MODE_KILL;
178 
179  guidance_v_kp = GUIDANCE_V_HOVER_KP;
180  guidance_v_kd = GUIDANCE_V_HOVER_KD;
181  guidance_v_ki = GUIDANCE_V_HOVER_KI;
182 
183  guidance_v_z_sum_err = 0;
184 
185  guidance_v_nominal_throttle = GUIDANCE_V_NOMINAL_HOVER_THROTTLE;
186  guidance_v_adapt_throttle_enabled = GUIDANCE_V_ADAPT_THROTTLE_ENABLED;
187  desired_zd_updated = false;
188  guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
189 
190  guidance_v_thrust_coeff = BFP_OF_REAL(1.f, INT32_TRIG_FRAC);
191 
192  gv_adapt_init();
193 
194 #if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
196 #endif
197 
198 #if PERIODIC_TELEMETRY
201 #endif
202 }
203 
204 
206 {
207 
208  /* used in RC_DIRECT directly and as saturation in CLIMB and HOVER */
209  guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE];
210 
211  /* used in RC_CLIMB */
212  guidance_v_rc_zd_sp = (MAX_PPRZ / 2) - (int32_t)radio_control.values[RADIO_THROTTLE];
213  DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_CLIMB_RC_DEADBAND);
214 
215  static const int32_t climb_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_CLIMB_SPEED) /
217  static const int32_t descent_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_DESCENT_SPEED) /
219 
220  if (guidance_v_rc_zd_sp > 0) {
221  guidance_v_rc_zd_sp *= descent_scale;
222  } else {
223  guidance_v_rc_zd_sp *= climb_scale;
224  }
225 }
226 
228 {
229 
230  if (new_mode == guidance_v_mode) {
231  return;
232  }
233 
234  switch (new_mode) {
238  break;
239 
242  guidance_v_zd_sp = 0;
243  /* Falls through. */
244  case GUIDANCE_V_MODE_NAV:
245  guidance_v_z_sum_err = 0;
247  break;
248 
249 #if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
252  break;
253 #endif
254 
256  break;
257 
258  default:
259  break;
260 
261  }
262 
263  guidance_v_mode = new_mode;
264 
265 }
266 
267 void guidance_v_notify_in_flight(bool in_flight)
268 {
269  if (in_flight) {
270  gv_adapt_init();
271  }
272 }
273 
274 void guidance_v_thrust_adapt(bool in_flight)
275 {
276  guidance_v_thrust_coeff = get_vertical_thrust_coeff();
277 
278  if (in_flight) {
279  /* Only run adaptive throttle estimation if we are in flight and
280  * the desired vertical velocity (zd) was updated (i.e. we ran hover_loop before).
281  * This means that the estimation is not updated when using direct throttle commands.
282  *
283  * FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT, AKA SUPERVISION and co
284  */
285  if (desired_zd_updated) {
286  int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC;
287  gv_adapt_run(stateGetAccelNed_i()->z, vertical_thrust, guidance_v_zd_ref);
288  }
289  } else {
290  /* reset estimate while not in_flight */
291  gv_adapt_init();
292  }
293 }
294 
295 void guidance_v_run(bool in_flight)
296 {
297  guidance_v_thrust_adapt(in_flight);
298 
299  /* reset flag indicating if desired zd was updated */
300  desired_zd_updated = false;
301 
302  switch (guidance_v_mode) {
303 
305  guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only
306  stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t;
307  break;
308 
310  guidance_v_zd_sp = guidance_v_rc_zd_sp;
311  gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
312  run_hover_loop(in_flight);
313  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
314  break;
315 
317  gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
318  run_hover_loop(in_flight);
319 #if !NO_RC_THRUST_LIMIT
320  /* use rc limitation if available */
321  if (radio_control.status == RC_OK) {
322  stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t);
323  } else
324 #endif
325  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
326  break;
327 
329  guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
330  /* Falls through. */
332  guidance_v_guided_run(in_flight);
333  break;
334 
335 #if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
337  guidance_v_module_run(in_flight);
338  break;
339 #endif
340 
341  case GUIDANCE_V_MODE_NAV: {
342  guidance_v_from_nav(in_flight);
343  break;
344  }
345 
347  break;
348 
349  default:
350  break;
351  }
352 }
353 
354 
356 {
357  /* set current altitude as setpoint */
358  guidance_v_z_sp = stateGetPositionNed_i()->z;
359 
360  /* reset guidance reference */
361  guidance_v_z_sum_err = 0;
363 
364  /* reset speed setting */
365  guidance_v_zd_sp = 0;
366 }
367 
368 void guidance_v_set_ref(int32_t pos, int32_t speed, int32_t accel)
369 {
370  gv_set_ref(pos, speed, accel);
371  guidance_v_z_ref = pos;
372  guidance_v_zd_ref = speed;
373  guidance_v_zdd_ref = accel;
374 }
375 
376 
379 {
380  // cos(30°) = 0.8660254
381  static const int32_t max_bank_coef = BFP_OF_REAL(0.8660254f, INT32_TRIG_FRAC);
382 
383  struct Int32RMat *att = stateGetNedToBodyRMat_i();
384  /* thrust vector:
385  * int32_rmat_vmult(&thrust_vect, &att, &zaxis)
386  * same as last colum of rmat with INT32_TRIG_FRAC
387  * struct Int32Vect thrust_vect = {att.m[2], att.m[5], att.m[8]};
388  *
389  * Angle between two vectors v1 and v2:
390  * angle = acos(dot(v1, v2) / (norm(v1) * norm(v2)))
391  * since here both are already of unit length:
392  * angle = acos(dot(v1, v2))
393  * since we we want the cosine of the angle we simply need
394  * thrust_coeff = dot(v1, v2)
395  * also can be simplified considering: v1 is zaxis with (0,0,1)
396  * dot(v1, v2) = v1.z * v2.z = v2.z
397  */
398  int32_t coef = att->m[8];
399  if (coef < max_bank_coef) {
400  coef = max_bank_coef;
401  }
402  return coef;
403 }
404 
405 
406 #define FF_CMD_FRAC 18
407 
408 void run_hover_loop(bool in_flight)
409 {
410 
411  /* convert our reference to generic representation */
413  guidance_v_z_ref = (int32_t)tmp;
414  guidance_v_zd_ref = gv_zd_ref << (INT32_SPEED_FRAC - GV_ZD_REF_FRAC);
415  guidance_v_zdd_ref = gv_zdd_ref << (INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC);
416  /* set flag to indicate that desired zd was updated */
417  desired_zd_updated = true;
418  /* compute the error to our reference */
419  int32_t err_z = guidance_v_z_ref - stateGetPositionNed_i()->z;
421  int32_t err_zd = guidance_v_zd_ref - stateGetSpeedNed_i()->z;
423 
424  if (in_flight) {
425  guidance_v_z_sum_err += err_z;
426  Bound(guidance_v_z_sum_err, -GUIDANCE_V_MAX_SUM_ERR, GUIDANCE_V_MAX_SUM_ERR);
427  } else {
428  guidance_v_z_sum_err = 0;
429  }
430 
431  /* our nominal command : (g + zdd)*m */
432  int32_t inv_m;
433  if (guidance_v_adapt_throttle_enabled) {
434  inv_m = gv_adapt_X >> (GV_ADAPT_X_FRAC - FF_CMD_FRAC);
435  } else {
436  /* use the fixed nominal throttle */
437  inv_m = BFP_OF_REAL(9.81 / (guidance_v_nominal_throttle * MAX_PPRZ), FF_CMD_FRAC);
438  }
439 
440  const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) -
441  (guidance_v_zdd_ref << (FF_CMD_FRAC - INT32_ACCEL_FRAC));
442 
443  guidance_v_ff_cmd = g_m_zdd / inv_m;
444  /* feed forward command */
445  guidance_v_ff_cmd = (guidance_v_ff_cmd << INT32_TRIG_FRAC) / guidance_v_thrust_coeff;
446 
447 #if HYBRID_NAVIGATION
448  //FIXME: NOT USING FEEDFORWARD COMMAND BECAUSE OF QUADSHOT NAVIGATION
449  guidance_v_ff_cmd = guidance_v_nominal_throttle * MAX_PPRZ;
450 #endif
451 
452  /* bound the nominal command to 0.9*MAX_PPRZ */
453  Bound(guidance_v_ff_cmd, 0, 8640);
454 
455 
456  /* our error feed back command */
457  /* z-axis pointing down -> positive error means we need less thrust */
458  guidance_v_fb_cmd = ((-guidance_v_kp * err_z) >> 7) +
459  ((-guidance_v_kd * err_zd) >> 16) +
460  ((-guidance_v_ki * guidance_v_z_sum_err) >> 16);
461 
462  guidance_v_delta_t = guidance_v_ff_cmd + guidance_v_fb_cmd;
463 
464  /* bound the result */
465  Bound(guidance_v_delta_t, 0, MAX_PPRZ);
466 
467 }
468 
469 void guidance_v_from_nav(bool in_flight)
470 {
472  guidance_v_z_sp = -nav_flight_altitude;
473  guidance_v_zd_sp = 0;
474  gv_update_ref_from_z_sp(guidance_v_z_sp);
475  run_hover_loop(in_flight);
476  } else if (vertical_mode == VERTICAL_MODE_CLIMB) {
477  guidance_v_z_sp = stateGetPositionNed_i()->z;
478  guidance_v_zd_sp = -nav_climb;
479  gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
480  run_hover_loop(in_flight);
481  } else if (vertical_mode == VERTICAL_MODE_MANUAL) {
482  guidance_v_z_sp = stateGetPositionNed_i()->z;
483  guidance_v_zd_sp = stateGetSpeedNed_i()->z;
484  GuidanceVSetRef(guidance_v_z_sp, guidance_v_zd_sp, 0);
485  guidance_v_z_sum_err = 0;
486  guidance_v_delta_t = nav_throttle;
487  }
488 #if HYBRID_NAVIGATION
490 #else
491 #if !NO_RC_THRUST_LIMIT
492  /* use rc limitation if available */
493  if (radio_control.status == RC_OK) {
494  stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t);
495  } else
496 #endif
497  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
498 #endif
499 }
500 
502 {
503  /* disable vertical velocity setpoints */
504  guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
505 
506  /* set current altitude as setpoint and reset speed setpoint */
507  guidance_v_z_sp = stateGetPositionNed_i()->z;
508  guidance_v_zd_sp = 0;
509 
510  /* reset guidance reference */
511  guidance_v_z_sum_err = 0;
513 }
514 
515 void guidance_v_guided_run(bool in_flight)
516 {
517  switch(guidance_v_guided_mode)
518  {
520  // Altitude Hold
521  guidance_v_zd_sp = 0;
522  gv_update_ref_from_z_sp(guidance_v_z_sp);
523  run_hover_loop(in_flight);
524  break;
526  // Climb
527  gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
528  run_hover_loop(in_flight);
529  break;
531  // Throttle
532  guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only
533  guidance_v_delta_t = guidance_v_th_sp;
534  break;
535  default:
536  break;
537  }
538 #if !NO_RC_THRUST_LIMIT
539  /* use rc limitation if available */
540  if (radio_control.status == RC_OK) {
541  stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t);
542  } else
543 #endif
544  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
545 }
546 
548 {
549  if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED) {
550  /* disable vertical velocity setpoints */
551  guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
552 
553  /* set altitude setpoint */
554  guidance_v_z_sp = POS_BFP_OF_REAL(z);
555 
556  /* reset speed setting */
557  guidance_v_zd_sp = 0;
558 
559  /* reset guidance reference */
560  guidance_v_z_sum_err = 0;
562  return true;
563  }
564  return false;
565 }
566 
568 {
569  if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED) {
570  /* enable vertical velocity setpoints */
571  guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_CLIMB;
572 
573  /* set speed setting */
574  guidance_v_zd_sp = SPEED_BFP_OF_REAL(vz);
575 
576  /* reset guidance reference */
578  return true;
579  }
580  return false;
581 }
582 
584 {
585  if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED) {
586  /* enable vertical velocity setpoints */
587  guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_THROTTLE;
588 
589  /* reset guidance reference */
591  guidance_v_th_sp = (int32_t)(MAX_PPRZ * th);
592  Bound(guidance_v_th_sp, 0, MAX_PPRZ);
593  return true;
594  }
595  return false;
596 }
597 
598 
bool guidance_v_set_guided_z(float z)
Set z setpoint in GUIDED mode.
Definition: guidance_v.c:547
int32_t guidance_v_z_sp
altitude setpoint in meters (input).
Definition: guidance_v.c:125
bool guidance_v_set_guided_th(float th)
Definition: guidance_v.c:583
void guidance_v_guided_run(bool in_flight)
Run GUIDED mode control.
Definition: guidance_v.c:515
void guidance_hybrid_vertical(void)
Description.
#define GV_ZD_REF_FRAC
number of bits for the fractional part of gv_zd_ref
int32_t guidance_v_kd
vertical control D-gain
Definition: guidance_v.c:133
#define GUIDANCE_V_MODE_NAV
Definition: guidance_v.h:40
static void send_vert_loop(struct transport_tx *trans, struct link_device *dev)
Definition: guidance_v.c:146
bool guidance_v_set_guided_vz(float vz)
Set z velocity setpoint in GUIDED mode.
Definition: guidance_v.c:567
#define GUIDANCE_V_NOMINAL_HOVER_THROTTLE
Definition: guidance_v.c:58
#define GV_Z_REF_FRAC
number of bits for the fractional part of gv_z_ref
#define INT32_ACCEL_FRAC
#define INT32_SPEED_FRAC
int32_t guidance_v_fb_cmd
feed-back command
Definition: guidance_v.c:101
Periodic telemetry system header (includes downlink utility and generated code).
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
void guidance_v_set_ref(int32_t pos, int32_t speed, int32_t accel)
Set guidance ref parameters.
Definition: guidance_v.c:368
int32_t guidance_v_delta_t
thrust command.
Definition: guidance_v.c:102
uint8_t status
Definition: radio_control.h:64
#define GUIDANCE_V_GUIDED_MODE_ZHOLD
Definition: guidance_v.c:108
int32_t guidance_v_kp
vertical control P-gain
Definition: guidance_v.c:132
#define GV_ADAPT_X_FRAC
void guidance_v_init(void)
Definition: guidance_v.c:174
#define POS_BFP_OF_REAL(_af)
bool guidance_v_adapt_throttle_enabled
Use adaptive throttle command estimation.
Definition: guidance_v.c:105
#define GUIDANCE_V_MIN_ERR_Z
Definition: guidance_v.c:80
int32_t m[3 *3]
int32_t nav_flight_altitude
Definition: navigation.c:111
void guidance_v_mode_changed(uint8_t new_mode)
Definition: guidance_v.c:227
float guidance_v_nominal_throttle
nominal throttle for hover.
Definition: guidance_v.c:104
Vertical guidance for rotorcrafts.
void guidance_v_z_enter(void)
Definition: guidance_v.c:355
#define GV_ZDD_REF_FRAC
number of bits for the fractional part of gv_zdd_ref
void guidance_v_run(bool in_flight)
Definition: guidance_v.c:295
signed long long int64_t
Definition: types.h:21
int32_t guidance_v_z_sum_err
accumulator for I-gain
Definition: guidance_v.c:136
void guidance_v_notify_in_flight(bool in_flight)
Definition: guidance_v.c:267
Guidance controllers (horizontal and vertical) for Hybrid UAV configurations.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
int32_t z
Down.
#define GUIDANCE_V_MODE_KILL
Definition: guidance_v.h:35
void run_hover_loop(bool in_flight)
Definition: guidance_v.c:408
int32_t guidance_v_rc_zd_sp
Vertical speed setpoint from radio control.
Definition: guidance_v.c:123
#define BFP_OF_REAL(_vr, _frac)
#define GuidanceVSetRef
Definition: guidance_v.h:120
void gv_update_ref_from_z_sp(int32_t z_sp)
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:863
int32_t guidance_v_ki
vertical control I-gain
Definition: guidance_v.c:134
#define GUIDANCE_V_MODE_MODULE
Definition: guidance_v.h:41
void gv_adapt_init(void)
#define GUIDANCE_V_MAX_RC_DESCENT_SPEED
Definition: guidance_v.c:76
#define GUIDANCE_V_MODE_CLIMB
Definition: guidance_v.h:38
int32_t gv_zd_ref
reference model vertical speed in meters/sec (output) fixed point representation with GV_ZD_REF_FRAC ...
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
int32_t guidance_v_zdd_ref
vertical acceleration reference in meter/s^2.
Definition: guidance_v.c:130
#define GUIDANCE_V_MAX_ERR_Z
Definition: guidance_v.c:84
#define GUIDANCE_V_MODE_HOVER
Definition: guidance_v.h:39
struct RadioControl radio_control
Definition: radio_control.c:30
void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref)
Adaptation function.
#define GUIDANCE_V_MAX_RC_CLIMB_SPEED
Definition: guidance_v.c:72
int32_t guidance_v_z_ref
altitude reference in meters.
Definition: guidance_v.c:128
void guidance_v_module_run(UNUSED bool in_flight)
#define INT32_POS_FRAC
#define RADIO_THROTTLE
Definition: intermcu_ap.h:39
void gv_update_ref_from_zd_sp(int32_t zd_sp, int32_t z_pos)
update vertical reference from speed setpoint.
int32_t guidance_v_zd_ref
vertical speed reference in meter/s.
Definition: guidance_v.c:129
int32_t guidance_v_th_sp
Definition: guidance_v.c:127
#define Min(x, y)
Definition: main_fbw.c:52
int32_t gv_adapt_P
Covariance.
signed long int32_t
Definition: types.h:19
#define RC_OK
Definition: radio_control.h:56
void guidance_v_guided_enter(void)
Enter GUIDED mode control.
Definition: guidance_v.c:501
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
#define INT32_TRIG_FRAC
void gv_set_ref(int32_t alt, int32_t speed, int32_t accel)
#define GUIDANCE_V_CLIMB_RC_DEADBAND
Definition: guidance_v.c:68
#define GUIDANCE_V_GUIDED_MODE_CLIMB
Definition: guidance_v.c:109
Rotorcraft navigation functions.
uint8_t guidance_v_mode
Definition: guidance_v.c:99
#define GUIDANCE_V_ADAPT_THROTTLE_ENABLED
Definition: guidance_v.c:60
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
void guidance_v_module_init(void)
int32_t gv_adapt_Xmeas
Measurement.
#define FF_CMD_FRAC
Definition: guidance_v.c:406
int32_t guidance_v_thrust_coeff
Definition: guidance_v.c:138
#define GUIDANCE_V_MIN_ERR_ZD
Definition: guidance_v.c:88
int64_t gv_z_ref
reference model altitude in meters (output) fixed point representation with GV_Z_REF_FRAC Q37...
static int32_t get_vertical_thrust_coeff(void)
get the cosine of the angle between thrust vector and gravity vector
Definition: guidance_v.c:378
General stabilization interface for rotorcrafts.
void guidance_v_read_rc(void)
Definition: guidance_v.c:205
#define GUIDANCE_V_GUIDED_MODE_THROTTLE
Definition: guidance_v.c:110
void guidance_v_module_enter(void)
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
#define GUIDANCE_V_MAX_ERR_ZD
Definition: guidance_v.c:92
#define VERTICAL_MODE_ALT
Definition: navigation.h:75
#define GUIDANCE_V_MODE_FLIP
Definition: guidance_v.h:42
static void send_tune_vert(struct transport_tx *trans, struct link_device *dev)
Definition: guidance_v.c:164
Guidance in a module file.
int32_t guidance_v_zd_sp
vertical speed setpoint in meter/s (input).
Definition: guidance_v.c:126
#define GUIDANCE_V_MAX_SUM_ERR
Definition: guidance_v.c:96
int32_t guidance_v_rc_delta_t
Direct throttle from radio control.
Definition: guidance_v.c:117
rotation matrix
uint32_t nav_throttle
direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL
Definition: navigation.c:110
static bool desired_zd_updated
Definition: guidance_v.c:106
#define SPEED_BFP_OF_REAL(_af)
int32_t guidance_v_ff_cmd
feed-forward command
Definition: guidance_v.c:100
#define GUIDANCE_V_MODE_RC_CLIMB
Definition: guidance_v.h:37
#define MAX_PPRZ
Definition: paparazzi.h:8
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:665
#define VERTICAL_MODE_CLIMB
Definition: navigation.h:74
void guidance_v_thrust_adapt(bool in_flight)
Definition: guidance_v.c:274
#define GUIDANCE_V_MODE_RC_DIRECT
Definition: guidance_v.h:36
uint8_t vertical_mode
Definition: sim_ap.c:35
int32_t gv_adapt_X
State of the estimator.
#define VERTICAL_MODE_MANUAL
Definition: navigation.h:73
#define GUIDANCE_V_MODE_GUIDED
Definition: guidance_v.h:43
int32_t gv_zdd_ref
reference model vertical accel in meters/s^2 (output) fixed point representation with GV_ZDD_REF_FRAC...
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
int guidance_v_guided_mode
Definition: guidance_v.c:112
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:1020
Paparazzi fixed point algebra.
void guidance_v_from_nav(bool in_flight)
Set guidance setpoint from NAV and run hover loop.
Definition: guidance_v.c:469