Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 
34 
35 /* for guidance_v_thrust_coeff */
37 
38 #include "state.h"
39 
40 #ifndef GUIDANCE_H_AGAIN
41 #define GUIDANCE_H_AGAIN 0
42 #endif
43 
44 #ifndef GUIDANCE_H_VGAIN
45 #define GUIDANCE_H_VGAIN 0
46 #endif
47 
48 /* error if some gains are negative */
49 #if (GUIDANCE_H_PGAIN < 0) || \
50  (GUIDANCE_H_DGAIN < 0) || \
51  (GUIDANCE_H_IGAIN < 0) || \
52  (GUIDANCE_H_AGAIN < 0) || \
53  (GUIDANCE_H_VGAIN < 0)
54 #error "ALL control gains have to be positive!!!"
55 #endif
56 
57 #ifndef GUIDANCE_H_MAX_BANK
58 #define GUIDANCE_H_MAX_BANK RadOfDeg(20)
59 #endif
60 
63 
64 #ifndef GUIDANCE_H_APPROX_FORCE_BY_THRUST
65 #define GUIDANCE_H_APPROX_FORCE_BY_THRUST FALSE
66 #endif
67 
68 
72 
77 #if GUIDANCE_H_USE_SPEED_REF
78 struct Int32Vect2 guidance_h_speed_sp;
79 #endif
83 
87 
93 
96 
97 
98 static void guidance_h_update_reference(void);
99 static void guidance_h_traj_run(bool_t in_flight);
100 static void guidance_h_hover_enter(void);
101 static void guidance_h_nav_enter(void);
102 static inline void transition_run(void);
103 static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flight);
104 
105 #if PERIODIC_TELEMETRY
107 
108 static void send_gh(void) {
109  struct NedCoor_i* pos = stateGetPositionNed_i();
110  DOWNLINK_SEND_GUIDANCE_H_INT(DefaultChannel, DefaultDevice,
113  &(pos->x), &(pos->y));
114 }
115 
116 static void send_hover_loop(void) {
117  struct NedCoor_i* pos = stateGetPositionNed_i();
118  struct NedCoor_i* speed = stateGetSpeedNed_i();
119  struct NedCoor_i* accel = stateGetAccelNed_i();
120  DOWNLINK_SEND_HOVER_LOOP(DefaultChannel, DefaultDevice,
123  &(pos->x), &(pos->y),
124  &(speed->x), &(speed->y),
125  &(accel->x), &(accel->y),
134  &guidance_h_heading_sp);
135 }
136 
137 static void send_href(void) {
138  DOWNLINK_SEND_GUIDANCE_H_REF_INT(DefaultChannel, DefaultDevice,
140  &guidance_h_speed_sp.x, &guidance_h_speed_ref.x,
143  &guidance_h_speed_sp.y, &guidance_h_speed_ref.y,
145 }
146 
147 static void send_tune_hover(void) {
148  DOWNLINK_SEND_ROTORCRAFT_TUNE_HOVER(DefaultChannel, DefaultDevice,
152  &stabilization_cmd[COMMAND_ROLL],
153  &stabilization_cmd[COMMAND_PITCH],
154  &stabilization_cmd[COMMAND_YAW],
155  &stabilization_cmd[COMMAND_THRUST],
156  &(stateGetNedToBodyEulers_i()->phi),
157  &(stateGetNedToBodyEulers_i()->theta),
158  &(stateGetNedToBodyEulers_i()->psi));
159 }
160 
161 #endif
162 
163 void guidance_h_init(void) {
164 
165  guidance_h_mode = GUIDANCE_H_MODE_KILL;
166  guidance_h_use_ref = GUIDANCE_H_USE_REF;
167  guidance_h_approx_force_by_thrust = GUIDANCE_H_APPROX_FORCE_BY_THRUST;
168 
172  guidance_h_heading_sp = 0;
173  guidance_h_pgain = GUIDANCE_H_PGAIN;
174  guidance_h_igain = GUIDANCE_H_IGAIN;
175  guidance_h_dgain = GUIDANCE_H_DGAIN;
176  guidance_h_again = GUIDANCE_H_AGAIN;
177  guidance_h_vgain = GUIDANCE_H_VGAIN;
178  transition_percentage = 0;
179  transition_theta_offset = 0;
180 
181 #if PERIODIC_TELEMETRY
182  register_periodic_telemetry(DefaultPeriodic, "GUIDANCE_H_INT", send_gh);
183  register_periodic_telemetry(DefaultPeriodic, "HOVER_LOOP", send_hover_loop);
184  register_periodic_telemetry(DefaultPeriodic, "GUIDANCE_H_REF", send_href);
185  register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_TUNE_HOVER", send_tune_hover);
186 #endif
187 }
188 
189 
195 
197 }
198 
200  if (new_mode == guidance_h_mode)
201  return;
202 
203  if (new_mode != GUIDANCE_H_MODE_FORWARD && new_mode != GUIDANCE_H_MODE_RATE) {
204  transition_percentage = 0;
205  transition_theta_offset = 0;
206  }
207 
208  switch (new_mode) {
211  break;
212 
215  break;
216 
221 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
222  /* reset attitude stabilization if previous mode was not using it */
223  if (guidance_h_mode == GUIDANCE_H_MODE_KILL ||
224  guidance_h_mode == GUIDANCE_H_MODE_RATE ||
225  guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT)
226 #endif
228  break;
229 
232 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
233  /* reset attitude stabilization if previous mode was not using it */
234  if (guidance_h_mode == GUIDANCE_H_MODE_KILL ||
235  guidance_h_mode == GUIDANCE_H_MODE_RATE ||
236  guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT)
237 #endif
239  break;
240 
241  case GUIDANCE_H_MODE_NAV:
243 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
244  /* reset attitude stabilization if previous mode was not using it */
245  if (guidance_h_mode == GUIDANCE_H_MODE_KILL ||
246  guidance_h_mode == GUIDANCE_H_MODE_RATE ||
247  guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT)
248 #endif
250  break;
251 
252  default:
253  break;
254  }
255 
256  guidance_h_mode = new_mode;
257 
258 }
259 
260 
261 void guidance_h_read_rc(bool_t in_flight) {
262 
263  switch ( guidance_h_mode ) {
264 
267  break;
268 
270 #if SWITCH_STICKS_FOR_RATE_CONTROL
272 #else
274 #endif
275  break;
278  break;
281  break;
284  break;
287 #if GUIDANCE_H_USE_SPEED_REF
288  read_rc_setpoint_speed_i(&guidance_h_speed_sp, in_flight);
289 #endif
290  break;
291 
292  case GUIDANCE_H_MODE_NAV:
293  if (radio_control.status == RC_OK) {
295  }
296  else {
298  }
299  break;
300  default:
301  break;
302  }
303 
304 }
305 
306 
307 void guidance_h_run(bool_t in_flight) {
308  switch ( guidance_h_mode ) {
309 
311  stabilization_none_run(in_flight);
312  break;
313 
315  stabilization_rate_run(in_flight);
316  break;
317 
319  if(transition_percentage < (100<<INT32_PERCENTAGE_FRAC)) {
320  transition_run();
321  }
324  stabilization_attitude_run(in_flight);
325  break;
326 
328  if (!in_flight)
330 
332 
333  /* set psi command */
334  guidance_h_heading_sp = guidance_h_rc_sp.psi;
335  /* compute x,y earth commands */
336  guidance_h_traj_run(in_flight);
337  /* set final attitude setpoint */
339  guidance_h_heading_sp);
340  stabilization_attitude_run(in_flight);
341  break;
342 
343  case GUIDANCE_H_MODE_NAV:
344  if (!in_flight)
346 
348  struct Int32Eulers sp_cmd_i;
349  sp_cmd_i.phi = nav_roll;
350  sp_cmd_i.theta = nav_pitch;
354  sp_cmd_i.psi = stateGetNedToBodyEulers_i()->psi;
356  }
357  else {
359 
361 
362  /* set psi command */
363  guidance_h_heading_sp = nav_heading;
364  INT32_ANGLE_NORMALIZE(guidance_h_heading_sp);
365  /* compute x,y earth commands */
366  guidance_h_traj_run(in_flight);
367  /* set final attitude setpoint */
369  guidance_h_heading_sp);
370  }
371  stabilization_attitude_run(in_flight);
372  break;
373 
374  default:
375  break;
376  }
377 }
378 
379 
380 static void guidance_h_update_reference(void) {
381  /* compute reference even if usage temporarily disabled via guidance_h_use_ref */
382 #if GUIDANCE_H_USE_REF
383 #if GUIDANCE_H_USE_SPEED_REF
384  if(guidance_h_mode == GUIDANCE_H_MODE_HOVER)
385  gh_update_ref_from_speed_sp(guidance_h_speed_sp);
386  else
387 #endif
389 #endif
390 
391  /* either use the reference or simply copy the pos setpoint */
392  if (guidance_h_use_ref) {
393  /* convert our reference to generic representation */
397  } else {
401  }
402 
403 #if GUIDANCE_H_USE_SPEED_REF
404  if (guidance_h_mode == GUIDANCE_H_MODE_HOVER) {
405  VECT2_COPY(guidance_h_pos_sp, guidance_h_pos_ref); // for display only
406  }
407 #endif
408 }
409 
410 
411 #define MAX_POS_ERR POS_BFP_OF_REAL(16.)
412 #define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)
413 
414 #ifndef GUIDANCE_H_THRUST_CMD_FILTER
415 #define GUIDANCE_H_THRUST_CMD_FILTER 10
416 #endif
417 
418 /* with a pgain of 100 and a scale of 2,
419  * you get an angle of 5.6 degrees for 1m pos error */
420 #define GH_GAIN_SCALE 2
421 
422 static void guidance_h_traj_run(bool_t in_flight) {
423  /* maximum bank angle: default 20 deg, max 40 deg*/
424  static const int32_t traj_max_bank = Max(BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC),
425  BFP_OF_REAL(RadOfDeg(40), INT32_ANGLE_FRAC));
426  static const int32_t total_max_bank = BFP_OF_REAL(RadOfDeg(45), INT32_ANGLE_FRAC);
427 
428  /* compute position error */
430  /* saturate it */
432 
433  /* compute speed error */
435  /* saturate it */
437 
438  /* run PID */
439  int32_t pd_x =
440  ((guidance_h_pgain * guidance_h_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
441  ((guidance_h_dgain * (guidance_h_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2));
442  int32_t pd_y =
443  ((guidance_h_pgain * guidance_h_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
444  ((guidance_h_dgain * (guidance_h_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2));
446  pd_x +
447  ((guidance_h_vgain * guidance_h_speed_ref.x) >> 17) + /* speed feedforward gain */
448  ((guidance_h_again * guidance_h_accel_ref.x) >> 8); /* acceleration feedforward gain */
450  pd_y +
451  ((guidance_h_vgain * guidance_h_speed_ref.y) >> 17) + /* speed feedforward gain */
452  ((guidance_h_again * guidance_h_accel_ref.y) >> 8); /* acceleration feedforward gain */
453 
454  /* trim max bank angle from PD */
455  VECT2_STRIM(guidance_h_cmd_earth, -traj_max_bank, traj_max_bank);
456 
457  /* Update pos & speed error integral, zero it if not in_flight.
458  * Integrate twice as fast when not only POS but also SPEED are wrong,
459  * but do not integrate POS errors when the SPEED is already catching up.
460  */
461  if (in_flight) {
462  /* ANGLE_FRAC (12) * GAIN (8) * LOOP_FREQ (9) -> INTEGRATOR HIGH RES ANGLE_FRAX (28) */
463  guidance_h_trim_att_integrator.x += (guidance_h_igain * pd_x);
464  guidance_h_trim_att_integrator.y += (guidance_h_igain * pd_y);
465  /* saturate it */
466  VECT2_STRIM(guidance_h_trim_att_integrator, -(traj_max_bank << 16), (traj_max_bank << 16));
467  /* add it to the command */
470  } else {
472  }
473 
474  /* compute a better approximation of force commands by taking thrust into account */
475  if (guidance_h_approx_force_by_thrust && in_flight) {
476  static int32_t thrust_cmd_filt;
477  int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC;
478  thrust_cmd_filt = (thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + vertical_thrust) / (GUIDANCE_H_THRUST_CMD_FILTER + 1);
481  }
482 
483  VECT2_STRIM(guidance_h_cmd_earth, -total_max_bank, total_max_bank);
484 }
485 
486 static void guidance_h_hover_enter(void) {
487 
488  /* set horizontal setpoint to current position */
490 
492 
494 }
495 
496 static void guidance_h_nav_enter(void) {
497 
498  /* horizontal position setpoint from navigation/flightplan */
500 
502 
504 }
505 
506 static inline void transition_run(void) {
507  //Add 0.00625%
508  transition_percentage += 1<<(INT32_PERCENTAGE_FRAC-4);
509 
510 #ifdef TRANSITION_MAX_OFFSET
511  const int32_t max_offset = ANGLE_BFP_OF_REAL(TRANSITION_MAX_OFFSET);
512  transition_theta_offset = INT_MULT_RSHIFT((transition_percentage<<(INT32_ANGLE_FRAC-INT32_PERCENTAGE_FRAC))/100, max_offset, INT32_ANGLE_FRAC);
513 #endif
514 }
515 
517 static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flight) {
518  if (in_flight) {
519  // negative pitch is forward
522  DeadBand(rc_x, MAX_PPRZ/20);
523  DeadBand(rc_y, MAX_PPRZ/20);
524 
525  // convert input from MAX_PPRZ range to SPEED_BFP
528  //int32_t rc_norm = sqrtf(rc_x * rc_x + rc_y * rc_y);
529  //int32_t max_pprz = rc_norm * MAX_PPRZ / Max(abs(rc_x), abs(rc_y);
530  rc_x = rc_x * max_speed / MAX_PPRZ;
531  rc_y = rc_y * max_speed / MAX_PPRZ;
532 
533  /* Rotate from body to NED frame by negative psi angle */
535  int32_t s_psi, c_psi;
536  PPRZ_ITRIG_SIN(s_psi, psi);
537  PPRZ_ITRIG_COS(c_psi, psi);
538  speed_sp->x = (int32_t)(( (int64_t)c_psi * rc_x + (int64_t)s_psi * rc_y) >> INT32_TRIG_FRAC);
539  speed_sp->y = (int32_t)((-(int64_t)s_psi * rc_x + (int64_t)c_psi * rc_y) >> INT32_TRIG_FRAC);
540  }
541  else {
542  speed_sp->x = 0;
543  speed_sp->y = 0;
544  }
545 }
void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp)
#define INT32_PERCENTAGE_FRAC
int32_t phi
in rad with INT32_ANGLE_FRAC
void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel)
#define INT32_VECT2_RSHIFT(_o, _i, _r)
struct Int32Vect2 guidance_h_cmd_earth
horizontal guidance command.
Definition: guidance_h.c:84
int32_t guidance_h_dgain
Definition: guidance_h.c:89
void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp)
#define GUIDANCE_H_MODE_RC_DIRECT
Definition: guidance_h.h:57
void stabilization_rate_run(bool_t in_flight)
#define INT_MULT_RSHIFT(_a, _b, _r)
#define INT32_VECT2_NED_OF_ENU(_o, _i)
#define GUIDANCE_H_MODE_CARE_FREE
Definition: guidance_h.h:58
int32_t guidance_h_vgain
Definition: guidance_h.c:92
#define ANGLE_BFP_OF_REAL(_af)
Periodic telemetry system header (includes downlink utility and generated code).
#define INT32_POS_FRAC
void stabilization_rate_enter(void)
struct Int64Vect2 gh_pos_ref
Read an attitude setpoint from the RC.
#define GUIDANCE_H_MODE_ATTITUDE
Definition: guidance_h.h:54
struct Int32Vect2 gh_accel_ref
Reference model acceleration.
int32_t x
North.
Vertical guidance for rotorcrafts.
static void transition_run(void)
Definition: guidance_h.c:506
int32_t theta
in rad with INT32_ANGLE_FRAC
#define GUIDANCE_H_AGAIN
Definition: guidance_h.c:41
#define INT_EULERS_ZERO(_e)
void stabilization_attitude_enter(void)
static void reset_guidance_reference_from_current_position(void)
Definition: guidance_h.c:190
signed long long int64_t
Definition: types.h:21
int32_t transition_percentage
Definition: guidance_h.c:94
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
#define INT32_TRIG_FRAC
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
bool_t guidance_h_use_ref
Definition: guidance_h.c:70
#define GUIDANCE_H_VGAIN
Definition: guidance_h.c:45
#define GUIDANCE_H_REF_MAX_SPEED
Default speed saturation.
static void guidance_h_traj_run(bool_t in_flight)
Definition: guidance_h.c:422
#define GUIDANCE_H_USE_SPEED_REF
Use horizontal guidance speed reference.
Definition: guidance_h.h:49
#define FALSE
Definition: imu_chimu.h:141
#define RADIO_PITCH
Definition: spektrum_arch.h:45
struct Int32Vect2 guidance_h_pos_ref
with INT32_POS_FRAC
Definition: guidance_h.c:74
static void guidance_h_nav_enter(void)
Definition: guidance_h.c:496
vector in North East Down coordinates
void stabilization_none_enter(void)
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:40
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:798
#define GUIDANCE_H_MODE_HOVER
Definition: guidance_h.h:55
struct Int32Vect2 guidance_h_accel_ref
with INT32_ACCEL_FRAC
Definition: guidance_h.c:76
#define MAX_POS_ERR
Definition: guidance_h.c:411
int32_t guidance_h_heading_sp
with INT32_ANGLE_FRAC
Definition: guidance_h.c:86
struct Int32Vect2 guidance_h_speed_err
Definition: guidance_h.c:81
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:55
void guidance_h_read_rc(bool_t in_flight)
Definition: guidance_h.c:261
#define Max(x, y)
int32_t guidance_h_igain
Definition: guidance_h.c:90
struct Int32Eulers guidance_h_rc_sp
with INT32_ANGLE_FRAC
Definition: guidance_h.c:85
#define INT32_ANGLE_FRAC
#define INT32_ANGLE_PI_2
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:74
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:517
bool_t guidance_h_approx_force_by_thrust
Definition: guidance_h.c:71
#define GUIDANCE_H_APPROX_FORCE_BY_THRUST
Definition: guidance_h.c:65
void stabilization_rate_read_rc_switched_sticks(void)
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)
struct Int32Vect2 guidance_h_pos_sp
horizontal position setpoint in NED.
Definition: guidance_h.c:73
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
#define BFP_OF_REAL(_vr, _frac)
void guidance_h_run(bool_t in_flight)
Definition: guidance_h.c:307
void stabilization_none_read_rc(void)
struct RadioControl radio_control
Definition: radio_control.c:25
int32_t guidance_h_again
Definition: guidance_h.c:91
#define GH_SPEED_REF_FRAC
#define GUIDANCE_H_MODE_NAV
Definition: guidance_h.h:56
int32_t y
East.
#define GUIDANCE_H_MAX_BANK
Definition: guidance_h.c:58
#define INT32_ANGLE_NORMALIZE(_a)
uint8_t guidance_h_mode
Definition: guidance_h.c:69
#define RADIO_YAW
Definition: spektrum_arch.h:48
#define GUIDANCE_H_MODE_KILL
Definition: guidance_h.h:52
signed long int32_t
Definition: types.h:19
#define TRUE
Definition: imu_chimu.h:144
#define RC_OK
Definition: radio_control.h:45
uint8_t status
Definition: radio_control.h:50
#define GH_ACCEL_REF_FRAC
#define INT32_VECT2_LSHIFT(_o, _i, _l)
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
int32_t guidance_v_thrust_coeff
Definition: guidance_v.c:129
static void guidance_h_update_reference(void)
Definition: guidance_h.c:380
void guidance_h_mode_changed(uint8_t new_mode)
Definition: guidance_h.c:199
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t transition_theta_offset
Definition: guidance_h.c:95
General stabilization interface for rotorcrafts.
Horizontal guidance for rotorcrafts.
#define GUIDANCE_H_THRUST_CMD_FILTER
Definition: guidance_h.c:415
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)
int32_t guidance_h_pgain
Definition: guidance_h.c:88
#define MAX_SPEED_ERR
Definition: guidance_h.c:412
struct Int32Vect2 guidance_h_pos_err
Definition: guidance_h.c:80
#define INT32_SPEED_FRAC
struct Int32Vect2 guidance_h_speed_ref
with INT32_SPEED_FRAC
Definition: guidance_h.c:75
#define MAX_PPRZ
Definition: paparazzi.h:8
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:630
#define RADIO_ROLL
Definition: spektrum_arch.h:42
#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:1012
#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 INT32_ACCEL_FRAC
#define GH_POS_REF_FRAC
#define VECT2_STRIM(_v, _min, _max)
Definition: pprz_algebra.h:92
#define SPEED_BFP_OF_REAL(_af)
#define GH_GAIN_SCALE
Definition: guidance_h.c:420
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:50
static void guidance_h_hover_enter(void)
Definition: guidance_h.c:486
void guidance_h_init(void)
Definition: guidance_h.c:163
struct Int32Vect2 guidance_h_trim_att_integrator
Definition: guidance_h.c:82
struct Int32Vect2 gh_speed_ref
Reference model speed.
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:924
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:50
euler angles