Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
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 
38 
42 
43 /* for guidance_v_thrust_coeff */
45 
46 #include "state.h"
47 
48 #ifndef GUIDANCE_H_AGAIN
49 #define GUIDANCE_H_AGAIN 0
50 #endif
51 
52 #ifndef GUIDANCE_H_VGAIN
53 #define GUIDANCE_H_VGAIN 0
54 #endif
55 
56 /* error if some gains are negative */
57 #if (GUIDANCE_H_PGAIN < 0) || \
58  (GUIDANCE_H_DGAIN < 0) || \
59  (GUIDANCE_H_IGAIN < 0) || \
60  (GUIDANCE_H_AGAIN < 0) || \
61  (GUIDANCE_H_VGAIN < 0)
62 #error "ALL control gains have to be positive!!!"
63 #endif
64 
65 #ifndef GUIDANCE_H_MAX_BANK
66 #define GUIDANCE_H_MAX_BANK RadOfDeg(20)
67 #endif
68 
69 PRINT_CONFIG_VAR(GUIDANCE_H_USE_REF)
70 PRINT_CONFIG_VAR(GUIDANCE_H_USE_SPEED_REF)
71 
72 #ifndef GUIDANCE_H_APPROX_FORCE_BY_THRUST
73 #define GUIDANCE_H_APPROX_FORCE_BY_THRUST FALSE
74 #endif
75 
76 #ifndef GUIDANCE_INDI
77 #define GUIDANCE_INDI FALSE
78 #endif
79 
81 
84 
85 /*
86  * internal variables
87  */
91 
97 
98 static void guidance_h_update_reference(void);
99 #if !GUIDANCE_INDI
100 static void guidance_h_traj_run(bool in_flight);
101 #endif
102 static void guidance_h_hover_enter(void);
103 static void guidance_h_nav_enter(void);
104 static inline void transition_run(void);
105 static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight);
106 
107 #if PERIODIC_TELEMETRY
109 
110 static void send_gh(struct transport_tx *trans, struct link_device *dev)
111 {
112  struct NedCoor_i *pos = stateGetPositionNed_i();
113  pprz_msg_send_GUIDANCE_H_INT(trans, dev, AC_ID,
116  &(pos->x), &(pos->y));
117 }
118 
119 static void send_hover_loop(struct transport_tx *trans, struct link_device *dev)
120 {
121  struct NedCoor_i *pos = stateGetPositionNed_i();
122  struct NedCoor_i *speed = stateGetSpeedNed_i();
123  struct NedCoor_i *accel = stateGetAccelNed_i();
124  pprz_msg_send_HOVER_LOOP(trans, dev, AC_ID,
125  &guidance_h.sp.pos.x,
126  &guidance_h.sp.pos.y,
127  &(pos->x), &(pos->y),
128  &(speed->x), &(speed->y),
129  &(accel->x), &(accel->y),
139 }
140 
141 static void send_href(struct transport_tx *trans, struct link_device *dev)
142 {
143  pprz_msg_send_GUIDANCE_H_REF_INT(trans, dev, AC_ID,
149  &guidance_h.ref.accel.y);
150 }
151 
152 static void send_tune_hover(struct transport_tx *trans, struct link_device *dev)
153 {
154  pprz_msg_send_ROTORCRAFT_TUNE_HOVER(trans, dev, AC_ID,
158  &stabilization_cmd[COMMAND_ROLL],
159  &stabilization_cmd[COMMAND_PITCH],
160  &stabilization_cmd[COMMAND_YAW],
161  &stabilization_cmd[COMMAND_THRUST],
162  &(stateGetNedToBodyEulers_i()->phi),
163  &(stateGetNedToBodyEulers_i()->theta),
164  &(stateGetNedToBodyEulers_i()->psi));
165 }
166 
167 #endif
168 
169 void guidance_h_init(void)
170 {
171 
175 
179  guidance_h.sp.heading = 0;
181  guidance_h.gains.p = GUIDANCE_H_PGAIN;
182  guidance_h.gains.i = GUIDANCE_H_IGAIN;
183  guidance_h.gains.d = GUIDANCE_H_DGAIN;
186  transition_percentage = 0;
187  transition_theta_offset = 0;
188 
189  gh_ref_init();
190 
191 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
193 #endif
194 
195 #if PERIODIC_TELEMETRY
196  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_H_INT, send_gh);
198  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_H_REF_INT, send_href);
199  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_TUNE_HOVER, send_tune_hover);
200 #endif
201 
202 #if GUIDANCE_INDI
204 #endif
205 
206 #if HYBRID_NAVIGATION
208 #endif
209 }
210 
211 
213 {
218 
220 }
221 
223 {
224  if (new_mode == guidance_h.mode) {
225  return;
226  }
227 
228  if (new_mode != GUIDANCE_H_MODE_FORWARD && new_mode != GUIDANCE_H_MODE_RATE) {
229  transition_percentage = 0;
230  transition_theta_offset = 0;
231  }
232 
233 #if HYBRID_NAVIGATION
235 #endif
236 
237  switch (new_mode) {
240  break;
241 
242 #if USE_STABILIZATION_RATE
245  break;
246 #endif
247 
252 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
253  /* reset attitude stabilization if previous mode was not using it */
257 #endif
259  break;
260 
263 #if GUIDANCE_INDI
265 #endif
267 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
268  /* reset attitude stabilization if previous mode was not using it */
272 #endif
274  break;
275 
276 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
279  break;
280 #endif
281 
282  case GUIDANCE_H_MODE_NAV:
284 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
285  /* reset attitude stabilization if previous mode was not using it */
289 #endif
291  break;
292 
295  break;
296 
297  default:
298  break;
299  }
300 
301  guidance_h.mode = new_mode;
302 
303 }
304 
305 
306 void guidance_h_read_rc(bool in_flight)
307 {
308 
309  switch (guidance_h.mode) {
310 
313  break;
314 
315 #if USE_STABILIZATION_RATE
317 #if SWITCH_STICKS_FOR_RATE_CONTROL
319 #else
321 #endif
322  break;
323 #endif
324 
327  break;
330  break;
333  break;
336 #if GUIDANCE_H_USE_SPEED_REF
338  /* enable x,y velocity setpoints */
339  SetBit(guidance_h.sp.mask, 5);
340 #endif
341  break;
342 
343 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
346  break;
347 #endif
348 
349  case GUIDANCE_H_MODE_NAV:
350  if (radio_control.status == RC_OK) {
352  } else {
354  }
355  break;
358  break;
359  default:
360  break;
361  }
362 
363 }
364 
365 void guidance_h_run(bool in_flight)
366 {
367  switch (guidance_h.mode) {
368 
370  stabilization_none_run(in_flight);
371  break;
372 
373 #if USE_STABILIZATION_RATE
375  stabilization_rate_run(in_flight);
376  break;
377 #endif
378 
380  if (transition_percentage < (100 << INT32_PERCENTAGE_FRAC)) {
381  transition_run();
382  }
385  stabilization_attitude_run(in_flight);
386  break;
387 
389  /* set psi command from RC */
391  /* fall trough to GUIDED to update ref, run traj and set final attitude setpoint */
392 
394  /* guidance_h.sp.pos and guidance_h.sp.heading need to be set from external source */
395  if (!in_flight) {
397  }
398 
400 
401 #if GUIDANCE_INDI
403 #else
404  /* compute x,y earth commands */
405  guidance_h_traj_run(in_flight);
406  /* set final attitude setpoint */
408 #endif
409  stabilization_attitude_run(in_flight);
410  break;
411 
412  case GUIDANCE_H_MODE_NAV:
413  if (!in_flight) {
415  }
416 
418  stabilization_cmd[COMMAND_ROLL] = nav_cmd_roll;
419  stabilization_cmd[COMMAND_PITCH] = nav_cmd_pitch;
420  stabilization_cmd[COMMAND_YAW] = nav_cmd_yaw;
422  struct Int32Eulers sp_cmd_i;
423  sp_cmd_i.phi = nav_roll;
424  sp_cmd_i.theta = nav_pitch;
425  sp_cmd_i.psi = nav_heading;
427  stabilization_attitude_run(in_flight);
428 
429 #if HYBRID_NAVIGATION
430  //make sure the heading is right before leaving horizontal_mode attitude
432 #endif
433  } else {
434 
435 #if HYBRID_NAVIGATION
438 #else
440 
442 
443  /* set psi command */
446 
447 #if GUIDANCE_INDI
449 #else
450  /* compute x,y earth commands */
451  guidance_h_traj_run(in_flight);
452  /* set final attitude setpoint */
455 #endif
456 
457 #endif
458  stabilization_attitude_run(in_flight);
459  }
460  break;
461 
462 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
464  guidance_h_module_run(in_flight);
465  break;
466 #endif
467 
470  break;
471 
472  default:
473  break;
474  }
475 }
476 
477 
479 {
480  /* compute reference even if usage temporarily disabled via guidance_h_use_ref */
481 #if GUIDANCE_H_USE_REF
482  if (bit_is_set(guidance_h.sp.mask, 5)) {
484  } else {
486  }
487 #endif
488 
489  /* either use the reference or simply copy the pos setpoint */
490  if (guidance_h.use_ref) {
491  /* convert our reference to generic representation */
495  } else {
499  }
500 
501 #if GUIDANCE_H_USE_SPEED_REF
503  VECT2_COPY(guidance_h.sp.pos, guidance_h.ref.pos); // for display only
504  }
505 #endif
506 
507  /* update heading setpoint from rate */
508  if (bit_is_set(guidance_h.sp.mask, 7)) {
511  }
512 }
513 
514 #define MAX_POS_ERR POS_BFP_OF_REAL(16.)
515 #define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)
516 
517 #ifndef GUIDANCE_H_THRUST_CMD_FILTER
518 #define GUIDANCE_H_THRUST_CMD_FILTER 10
519 #endif
520 
521 /* with a pgain of 100 and a scale of 2,
522  * you get an angle of 5.6 degrees for 1m pos error */
523 #define GH_GAIN_SCALE 2
524 
525 #if !GUIDANCE_INDI
526 static void guidance_h_traj_run(bool in_flight)
527 {
528  /* maximum bank angle: default 20 deg, max 40 deg*/
529  static const int32_t traj_max_bank = Min(BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC),
530  BFP_OF_REAL(RadOfDeg(40), INT32_ANGLE_FRAC));
531  static const int32_t total_max_bank = BFP_OF_REAL(RadOfDeg(45), INT32_ANGLE_FRAC);
532 
533  /* compute position error */
535  /* saturate it */
537 
538  /* compute speed error */
540  /* saturate it */
542 
543  /* run PID */
544  int32_t pd_x =
547  int32_t pd_y =
550  guidance_h_cmd_earth.x = pd_x +
551  ((guidance_h.gains.v * guidance_h.ref.speed.x) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) + /* speed feedforward gain */
553  GH_GAIN_SCALE)); /* acceleration feedforward gain */
554  guidance_h_cmd_earth.y = pd_y +
555  ((guidance_h.gains.v * guidance_h.ref.speed.y) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) + /* speed feedforward gain */
557  GH_GAIN_SCALE)); /* acceleration feedforward gain */
558 
559  /* trim max bank angle from PD */
560  VECT2_STRIM(guidance_h_cmd_earth, -traj_max_bank, traj_max_bank);
561 
562  /* Update pos & speed error integral, zero it if not in_flight.
563  * Integrate twice as fast when not only POS but also SPEED are wrong,
564  * but do not integrate POS errors when the SPEED is already catching up.
565  */
566  if (in_flight) {
567  /* ANGLE_FRAC (12) * GAIN (8) * LOOP_FREQ (9) -> INTEGRATOR HIGH RES ANGLE_FRAX (28) */
570  /* saturate it */
572  (traj_max_bank << (INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2)));
573  /* add it to the command */
576  } else {
578  }
579 
580  /* compute a better approximation of force commands by taking thrust into account */
581  if (guidance_h.approx_force_by_thrust && in_flight) {
582  static int32_t thrust_cmd_filt;
583  int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC;
584  thrust_cmd_filt = (thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + vertical_thrust) /
587  thrust_cmd_filt));
589  thrust_cmd_filt));
590  }
591 
592  VECT2_STRIM(guidance_h_cmd_earth, -total_max_bank, total_max_bank);
593 }
594 #endif
595 
596 static void guidance_h_hover_enter(void)
597 {
598  /* reset speed setting */
599  guidance_h.sp.speed.x = 0;
600  guidance_h.sp.speed.y = 0;
601 
602  /* disable horizontal velocity setpoints,
603  * might still be activated in guidance_h_read_rc if GUIDANCE_H_USE_SPEED_REF
604  */
605  ClearBit(guidance_h.sp.mask, 5);
606  ClearBit(guidance_h.sp.mask, 7);
607 
608  /* set horizontal setpoint to current position */
610 
611  /* reset guidance reference */
613 
614  /* set guidance to current heading and position */
617 }
618 
619 static void guidance_h_nav_enter(void)
620 {
621  ClearBit(guidance_h.sp.mask, 5);
622  ClearBit(guidance_h.sp.mask, 7);
623 
624  /* horizontal position setpoint from navigation/flightplan */
626 
628 
630 }
631 
632 static inline void transition_run(void)
633 {
634  //Add 0.00625%
635  transition_percentage += 1 << (INT32_PERCENTAGE_FRAC - 4);
636 
637 #ifdef TRANSITION_MAX_OFFSET
638  const int32_t max_offset = ANGLE_BFP_OF_REAL(TRANSITION_MAX_OFFSET);
639  transition_theta_offset = INT_MULT_RSHIFT((transition_percentage << (INT32_ANGLE_FRAC - INT32_PERCENTAGE_FRAC)) / 100,
640  max_offset, INT32_ANGLE_FRAC);
641 #endif
642 }
643 
645 static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight)
646 {
647  if (in_flight) {
648  // negative pitch is forward
651  DeadBand(rc_x, MAX_PPRZ / 20);
652  DeadBand(rc_y, MAX_PPRZ / 20);
653 
654  // convert input from MAX_PPRZ range to SPEED_BFP
657  //int32_t rc_norm = sqrtf(rc_x * rc_x + rc_y * rc_y);
658  //int32_t max_pprz = rc_norm * MAX_PPRZ / Max(abs(rc_x), abs(rc_y);
659  rc_x = rc_x * max_speed / MAX_PPRZ;
660  rc_y = rc_y * max_speed / MAX_PPRZ;
661 
662  /* Rotate from body to NED frame by negative psi angle */
664  int32_t s_psi, c_psi;
665  PPRZ_ITRIG_SIN(s_psi, psi);
666  PPRZ_ITRIG_COS(c_psi, psi);
667  speed_sp->x = (int32_t)(((int64_t)c_psi * rc_x + (int64_t)s_psi * rc_y) >> INT32_TRIG_FRAC);
668  speed_sp->y = (int32_t)((-(int64_t)s_psi * rc_x + (int64_t)c_psi * rc_y) >> INT32_TRIG_FRAC);
669  } else {
670  speed_sp->x = 0;
671  speed_sp->y = 0;
672  }
673 }
674 
676 {
677  guidance_h.gains.i = igain;
679 }
680 
681 bool guidance_h_set_guided_pos(float x, float y)
682 {
684  ClearBit(guidance_h.sp.mask, 5);
687  return true;
688  }
689  return false;
690 }
691 
693 {
695  ClearBit(guidance_h.sp.mask, 7);
698  return true;
699  }
700  return false;
701 }
702 
703 bool guidance_h_set_guided_body_vel(float vx, float vy)
704 {
705  float psi = stateGetNedToBodyEulers_f()->psi;
706  float newvx = cosf(-psi) * vx + sinf(-psi) * vy;
707  float newvy = -sinf(-psi) * vx + cosf(-psi) * vy;
708  return guidance_h_set_guided_vel(newvx, newvy);
709 }
710 
711 bool guidance_h_set_guided_vel(float vx, float vy)
712 {
714  SetBit(guidance_h.sp.mask, 5);
717  return true;
718  }
719  return false;
720 }
721 
723 {
725  SetBit(guidance_h.sp.mask, 7);
727  return true;
728  }
729  return false;
730 }
731 
733 {
734  return &guidance_h_pos_err;
735 }
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
int32_t heading_rate
with INT32_RATE_FRAC
Definition: guidance_h.h:73
void guidance_indi_enter(void)
Call upon entering indi guidance.
Definition: guidance_indi.c:94
#define INT32_RATE_FRAC
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:96
uint8_t mask
bit 5: vx & vy, bit 6: vz, bit 7: vyaw
Definition: guidance_h.h:74
void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp)
#define GUIDANCE_H_MODE_RC_DIRECT
Definition: guidance_h.h:57
void stabilization_none_run(bool in_flight)
struct Int32Eulers rc_sp
with INT32_ANGLE_FRAC
Definition: guidance_h.h:102
#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
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1125
#define RADIO_YAW
Definition: spektrum_arch.h:45
void guidance_h_read_rc(bool in_flight)
Definition: guidance_h.c:306
void guidance_h_set_igain(uint32_t igain)
Definition: guidance_h.c:675
#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)
void guidance_h_run(bool in_flight)
Definition: guidance_h.c:365
int32_t theta
in rad with INT32_ANGLE_FRAC
uint8_t status
Definition: radio_control.h:53
#define INT32_ANGLE_FRAC
void guidance_hybrid_init(void)
Hybrid Guidance Initialization function.
#define VECT2_STRIM(_v, _min, _max)
Definition: pprz_algebra.h:109
#define POS_BFP_OF_REAL(_af)
Open Loop guidance for making a flip.
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
float psi
in radians
Vertical guidance for rotorcrafts.
bool guidance_h_set_guided_body_vel(float vx, float vy)
Set body relative horizontal velocity setpoint in GUIDED mode.
Definition: guidance_h.c:703
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:91
static void transition_run(void)
Definition: guidance_h.c:632
#define GUIDANCE_H_AGAIN
Definition: guidance_h.c:49
void stabilization_attitude_run(bool in_flight)
struct HorizontalGuidanceReference ref
reference calculated from setpoints
Definition: guidance_h.h:100
void stabilization_attitude_enter(void)
static void reset_guidance_reference_from_current_position(void)
Definition: guidance_h.c:212
signed long long int64_t
Definition: types.h:21
int32_t transition_percentage
Definition: guidance_h.c:82
#define INT32_ANGLE_PI_2
void stabilization_attitude_reset_care_free_heading(void)
reset the heading for care-free mode to current heading
static void send_tune_hover(struct transport_tx *trans, struct link_device *dev)
Definition: guidance_h.c:152
Guidance controllers (horizontal and vertical) for Hybrid UAV configurations.
void guidance_hybrid_run(void)
Runs the Hybrid Guidance main functions.
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:80
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:58
#define GUIDANCE_H_VGAIN
Definition: guidance_h.c:53
#define GUIDANCE_H_REF_MAX_SPEED
Default speed saturation.
struct HorizontalGuidanceSetpoint sp
setpoints
Definition: guidance_h.h:99
struct Int32Vect2 accel
with INT32_ACCEL_FRAC
Definition: guidance_h.h:80
struct Int64Vect2 pos
Reference model position.
#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:619
static void guidance_h_traj_run(bool in_flight)
Definition: guidance_h.c:526
int32_t y
East.
#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:851
bool guidance_h_set_guided_heading(float heading)
Set heading setpoint in GUIDED mode.
Definition: guidance_h.c:692
#define GUIDANCE_H_MODE_HOVER
Definition: guidance_h.h:55
#define TRUE
Definition: std.h:4
#define MAX_POS_ERR
Definition: guidance_h.c:514
static void send_href(struct transport_tx *trans, struct link_device *dev)
Definition: guidance_h.c:141
struct Int32Vect2 guidance_h_speed_err
Definition: guidance_h.c:89
static float heading
Definition: ahrs_infrared.c:45
static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight)
read speed setpoint from RC
Definition: guidance_h.c:645
void stabilization_rate_read_rc(void)
void guidance_h_module_read_rc(void)
#define RADIO_PITCH
Definition: spektrum_arch.h:44
bool guidance_h_set_guided_pos(float x, float y)
Set horizontal position setpoint in GUIDED mode.
Definition: guidance_h.c:681
struct Int32Vect2 speed
with INT32_SPEED_FRAC
Definition: guidance_h.h:79
#define GUIDANCE_H_APPROX_FORCE_BY_THRUST
Definition: guidance_h.c:73
void stabilization_rate_read_rc_switched_sticks(void)
euler angles
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)
void guidance_hybrid_reset_heading(struct Int32Eulers *sp_cmd)
Description.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
static void send_hover_loop(struct transport_tx *trans, struct link_device *dev)
Definition: guidance_h.c:119
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:66
int32_t guidance_hybrid_norm_ref_airspeed
#define GUIDANCE_H_MODE_KILL
Definition: guidance_h.h:52
#define Min(x, y)
Definition: main_fbw.c:52
static void send_gh(struct transport_tx *trans, struct link_device *dev)
Definition: guidance_h.c:110
bool guidance_h_set_guided_heading_rate(float rate)
Set heading rate setpoint in GUIDED mode.
Definition: guidance_h.c:722
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:73
#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:97
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:131
static void guidance_h_update_reference(void)
Definition: guidance_h.c:478
void guidance_h_mode_changed(uint8_t new_mode)
Definition: guidance_h.c:222
int32_t transition_theta_offset
Definition: guidance_h.c:83
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:518
struct Int32Vect2 pos
with INT32_POS_FRAC
Definition: guidance_h.h:78
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:28
void stabilization_rate_run(bool in_flight)
#define INT_VECT2_ZERO(_v)
Guidance in a module file.
struct Int32Vect2 accel
Reference model acceleration.
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool in_flight, bool in_carefree, bool coordinated_turn)
Read attitude setpoint from RC as euler angles.
bool approx_force_by_thrust
Definition: guidance_h.h:95
void guidance_h_module_run(bool in_flight)
#define MAX_SPEED_ERR
Definition: guidance_h.c:515
#define SPEED_BFP_OF_REAL(_af)
struct Int32Vect2 guidance_h_pos_err
Definition: guidance_h.c:88
void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
#define MAX_PPRZ
Definition: paparazzi.h:8
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:659
#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:1107
const struct Int32Vect2 * guidance_h_get_pos_err(void)
Gets the position error.
Definition: guidance_h.c:732
#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 in HOVER mode if GUIDANCE_H_USE_SPEED_REF or in GUIDED mode
Definition: guidance_h.h:71
void guidance_indi_run(bool in_flight, int32_t heading)
#define GH_GAIN_SCALE
Definition: guidance_h.c:523
bool guidance_h_set_guided_vel(float vx, float vy)
Set horizontal velocity setpoint in GUIDED mode.
Definition: guidance_h.c:711
static void guidance_h_hover_enter(void)
Definition: guidance_h.c:596
void guidance_h_init(void)
Definition: guidance_h.c:169
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:90
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:1002
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
#define RATE_BFP_OF_REAL(_af)