Paparazzi UAS  v5.15_devel-50-g4d7045d
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 
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 in_flight);
100 #endif
101 static inline void transition_run(bool to_forward);
102 static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight);
103 
104 #if PERIODIC_TELEMETRY
106 
107 static void send_gh(struct transport_tx *trans, struct link_device *dev)
108 {
109  struct NedCoor_i *pos = stateGetPositionNed_i();
110  pprz_msg_send_GUIDANCE_H_INT(trans, dev, AC_ID,
113  &(pos->x), &(pos->y));
114 }
115 
116 static void send_hover_loop(struct transport_tx *trans, struct link_device *dev)
117 {
118  struct NedCoor_i *pos = stateGetPositionNed_i();
119  struct NedCoor_i *speed = stateGetSpeedNed_i();
120  struct NedCoor_i *accel = stateGetAccelNed_i();
121  pprz_msg_send_HOVER_LOOP(trans, dev, AC_ID,
122  &guidance_h.sp.pos.x,
123  &guidance_h.sp.pos.y,
124  &(pos->x), &(pos->y),
125  &(speed->x), &(speed->y),
126  &(accel->x), &(accel->y),
136 }
137 
138 static void send_href(struct transport_tx *trans, struct link_device *dev)
139 {
140  pprz_msg_send_GUIDANCE_H_REF_INT(trans, dev, AC_ID,
146  &guidance_h.ref.accel.y);
147 }
148 
149 static void send_tune_hover(struct transport_tx *trans, struct link_device *dev)
150 {
151  pprz_msg_send_ROTORCRAFT_TUNE_HOVER(trans, dev, AC_ID,
155  &stabilization_cmd[COMMAND_ROLL],
156  &stabilization_cmd[COMMAND_PITCH],
157  &stabilization_cmd[COMMAND_YAW],
158  &stabilization_cmd[COMMAND_THRUST],
159  &(stateGetNedToBodyEulers_i()->phi),
160  &(stateGetNedToBodyEulers_i()->theta),
161  &(stateGetNedToBodyEulers_i()->psi));
162 }
163 
164 #endif
165 
166 void guidance_h_init(void)
167 {
168 
172 
176  guidance_h.sp.heading = 0.0;
177  guidance_h.sp.heading_rate = 0.0;
178  guidance_h.gains.p = GUIDANCE_H_PGAIN;
179  guidance_h.gains.i = GUIDANCE_H_IGAIN;
180  guidance_h.gains.d = GUIDANCE_H_DGAIN;
183  transition_percentage = 0;
185 
186  gh_ref_init();
187 
188 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
190 #endif
191 
192 #if PERIODIC_TELEMETRY
193  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_H_INT, send_gh);
195  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_H_REF_INT, send_href);
196  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_TUNE_HOVER, send_tune_hover);
197 #endif
198 
199 }
200 
201 
203 {
208 
210 }
211 
213 {
214  if (new_mode == guidance_h.mode) {
215  return;
216  }
217 
218 #if HYBRID_NAVIGATION
220 #endif
221 
222  switch (new_mode) {
225  break;
226 
227 #if USE_STABILIZATION_RATE
230  break;
231 #endif
232 
235  /* Falls through. */
238 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
239  /* reset attitude stabilization if previous mode was not using it */
243 #endif
245  break;
246 
249 #if GUIDANCE_INDI
251 #endif
253 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
254  /* reset attitude stabilization if previous mode was not using it */
258 #endif
260  break;
261 
262 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
265  break;
266 #endif
267 
268  case GUIDANCE_H_MODE_NAV:
270 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
271  /* reset attitude stabilization if previous mode was not using it */
275 #endif
277  break;
278 
281  break;
282 
283  default:
284  break;
285  }
286 
287  guidance_h.mode = new_mode;
288 
289 }
290 
291 
292 void guidance_h_read_rc(bool in_flight)
293 {
294 
295  switch (guidance_h.mode) {
296 
299  break;
300 
301 #if USE_STABILIZATION_RATE
303 #if SWITCH_STICKS_FOR_RATE_CONTROL
305 #else
307 #endif
308  break;
309 #endif
310 
313  break;
316  break;
319  break;
322 #if GUIDANCE_H_USE_SPEED_REF
324  /* enable x,y velocity setpoints */
325  SetBit(guidance_h.sp.mask, 5);
326 #endif
327  break;
328 
329 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
332  break;
333 #endif
334 
335  case GUIDANCE_H_MODE_NAV:
336  if (radio_control.status == RC_OK) {
338  } else {
340  }
341  break;
344  break;
345  default:
346  break;
347  }
348 
349 }
350 
351 void guidance_h_run(bool in_flight)
352 {
353  switch (guidance_h.mode) {
354 
356  stabilization_none_run(in_flight);
357  break;
358 
359 #if USE_STABILIZATION_RATE
361  stabilization_rate_run(in_flight);
362  break;
363 #endif
364 
366  if (transition_percentage < (100 << INT32_PERCENTAGE_FRAC)) {
367  transition_run(true);
368  }
369  /* Falls through. */
372  if ((!(guidance_h.mode == GUIDANCE_H_MODE_FORWARD)) && transition_percentage > 0) {
373  transition_run(false);
374  }
375  stabilization_attitude_run(in_flight);
376 #if (STABILIZATION_FILTER_CMD_ROLL_PITCH || STABILIZATION_FILTER_CMD_YAW)
377  if (in_flight) {
379  }
380 #endif
381 
382  break;
383 
385  /* set psi command from RC */
387  /* fall trough to GUIDED to update ref, run traj and set final attitude setpoint */
388 
389  /* Falls through. */
391  guidance_h_guided_run(in_flight);
392  break;
393 
394  case GUIDANCE_H_MODE_NAV:
395  guidance_h_from_nav(in_flight);
396  break;
397 
398 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
400  guidance_h_module_run(in_flight);
401  break;
402 #endif
403 
406  break;
407 
408  default:
409  break;
410  }
411 }
412 
413 
415 {
416  /* compute reference even if usage temporarily disabled via guidance_h_use_ref */
417 #if GUIDANCE_H_USE_REF
418  if (bit_is_set(guidance_h.sp.mask, 5)) {
420  } else {
422  }
423 #endif
424 
425  /* either use the reference or simply copy the pos setpoint */
426  if (guidance_h.use_ref) {
427  /* convert our reference to generic representation */
431  } else {
435  }
436 
437 #if GUIDANCE_H_USE_SPEED_REF
439  VECT2_COPY(guidance_h.sp.pos, guidance_h.ref.pos); // for display only
440  }
441 #endif
442 
443  /* update heading setpoint from rate */
444  if (bit_is_set(guidance_h.sp.mask, 7)) {
445  guidance_h.sp.heading += guidance_h.sp.heading_rate / PERIODIC_FREQUENCY;
447  }
448 }
449 
450 #define MAX_POS_ERR POS_BFP_OF_REAL(16.)
451 #define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)
452 
453 #ifndef GUIDANCE_H_THRUST_CMD_FILTER
454 #define GUIDANCE_H_THRUST_CMD_FILTER 10
455 #endif
456 
457 /* with a pgain of 100 and a scale of 2,
458  * you get an angle of 5.6 degrees for 1m pos error */
459 #define GH_GAIN_SCALE 2
460 
461 #if !GUIDANCE_INDI
462 static void guidance_h_traj_run(bool in_flight)
463 {
464  /* maximum bank angle: default 20 deg, max 40 deg*/
465  static const int32_t traj_max_bank = Min(BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC),
466  BFP_OF_REAL(RadOfDeg(40), INT32_ANGLE_FRAC));
467  static const int32_t total_max_bank = BFP_OF_REAL(RadOfDeg(45), INT32_ANGLE_FRAC);
468 
469  /* compute position error */
471  /* saturate it */
473 
474  /* compute speed error */
476  /* saturate it */
478 
479  /* run PID */
480  int32_t pd_x =
483  int32_t pd_y =
486  guidance_h_cmd_earth.x = pd_x +
487  ((guidance_h.gains.v * guidance_h.ref.speed.x) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) + /* speed feedforward gain */
489  GH_GAIN_SCALE)); /* acceleration feedforward gain */
490  guidance_h_cmd_earth.y = pd_y +
491  ((guidance_h.gains.v * guidance_h.ref.speed.y) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) + /* speed feedforward gain */
493  GH_GAIN_SCALE)); /* acceleration feedforward gain */
494 
495  /* trim max bank angle from PD */
496  VECT2_STRIM(guidance_h_cmd_earth, -traj_max_bank, traj_max_bank);
497 
498  /* Update pos & speed error integral, zero it if not in_flight.
499  * Integrate twice as fast when not only POS but also SPEED are wrong,
500  * but do not integrate POS errors when the SPEED is already catching up.
501  */
502  if (in_flight) {
503  /* ANGLE_FRAC (12) * GAIN (8) * LOOP_FREQ (9) -> INTEGRATOR HIGH RES ANGLE_FRAX (28) */
506  /* saturate it */
508  (traj_max_bank << (INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2)));
509  /* add it to the command */
512  } else {
514  }
515 
516  /* compute a better approximation of force commands by taking thrust into account */
517  if (guidance_h.approx_force_by_thrust && in_flight) {
518  static int32_t thrust_cmd_filt;
519  int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC;
520  thrust_cmd_filt = (thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + vertical_thrust) /
523  thrust_cmd_filt));
525  thrust_cmd_filt));
526  }
527 
528  VECT2_STRIM(guidance_h_cmd_earth, -total_max_bank, total_max_bank);
529 }
530 #endif
531 
533 {
534  /* reset speed setting */
535  guidance_h.sp.speed.x = 0;
536  guidance_h.sp.speed.y = 0;
537 
538  /* disable horizontal velocity setpoints,
539  * might still be activated in guidance_h_read_rc if GUIDANCE_H_USE_SPEED_REF
540  */
541  ClearBit(guidance_h.sp.mask, 5);
542  ClearBit(guidance_h.sp.mask, 7);
543 
544  /* set horizontal setpoint to current position */
546 
547  /* reset guidance reference */
549 
550  /* set guidance to current heading and position */
553 }
554 
556 {
557  ClearBit(guidance_h.sp.mask, 5);
558  ClearBit(guidance_h.sp.mask, 7);
559 
560  /* horizontal position setpoint from navigation/flightplan */
562 
564 
567 }
568 
569 void guidance_h_from_nav(bool in_flight)
570 {
571  if (!in_flight) {
573  }
574 
576  stabilization_cmd[COMMAND_ROLL] = nav_cmd_roll;
577  stabilization_cmd[COMMAND_PITCH] = nav_cmd_pitch;
578  stabilization_cmd[COMMAND_YAW] = nav_cmd_yaw;
580  struct Int32Eulers sp_cmd_i;
581  sp_cmd_i.phi = nav_roll;
582  sp_cmd_i.theta = nav_pitch;
583  sp_cmd_i.psi = nav_heading;
585  stabilization_attitude_run(in_flight);
586 
587 #if HYBRID_NAVIGATION
588  //make sure the heading is right before leaving horizontal_mode attitude
590 #endif
591  } else {
592 
593 #if HYBRID_NAVIGATION
596 #else
598 
600 
601  /* set psi command */
604 
605 #if GUIDANCE_INDI
607 #else
608  /* compute x,y earth commands */
609  guidance_h_traj_run(in_flight);
610  /* set final attitude setpoint */
613  heading_sp_i);
614 #endif
615 
616 #endif
617  stabilization_attitude_run(in_flight);
618  }
619 }
620 
621 static inline void transition_run(bool to_forward)
622 {
623  if (to_forward) {
624  //Add 0.00625%
625  transition_percentage += 1 << (INT32_PERCENTAGE_FRAC - 4);
626  } else {
627  //Subtract 0.00625%
628  transition_percentage -= 1 << (INT32_PERCENTAGE_FRAC - 4);
629  }
630 
631 #ifdef TRANSITION_MAX_OFFSET
632  const int32_t max_offset = ANGLE_BFP_OF_REAL(TRANSITION_MAX_OFFSET);
633  transition_theta_offset = INT_MULT_RSHIFT((transition_percentage << (INT32_ANGLE_FRAC - INT32_PERCENTAGE_FRAC)) / 100,
634  max_offset, INT32_ANGLE_FRAC);
635 #endif
636 }
637 
639 static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight)
640 {
641  if (in_flight) {
642  // negative pitch is forward
645  DeadBand(rc_x, MAX_PPRZ / 20);
646  DeadBand(rc_y, MAX_PPRZ / 20);
647 
648  // convert input from MAX_PPRZ range to SPEED_BFP
651  //int32_t rc_norm = sqrtf(rc_x * rc_x + rc_y * rc_y);
652  //int32_t max_pprz = rc_norm * MAX_PPRZ / Max(abs(rc_x), abs(rc_y);
653  rc_x = rc_x * max_speed / MAX_PPRZ;
654  rc_y = rc_y * max_speed / MAX_PPRZ;
655 
656  /* Rotate from body to NED frame by negative psi angle */
658  int32_t s_psi, c_psi;
659  PPRZ_ITRIG_SIN(s_psi, psi);
660  PPRZ_ITRIG_COS(c_psi, psi);
661  speed_sp->x = (int32_t)(((int64_t)c_psi * rc_x + (int64_t)s_psi * rc_y) >> INT32_TRIG_FRAC);
662  speed_sp->y = (int32_t)((-(int64_t)s_psi * rc_x + (int64_t)c_psi * rc_y) >> INT32_TRIG_FRAC);
663  } else {
664  speed_sp->x = 0;
665  speed_sp->y = 0;
666  }
667 }
668 
670 {
671  guidance_h.gains.i = igain;
673 }
674 
675 
676 void guidance_h_guided_run(bool in_flight)
677 {
678  /* guidance_h.sp.pos and guidance_h.sp.heading need to be set from external source */
679  if (!in_flight) {
681  }
682 
684 
685 #if GUIDANCE_INDI
687 #else
688  /* compute x,y earth commands */
689  guidance_h_traj_run(in_flight);
690  /* set final attitude setpoint */
693 #endif
694  stabilization_attitude_run(in_flight);
695 }
696 
697 bool guidance_h_set_guided_pos(float x, float y)
698 {
700  ClearBit(guidance_h.sp.mask, 5);
703  return true;
704  }
705  return false;
706 }
707 
709 {
711  ClearBit(guidance_h.sp.mask, 7);
714  return true;
715  }
716  return false;
717 }
718 
719 bool guidance_h_set_guided_body_vel(float vx, float vy)
720 {
721  float psi = stateGetNedToBodyEulers_f()->psi;
722  float newvx = cosf(-psi) * vx + sinf(-psi) * vy;
723  float newvy = -sinf(-psi) * vx + cosf(-psi) * vy;
724  return guidance_h_set_guided_vel(newvx, newvy);
725 }
726 
727 bool guidance_h_set_guided_vel(float vx, float vy)
728 {
730  SetBit(guidance_h.sp.mask, 5);
733  return true;
734  }
735  return false;
736 }
737 
739 {
741  SetBit(guidance_h.sp.mask, 7);
742  guidance_h.sp.heading_rate = rate;
743  return true;
744  }
745  return false;
746 }
747 
749 {
750  return &guidance_h_pos_err;
751 }
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
struct EnuCoor_i navigation_target
Definition: navigation.c:87
#define FLOAT_ANGLE_NORMALIZE(_a)
int32_t transition_percentage
Definition: guidance_h.c:82
void guidance_indi_enter(void)
Call upon entering indi guidance.
void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel)
void guidance_h_init(void)
Definition: guidance_h.c:166
#define RADIO_ROLL
Definition: intermcu_ap.h:41
int32_t nav_cmd_pitch
Definition: navigation.c:105
#define GUIDANCE_H_MODE_GUIDED
Definition: guidance_h.h:66
void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp)
#define Min(x, y)
Definition: esc_dshot.c:85
void guidance_h_read_rc(bool in_flight)
Definition: guidance_h.c:292
#define GUIDANCE_H_MAX_BANK
Definition: guidance_h.c:66
void stabilization_none_run(bool in_flight)
bool guidance_h_set_guided_heading_rate(float rate)
Set heading rate setpoint in GUIDED mode.
Definition: guidance_h.c:738
#define GUIDANCE_H_MODE_CARE_FREE
Definition: guidance_h.h:62
struct Int32Vect2 pos
horizontal position setpoint in NED.
Definition: guidance_h.h:74
struct Int32Vect2 guidance_h_cmd_earth
horizontal guidance command.
Definition: guidance_h.c:95
void guidance_h_module_run(bool in_flight)
#define INT32_ACCEL_FRAC
#define MAX_SPEED_ERR
Definition: guidance_h.c:451
#define INT32_SPEED_FRAC
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
const struct Int32Vect2 * guidance_h_get_pos_err(void)
Gets the position error.
Definition: guidance_h.c:748
#define INT32_PERCENTAGE_FRAC
Dummy stabilization for rotorcrafts.
Periodic telemetry system header (includes downlink utility and generated code).
int32_t nav_cmd_roll
Definition: navigation.c:105
General attitude stabilization interface for rotorcrafts.
void guidance_h_run(bool in_flight)
Definition: guidance_h.c:351
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:64
#define GUIDANCE_H_VGAIN
Definition: guidance_h.c:53
#define FLOAT_EULERS_ZERO(_e)
#define INT32_ANGLE_FRAC
#define HORIZONTAL_MODE_MANUAL
Definition: navigation.h:57
#define VECT2_STRIM(_v, _min, _max)
Definition: pprz_algebra.h:110
#define POS_BFP_OF_REAL(_af)
#define GUIDANCE_H_APPROX_FORCE_BY_THRUST
Definition: guidance_h.c:73
Open Loop guidance for making a flip.
static void reset_guidance_reference_from_current_position(void)
Definition: guidance_h.c:202
Read an attitude setpoint from the RC.
#define MAX_POS_ERR
Definition: guidance_h.c:450
bool approx_force_by_thrust
Definition: guidance_h.h:99
#define GUIDANCE_H_MODE_HOVER
Definition: guidance_h.h:59
void stabilization_filter_commands(void)
Definition: stabilization.c:75
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
bool guidance_h_set_guided_pos(float x, float y)
Set horizontal position setpoint in GUIDED mode.
Definition: guidance_h.c:697
float psi
in radians
Vertical guidance for rotorcrafts.
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
bool guidance_h_set_guided_body_vel(float vx, float vy)
Set body relative horizontal velocity setpoint in GUIDED mode.
Definition: guidance_h.c:719
void stabilization_attitude_run(bool in_flight)
void guidance_h_module_init(void)
struct Int32Vect2 guidance_h_trim_att_integrator
Definition: guidance_h.c:89
void stabilization_attitude_enter(void)
signed long long int64_t
Definition: types.h:21
#define INT32_ANGLE_PI_2
#define GUIDANCE_H_MODE_ATTITUDE
Definition: guidance_h.h:58
void stabilization_attitude_reset_care_free_heading(void)
reset the heading for care-free mode to current heading
struct HorizontalGuidanceReference ref
reference calculated from setpoints
Definition: guidance_h.h:104
void guidance_h_set_igain(uint32_t igain)
Definition: guidance_h.c:669
Guidance controllers (horizontal and vertical) for Hybrid UAV configurations.
void guidance_hybrid_run(void)
Runs the Hybrid Guidance main functions.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
struct HorizontalGuidanceGains gains
Definition: guidance_h.h:101
#define GUIDANCE_H_REF_MAX_SPEED
Default speed saturation.
void guidance_h_module_read_rc(void)
struct Int64Vect2 pos
Reference model position.
#define ANGLE_FLOAT_OF_BFP(_ai)
void guidance_h_hover_enter(void)
Definition: guidance_h.c:532
#define FALSE
Definition: std.h:5
static void send_hover_loop(struct transport_tx *trans, struct link_device *dev)
Definition: guidance_h.c:116
#define GUIDANCE_H_MODE_FLIP
Definition: guidance_h.h:65
Rate stabilization for rotorcrafts.
int32_t y
East.
void guidance_indi_run(float heading_sp)
#define BFP_OF_REAL(_vr, _frac)
void stabilization_none_enter(void)
#define PPRZ_ITRIG_SIN(_s, _a)
uint8_t mask
bit 5: vx & vy, bit 6: vz, bit 7: vyaw
Definition: guidance_h.h:78
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:863
#define GUIDANCE_H_THRUST_CMD_FILTER
Definition: guidance_h.c:454
#define TRUE
Definition: std.h:4
static void guidance_h_update_reference(void)
Definition: guidance_h.c:414
static float heading
Definition: ahrs_infrared.c:45
struct Int32Vect2 speed
with INT32_SPEED_FRAC
Definition: guidance_h.h:83
void guidance_h_module_enter(void)
struct Int32Vect2 guidance_h_pos_err
Definition: guidance_h.c:87
void stabilization_rate_read_rc(void)
struct Int32Vect2 speed
only used in HOVER mode if GUIDANCE_H_USE_SPEED_REF or in GUIDED mode
Definition: guidance_h.h:75
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:104
#define HORIZONTAL_MODE_ATTITUDE
Definition: navigation.h:56
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)
int32_t transition_theta_offset
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 transition_run(bool to_forward)
Definition: guidance_h.c:621
void stabilization_none_read_rc(void)
struct RadioControl radio_control
Definition: radio_control.c:30
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:80
#define GH_SPEED_REF_FRAC
void guidance_h_mode_changed(uint8_t new_mode)
Definition: guidance_h.c:212
#define INT32_POS_FRAC
int32_t guidance_hybrid_norm_ref_airspeed
#define GUIDANCE_H_MODE_FORWARD
Definition: guidance_h.h:63
static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight)
read speed setpoint from RC
Definition: guidance_h.c:639
void guidance_h_nav_enter(void)
Definition: guidance_h.c:555
signed long int32_t
Definition: types.h:19
struct EnuCoor_i navigation_carrot
Definition: navigation.c:88
static void send_tune_hover(struct transport_tx *trans, struct link_device *dev)
Definition: guidance_h.c:149
#define GUIDANCE_H_AGAIN
Definition: guidance_h.c:49
#define RC_OK
Definition: radio_control.h:56
#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
Rotorcraft navigation functions.
struct Int32Vect2 pos
with INT32_POS_FRAC
Definition: guidance_h.h:82
void guidance_h_from_nav(bool in_flight)
Set horizontal guidance from NAV and run control loop.
Definition: guidance_h.c:569
void gh_ref_init(void)
bool guidance_h_set_guided_vel(float vx, float vy)
Set horizontal velocity setpoint in GUIDED mode.
Definition: guidance_h.c:727
static void guidance_h_traj_run(bool in_flight)
Definition: guidance_h.c:462
int32_t phi
in rad with INT32_ANGLE_FRAC
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
vector in North East Down coordinates
#define RADIO_PITCH
Definition: intermcu_ap.h:42
struct Int32Vect2 speed
Reference model speed.
#define INT_MULT_RSHIFT(_a, _b, _r)
int32_t guidance_v_thrust_coeff
Definition: guidance_v.c:142
#define GUIDANCE_H_USE_REF
Use horizontal guidance reference trajectory.
Definition: guidance_h.h:45
#define GUIDANCE_H_MODE_NAV
Definition: guidance_h.h:60
General stabilization interface for rotorcrafts.
void guidance_flip_run(void)
Definition: guidance_flip.c:65
#define RADIO_YAW
Definition: intermcu_ap.h:43
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
void stabilization_rate_run(bool in_flight)
struct Int32Vect2 guidance_h_speed_err
Definition: guidance_h.c:88
#define INT_VECT2_ZERO(_v)
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool in_flight, bool in_carefree, bool coordinated_turn)
struct Int32Vect2 accel
with INT32_ACCEL_FRAC
Definition: guidance_h.h:84
Guidance in a module file.
int32_t nav_roll
Definition: navigation.c:103
struct Int32Vect2 accel
Reference model acceleration.
#define GH_GAIN_SCALE
Definition: guidance_h.c:459
#define SPEED_BFP_OF_REAL(_af)
bool guidance_h_set_guided_heading(float heading)
Set heading setpoint in GUIDED mode.
Definition: guidance_h.c:708
struct FloatEulers rc_sp
Definition: guidance_h.h:106
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:665
int32_t nav_cmd_yaw
Definition: navigation.c:105
Horizontal guidance for rotorcrafts.
#define INT32_VECT2_RSHIFT(_o, _i, _r)
#define INT32_VECT2_LSHIFT(_o, _i, _l)
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
#define GH_POS_REF_FRAC
#define GUIDANCE_H_MODE_RATE
Definition: guidance_h.h:57
void guidance_h_guided_run(bool in_flight)
Run GUIDED mode control.
Definition: guidance_h.c:676
#define GUIDANCE_H_USE_SPEED_REF
Use horizontal guidance speed reference.
Definition: guidance_h.h:53
static void send_gh(struct transport_tx *trans, struct link_device *dev)
Definition: guidance_h.c:107
struct HorizontalGuidanceSetpoint sp
setpoints
Definition: guidance_h.h:103
A guidance mode based on Incremental Nonlinear Dynamic Inversion.
static void send_href(struct transport_tx *trans, struct link_device *dev)
Definition: guidance_h.c:138
#define GUIDANCE_H_MODE_KILL
Definition: guidance_h.h:56
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:1020
#define PPRZ_ITRIG_COS(_c, _a)
#define GUIDANCE_H_MODE_MODULE
Definition: guidance_h.h:64
#define GUIDANCE_H_MODE_RC_DIRECT
Definition: guidance_h.h:61