Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
guidance_h.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"
28 
37 
41 
42 /* for guidance_v_thrust_coeff */
44 
45 #include "state.h"
46 
47 #ifndef GUIDANCE_H_AGAIN
48 #define GUIDANCE_H_AGAIN 0
49 #endif
50 
51 #ifndef GUIDANCE_H_VGAIN
52 #define GUIDANCE_H_VGAIN 0
53 #endif
54 
55 /* error if some gains are negative */
56 #if (GUIDANCE_H_PGAIN < 0) || \
57  (GUIDANCE_H_DGAIN < 0) || \
58  (GUIDANCE_H_IGAIN < 0) || \
59  (GUIDANCE_H_AGAIN < 0) || \
60  (GUIDANCE_H_VGAIN < 0)
61 #error "ALL control gains have to be positive!!!"
62 #endif
63 
64 #ifndef GUIDANCE_H_MAX_BANK
65 #define GUIDANCE_H_MAX_BANK RadOfDeg(20)
66 #endif
67 
68 PRINT_CONFIG_VAR(GUIDANCE_H_USE_REF)
69 PRINT_CONFIG_VAR(GUIDANCE_H_USE_SPEED_REF)
70 
71 #ifndef GUIDANCE_H_APPROX_FORCE_BY_THRUST
72 #define GUIDANCE_H_APPROX_FORCE_BY_THRUST FALSE
73 #endif
74 
75 #ifndef GUIDANCE_INDI
76 #define GUIDANCE_INDI FALSE
77 #endif
78 
80 
83 
84 /*
85  * internal variables
86  */
90 
96 
97 static void guidance_h_update_reference(void);
98 #if !GUIDANCE_INDI
99 static void guidance_h_traj_run(bool_t in_flight);
100 #endif
101 static void guidance_h_hover_enter(void);
102 static void guidance_h_nav_enter(void);
103 static inline void transition_run(void);
104 static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flight);
105 
106 #if PERIODIC_TELEMETRY
108 
109 static void send_gh(struct transport_tx *trans, struct link_device *dev)
110 {
111  struct NedCoor_i *pos = stateGetPositionNed_i();
112  pprz_msg_send_GUIDANCE_H_INT(trans, dev, AC_ID,
115  &(pos->x), &(pos->y));
116 }
117 
118 static void send_hover_loop(struct transport_tx *trans, struct link_device *dev)
119 {
120  struct NedCoor_i *pos = stateGetPositionNed_i();
121  struct NedCoor_i *speed = stateGetSpeedNed_i();
122  struct NedCoor_i *accel = stateGetAccelNed_i();
123  pprz_msg_send_HOVER_LOOP(trans, dev, AC_ID,
124  &guidance_h.sp.pos.x,
125  &guidance_h.sp.pos.y,
126  &(pos->x), &(pos->y),
127  &(speed->x), &(speed->y),
128  &(accel->x), &(accel->y),
138 }
139 
140 static void send_href(struct transport_tx *trans, struct link_device *dev)
141 {
142  pprz_msg_send_GUIDANCE_H_REF_INT(trans, dev, AC_ID,
148  &guidance_h.ref.accel.y);
149 }
150 
151 static void send_tune_hover(struct transport_tx *trans, struct link_device *dev)
152 {
153  pprz_msg_send_ROTORCRAFT_TUNE_HOVER(trans, dev, AC_ID,
157  &stabilization_cmd[COMMAND_ROLL],
158  &stabilization_cmd[COMMAND_PITCH],
159  &stabilization_cmd[COMMAND_YAW],
160  &stabilization_cmd[COMMAND_THRUST],
161  &(stateGetNedToBodyEulers_i()->phi),
162  &(stateGetNedToBodyEulers_i()->theta),
163  &(stateGetNedToBodyEulers_i()->psi));
164 }
165 
166 #endif
167 
168 void guidance_h_init(void)
169 {
170 
174 
178  guidance_h.sp.heading = 0;
179  guidance_h.gains.p = GUIDANCE_H_PGAIN;
180  guidance_h.gains.i = GUIDANCE_H_IGAIN;
181  guidance_h.gains.d = GUIDANCE_H_DGAIN;
184  transition_percentage = 0;
185  transition_theta_offset = 0;
186 
187  gh_ref_init();
188 
189 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
191 #endif
192 
193 #if PERIODIC_TELEMETRY
194  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_H_INT, send_gh);
195  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HOVER_LOOP, send_hover_loop);
196  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_H_REF_INT, send_href);
197  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_TUNE_HOVER, send_tune_hover);
198 #endif
199 
200 #if GUIDANCE_INDI
202 #endif
203 }
204 
205 
207 {
212 
214 }
215 
217 {
218  if (new_mode == guidance_h.mode) {
219  return;
220  }
221 
222  if (new_mode != GUIDANCE_H_MODE_FORWARD && new_mode != GUIDANCE_H_MODE_RATE) {
223  transition_percentage = 0;
224  transition_theta_offset = 0;
225  }
226 
227  switch (new_mode) {
230  break;
231 
234  break;
235 
240 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
241  /* reset attitude stabilization if previous mode was not using it */
245 #endif
247  break;
248 
250 #if GUIDANCE_INDI
252 #endif
254 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
255  /* reset attitude stabilization if previous mode was not using it */
259 #endif
261  break;
262 
263 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
266  break;
267 #endif
268 
269  case GUIDANCE_H_MODE_NAV:
271 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
272  /* reset attitude stabilization if previous mode was not using it */
276 #endif
278  break;
279 
282  break;
283 
284  default:
285  break;
286  }
287 
288  guidance_h.mode = new_mode;
289 
290 }
291 
292 
293 void guidance_h_read_rc(bool_t in_flight)
294 {
295 
296  switch (guidance_h.mode) {
297 
300  break;
301 
303 #if SWITCH_STICKS_FOR_RATE_CONTROL
305 #else
307 #endif
308  break;
311  break;
314  break;
317  break;
320 #if GUIDANCE_H_USE_SPEED_REF
322 #endif
323  break;
324 
325 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
328  break;
329 #endif
330 
331  case GUIDANCE_H_MODE_NAV:
332  if (radio_control.status == RC_OK) {
334  } else {
336  }
337  break;
340  break;
341  default:
342  break;
343  }
344 
345 }
346 
347 void guidance_h_run(bool_t in_flight)
348 {
349  switch (guidance_h.mode) {
350 
352  stabilization_none_run(in_flight);
353  break;
354 
356  stabilization_rate_run(in_flight);
357  break;
358 
360  if (transition_percentage < (100 << INT32_PERCENTAGE_FRAC)) {
361  transition_run();
362  }
365  stabilization_attitude_run(in_flight);
366  break;
367 
369  /* set psi command from RC */
371  /* fall trough to GUIDED to update ref, run traj and set final attitude setpoint */
372 
374  /* guidance_h.sp.pos and guidance_h.sp.heading need to be set from external source */
375  if (!in_flight) {
377  }
378 
380 
381 #if GUIDANCE_INDI
383 #else
384  /* compute x,y earth commands */
385  guidance_h_traj_run(in_flight);
386  /* set final attitude setpoint */
388 #endif
389  stabilization_attitude_run(in_flight);
390  break;
391 
392  case GUIDANCE_H_MODE_NAV:
393  if (!in_flight) {
395  }
396 
398  struct Int32Eulers sp_cmd_i;
399  sp_cmd_i.phi = nav_roll;
400  sp_cmd_i.theta = nav_pitch;
401  sp_cmd_i.psi = nav_heading;
403  } else {
405 
407 
408  /* set psi command */
411 #if GUIDANCE_INDI
413 #else
414  /* compute x,y earth commands */
415  guidance_h_traj_run(in_flight);
416  /* set final attitude setpoint */
419 #endif
420  }
421  stabilization_attitude_run(in_flight);
422  break;
423 
424 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
426  guidance_h_module_run(in_flight);
427  break;
428 #endif
429 
432  break;
433 
434  default:
435  break;
436  }
437 }
438 
439 
441 {
442  /* compute reference even if usage temporarily disabled via guidance_h_use_ref */
443 #if GUIDANCE_H_USE_REF
444 #if GUIDANCE_H_USE_SPEED_REF
447  } else
448 #endif
450 #endif
451 
452  /* either use the reference or simply copy the pos setpoint */
453  if (guidance_h.use_ref) {
454  /* convert our reference to generic representation */
458  } else {
462  }
463 
464 #if GUIDANCE_H_USE_SPEED_REF
466  VECT2_COPY(guidance_h.sp.pos, guidance_h.ref.pos); // for display only
467  }
468 #endif
469 }
470 
471 
472 #define MAX_POS_ERR POS_BFP_OF_REAL(16.)
473 #define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)
474 
475 #ifndef GUIDANCE_H_THRUST_CMD_FILTER
476 #define GUIDANCE_H_THRUST_CMD_FILTER 10
477 #endif
478 
479 /* with a pgain of 100 and a scale of 2,
480  * you get an angle of 5.6 degrees for 1m pos error */
481 #define GH_GAIN_SCALE 2
482 
483 #if !GUIDANCE_INDI
484 static void guidance_h_traj_run(bool_t in_flight)
485 {
486  /* maximum bank angle: default 20 deg, max 40 deg*/
487  static const int32_t traj_max_bank = Min(BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC),
488  BFP_OF_REAL(RadOfDeg(40), INT32_ANGLE_FRAC));
489  static const int32_t total_max_bank = BFP_OF_REAL(RadOfDeg(45), INT32_ANGLE_FRAC);
490 
491  /* compute position error */
493  /* saturate it */
495 
496  /* compute speed error */
498  /* saturate it */
500 
501  /* run PID */
502  int32_t pd_x =
505  int32_t pd_y =
508  guidance_h_cmd_earth.x = pd_x +
509  ((guidance_h.gains.v * guidance_h.ref.speed.x) >> 17) + /* speed feedforward gain */
510  ((guidance_h.gains.a * guidance_h.ref.accel.x) >> 8); /* acceleration feedforward gain */
511  guidance_h_cmd_earth.y = pd_y +
512  ((guidance_h.gains.v * guidance_h.ref.speed.y) >> 17) + /* speed feedforward gain */
513  ((guidance_h.gains.a * guidance_h.ref.accel.y) >> 8); /* acceleration feedforward gain */
514 
515  /* trim max bank angle from PD */
516  VECT2_STRIM(guidance_h_cmd_earth, -traj_max_bank, traj_max_bank);
517 
518  /* Update pos & speed error integral, zero it if not in_flight.
519  * Integrate twice as fast when not only POS but also SPEED are wrong,
520  * but do not integrate POS errors when the SPEED is already catching up.
521  */
522  if (in_flight) {
523  /* ANGLE_FRAC (12) * GAIN (8) * LOOP_FREQ (9) -> INTEGRATOR HIGH RES ANGLE_FRAX (28) */
526  /* saturate it */
527  VECT2_STRIM(guidance_h_trim_att_integrator, -(traj_max_bank << 16), (traj_max_bank << 16));
528  /* add it to the command */
531  } else {
533  }
534 
535  /* compute a better approximation of force commands by taking thrust into account */
536  if (guidance_h.approx_force_by_thrust && in_flight) {
537  static int32_t thrust_cmd_filt;
538  int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC;
539  thrust_cmd_filt = (thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + vertical_thrust) /
542  thrust_cmd_filt));
544  thrust_cmd_filt));
545  }
546 
547  VECT2_STRIM(guidance_h_cmd_earth, -total_max_bank, total_max_bank);
548 }
549 #endif
550 
551 static void guidance_h_hover_enter(void)
552 {
553  /* set horizontal setpoint to current position */
555 
557 
560 }
561 
562 static void guidance_h_nav_enter(void)
563 {
564  /* horizontal position setpoint from navigation/flightplan */
566 
568 
570 }
571 
572 static inline void transition_run(void)
573 {
574  //Add 0.00625%
575  transition_percentage += 1 << (INT32_PERCENTAGE_FRAC - 4);
576 
577 #ifdef TRANSITION_MAX_OFFSET
578  const int32_t max_offset = ANGLE_BFP_OF_REAL(TRANSITION_MAX_OFFSET);
579  transition_theta_offset = INT_MULT_RSHIFT((transition_percentage << (INT32_ANGLE_FRAC - INT32_PERCENTAGE_FRAC)) / 100,
580  max_offset, INT32_ANGLE_FRAC);
581 #endif
582 }
583 
585 static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flight)
586 {
587  if (in_flight) {
588  // negative pitch is forward
591  DeadBand(rc_x, MAX_PPRZ / 20);
592  DeadBand(rc_y, MAX_PPRZ / 20);
593 
594  // convert input from MAX_PPRZ range to SPEED_BFP
597  //int32_t rc_norm = sqrtf(rc_x * rc_x + rc_y * rc_y);
598  //int32_t max_pprz = rc_norm * MAX_PPRZ / Max(abs(rc_x), abs(rc_y);
599  rc_x = rc_x * max_speed / MAX_PPRZ;
600  rc_y = rc_y * max_speed / MAX_PPRZ;
601 
602  /* Rotate from body to NED frame by negative psi angle */
604  int32_t s_psi, c_psi;
605  PPRZ_ITRIG_SIN(s_psi, psi);
606  PPRZ_ITRIG_COS(c_psi, psi);
607  speed_sp->x = (int32_t)(((int64_t)c_psi * rc_x + (int64_t)s_psi * rc_y) >> INT32_TRIG_FRAC);
608  speed_sp->y = (int32_t)((-(int64_t)s_psi * rc_x + (int64_t)c_psi * rc_y) >> INT32_TRIG_FRAC);
609  } else {
610  speed_sp->x = 0;
611  speed_sp->y = 0;
612  }
613 }
614 
616 {
617  guidance_h.gains.i = igain;
619 }
620 
621 bool_t guidance_h_set_guided_pos(float x, float y)
622 {
626  return TRUE;
627  }
628  return FALSE;
629 }
630 
632 {
636  return TRUE;
637  }
638  return FALSE;
639 }
void guidance_flip_enter(void)
Definition: guidance_flip.c:56
void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp)
int32_t psi
in rad with INT32_ANGLE_FRAC
void guidance_indi_enter(void)
Definition: guidance_indi.c:70
void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel)
struct Int32Vect2 guidance_h_cmd_earth
horizontal guidance command.
Definition: guidance_h.c:95
void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp)
#define GUIDANCE_H_MODE_RC_DIRECT
Definition: guidance_h.h:57
void guidance_h_module_run(bool_t in_flight)
void stabilization_rate_run(bool_t in_flight)
#define Min(x, y)
struct Int32Eulers rc_sp
with INT32_ANGLE_FRAC
Definition: guidance_h.h:100
Generic transmission transport header.
Definition: transport.h:89
#define INT32_ACCEL_FRAC
#define GUIDANCE_H_MODE_MODULE
Definition: guidance_h.h:60
#define GUIDANCE_H_MODE_CARE_FREE
Definition: guidance_h.h:58
#define INT32_SPEED_FRAC
#define RADIO_ROLL
Definition: spektrum_arch.h:43
#define RADIO_YAW
Definition: spektrum_arch.h:45
void guidance_h_set_igain(uint32_t igain)
Definition: guidance_h.c:615
#define INT32_PERCENTAGE_FRAC
Dummy stabilization for rotorcrafts.
Periodic telemetry system header (includes downlink utility and generated code).
General attitude stabilization interface for rotorcrafts.
struct GuidanceHRef gh_ref
void stabilization_rate_enter(void)
int32_t theta
in rad with INT32_ANGLE_FRAC
uint8_t status
Definition: radio_control.h:53
#define INT32_ANGLE_FRAC
#define VECT2_STRIM(_v, _min, _max)
Definition: pprz_algebra.h:109
#define POS_BFP_OF_REAL(_af)
bool_t approx_force_by_thrust
Definition: guidance_h.h:93
Open Loop guidance for making a flip.
void guidance_indi_run(bool_t in_flight, int32_t heading)
Definition: guidance_indi.c:85
Read an attitude setpoint from the RC.
#define GUIDANCE_H_MODE_ATTITUDE
Definition: guidance_h.h:54
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:67
Vertical guidance for rotorcrafts.
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:91
static void transition_run(void)
Definition: guidance_h.c:572
#define GUIDANCE_H_AGAIN
Definition: guidance_h.c:48
struct HorizontalGuidanceReference ref
reference calculated from setpoints
Definition: guidance_h.h:98
void stabilization_attitude_enter(void)
static void reset_guidance_reference_from_current_position(void)
Definition: guidance_h.c:206
signed long long int64_t
Definition: types.h:21
int32_t transition_percentage
Definition: guidance_h.c:81
#define INT32_ANGLE_PI_2
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
Read attitude setpoint from RC as euler angles.
void stabilization_attitude_reset_care_free_heading(void)
reset the heading for care-free mode to current heading
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:79
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:58
#define GUIDANCE_H_VGAIN
Definition: guidance_h.c:52
#define GUIDANCE_H_REF_MAX_SPEED
Default speed saturation.
struct HorizontalGuidanceSetpoint sp
setpoints
Definition: guidance_h.h:97
struct Int32Vect2 accel
with INT32_ACCEL_FRAC
Definition: guidance_h.h:78
struct Int64Vect2 pos
Reference model position.
static void guidance_h_traj_run(bool_t in_flight)
Definition: guidance_h.c:484
#define GUIDANCE_H_USE_SPEED_REF
Use horizontal guidance speed reference.
Definition: guidance_h.h:49
#define FALSE
Definition: std.h:5
Rate stabilization for rotorcrafts.
static void guidance_h_nav_enter(void)
Definition: guidance_h.c:562
int32_t y
East.
bool_t guidance_h_set_guided_heading(float heading)
Set heading setpoint in GUIDED mode.
Definition: guidance_h.c:631
#define BFP_OF_REAL(_vr, _frac)
void stabilization_none_enter(void)
#define PPRZ_ITRIG_SIN(_s, _a)
void guidance_h_module_enter(void)
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:840
#define GUIDANCE_H_MODE_HOVER
Definition: guidance_h.h:55
#define TRUE
Definition: std.h:4
#define MAX_POS_ERR
Definition: guidance_h.c:472
struct Int32Vect2 guidance_h_speed_err
Definition: guidance_h.c:88
static float heading
Definition: ahrs_infrared.c:45
void guidance_h_read_rc(bool_t in_flight)
Definition: guidance_h.c:293
void stabilization_attitude_run(bool_t in_flight)
void stabilization_rate_read_rc(void)
static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flight)
read speed setpoint from RC
Definition: guidance_h.c:585
void guidance_h_module_read_rc(void)
#define RADIO_PITCH
Definition: spektrum_arch.h:44
struct Int32Vect2 speed
with INT32_SPEED_FRAC
Definition: guidance_h.h:77
#define GUIDANCE_H_APPROX_FORCE_BY_THRUST
Definition: guidance_h.c:72
void stabilization_rate_read_rc_switched_sticks(void)
euler angles
void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
#define INT32_VECT2_NED_OF_ENU(_o, _i)
#define INT32_ANGLE_NORMALIZE(_a)
int32_t x
North.
unsigned long uint32_t
Definition: types.h:18
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
void guidance_h_run(bool_t in_flight)
Definition: guidance_h.c:347
void stabilization_none_read_rc(void)
struct RadioControl radio_control
Definition: radio_control.c:30
#define GH_SPEED_REF_FRAC
#define GUIDANCE_H_MODE_NAV
Definition: guidance_h.h:56
#define INT32_POS_FRAC
#define GUIDANCE_H_MAX_BANK
Definition: guidance_h.c:65
#define GUIDANCE_H_MODE_KILL
Definition: guidance_h.h:52
bool_t guidance_h_set_guided_pos(float x, float y)
Set horizontal position setpoint in GUIDED mode.
Definition: guidance_h.c:621
signed long int32_t
Definition: types.h:19
void guidance_h_module_init(void)
#define RC_OK
Definition: radio_control.h:48
#define ANGLE_BFP_OF_REAL(_af)
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:69
#define INT32_TRIG_FRAC
#define GH_ACCEL_REF_FRAC
void gh_ref_init(void)
int32_t heading
with INT32_ANGLE_FRAC
Definition: guidance_h.h:72
int32_t phi
in rad with INT32_ANGLE_FRAC
#define INT_EULERS_ZERO(_e)
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
struct HorizontalGuidanceGains gains
Definition: guidance_h.h:95
vector in North East Down coordinates
struct Int32Vect2 speed
Reference model speed.
#define INT_MULT_RSHIFT(_a, _b, _r)
int32_t guidance_v_thrust_coeff
Definition: guidance_v.c:130
static void guidance_h_update_reference(void)
Definition: guidance_h.c:440
void guidance_h_mode_changed(uint8_t new_mode)
Definition: guidance_h.c:216
int32_t transition_theta_offset
Definition: guidance_h.c:82
General stabilization interface for rotorcrafts.
Horizontal guidance for rotorcrafts.
void guidance_flip_run(void)
Definition: guidance_flip.c:65
#define GUIDANCE_H_THRUST_CMD_FILTER
Definition: guidance_h.c:476
struct Int32Vect2 pos
with INT32_POS_FRAC
Definition: guidance_h.h:76
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:28
void stabilization_none_run(bool_t in_flight)
#define INT_VECT2_ZERO(_v)
Guidance in a module file.
struct Int32Vect2 accel
Reference model acceleration.
#define MAX_SPEED_ERR
Definition: guidance_h.c:473
#define SPEED_BFP_OF_REAL(_af)
struct Int32Vect2 guidance_h_pos_err
Definition: guidance_h.c:87
#define MAX_PPRZ
Definition: paparazzi.h:8
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:648
#define INT32_VECT2_RSHIFT(_o, _i, _r)
#define INT32_VECT2_LSHIFT(_o, _i, _l)
#define GUIDANCE_H_MODE_FORWARD
Definition: guidance_h.h:59
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1096
#define GUIDANCE_H_USE_REF
Use horizontal guidance reference trajectory.
Definition: guidance_h.h:41
#define GUIDANCE_H_MODE_RATE
Definition: guidance_h.h:53
#define GH_POS_REF_FRAC
#define GUIDANCE_H_MODE_FLIP
Definition: guidance_h.h:61
struct Int32Vect2 speed
only used if GUIDANCE_H_USE_SPEED_REF
Definition: guidance_h.h:71
#define GH_GAIN_SCALE
Definition: guidance_h.c:481
static void guidance_h_hover_enter(void)
Definition: guidance_h.c:551
void guidance_h_init(void)
Definition: guidance_h.c:168
A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to ICRA2016 to learn more! ...
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
struct Int32Vect2 guidance_h_trim_att_integrator
Definition: guidance_h.c:89
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:991
struct Int32Vect2 pos
horizontal position setpoint in NED.
Definition: guidance_h.h:70
#define PPRZ_ITRIG_COS(_c, _a)
#define GUIDANCE_H_MODE_GUIDED
Definition: guidance_h.h:62