Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 #if GUIDANCE_INDI_HYBRID
39 #else
41 #endif
42 
46 
47 /* for guidance_v_thrust_coeff */
49 
50 #include "state.h"
51 
52 #ifndef GUIDANCE_H_AGAIN
53 #define GUIDANCE_H_AGAIN 0
54 #endif
55 
56 #ifndef GUIDANCE_H_VGAIN
57 #define GUIDANCE_H_VGAIN 0
58 #endif
59 
60 /* error if some gains are negative */
61 #if (GUIDANCE_H_PGAIN < 0) || \
62  (GUIDANCE_H_DGAIN < 0) || \
63  (GUIDANCE_H_IGAIN < 0) || \
64  (GUIDANCE_H_AGAIN < 0) || \
65  (GUIDANCE_H_VGAIN < 0)
66 #error "ALL control gains have to be positive!!!"
67 #endif
68 
69 #ifndef GUIDANCE_H_MAX_BANK
70 #define GUIDANCE_H_MAX_BANK RadOfDeg(20)
71 #endif
72 
73 PRINT_CONFIG_VAR(GUIDANCE_H_USE_REF)
74 PRINT_CONFIG_VAR(GUIDANCE_H_USE_SPEED_REF)
75 
76 #ifndef GUIDANCE_H_APPROX_FORCE_BY_THRUST
77 #define GUIDANCE_H_APPROX_FORCE_BY_THRUST FALSE
78 #endif
79 
80 #ifndef GUIDANCE_INDI
81 #define GUIDANCE_INDI FALSE
82 #endif
83 
84 // Navigation can set heading freely
85 // This is false if sideslip is a problem
86 #ifndef GUIDANCE_HEADING_IS_FREE
87 #define GUIDANCE_HEADING_IS_FREE TRUE
88 #endif
89 
91 
93 
94 /*
95  * internal variables
96  */
100 
106 
107 static void guidance_h_update_reference(void);
108 #if !GUIDANCE_INDI
109 static void guidance_h_traj_run(bool in_flight);
110 #endif
111 static inline void transition_run(bool to_forward);
112 static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight);
113 
114 #if PERIODIC_TELEMETRY
116 
117 static void send_gh(struct transport_tx *trans, struct link_device *dev)
118 {
119  struct NedCoor_i *pos = stateGetPositionNed_i();
120  pprz_msg_send_GUIDANCE_H_INT(trans, dev, AC_ID,
123  &(pos->x), &(pos->y));
124 }
125 
126 static void send_hover_loop(struct transport_tx *trans, struct link_device *dev)
127 {
128  struct NedCoor_i *pos = stateGetPositionNed_i();
129  struct NedCoor_i *speed = stateGetSpeedNed_i();
130  struct NedCoor_i *accel = stateGetAccelNed_i();
131  pprz_msg_send_HOVER_LOOP(trans, dev, AC_ID,
132  &guidance_h.sp.pos.x,
133  &guidance_h.sp.pos.y,
134  &(pos->x), &(pos->y),
135  &(speed->x), &(speed->y),
136  &(accel->x), &(accel->y),
146 }
147 
148 static void send_href(struct transport_tx *trans, struct link_device *dev)
149 {
150  pprz_msg_send_GUIDANCE_H_REF_INT(trans, dev, AC_ID,
156  &guidance_h.ref.accel.y);
157 }
158 
159 static void send_tune_hover(struct transport_tx *trans, struct link_device *dev)
160 {
161  pprz_msg_send_ROTORCRAFT_TUNE_HOVER(trans, dev, AC_ID,
165  &stabilization_cmd[COMMAND_ROLL],
166  &stabilization_cmd[COMMAND_PITCH],
167  &stabilization_cmd[COMMAND_YAW],
168  &stabilization_cmd[COMMAND_THRUST],
169  &(stateGetNedToBodyEulers_i()->phi),
170  &(stateGetNedToBodyEulers_i()->theta),
171  &(stateGetNedToBodyEulers_i()->psi));
172 }
173 
174 #endif
175 
176 void guidance_h_init(void)
177 {
178 
182 
186  guidance_h.sp.heading = 0.0;
187  guidance_h.sp.heading_rate = 0.0;
188  guidance_h.gains.p = GUIDANCE_H_PGAIN;
189  guidance_h.gains.i = GUIDANCE_H_IGAIN;
190  guidance_h.gains.d = GUIDANCE_H_DGAIN;
195 
196  gh_ref_init();
197 
198 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
200 #endif
201 
202 #if PERIODIC_TELEMETRY
203  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_H_INT, send_gh);
205  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_H_REF_INT, send_href);
206  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_TUNE_HOVER, send_tune_hover);
207 #endif
208 
209 }
210 
211 
213 {
216  struct FloatVect2 ref_speed;
217  ref_speed.x = SPEED_FLOAT_OF_BFP(guidance_h.ref.speed.x);
218  ref_speed.y = SPEED_FLOAT_OF_BFP(guidance_h.ref.speed.y);
219 
221  struct FloatVect2 ref_accel;
222  FLOAT_VECT2_ZERO(ref_accel);
223  gh_set_ref(guidance_h.ref.pos, ref_speed, ref_accel);
224 
226 }
227 
229 {
230  if (new_mode == guidance_h.mode) {
231  return;
232  }
233 
234 #if HYBRID_NAVIGATION
236 #endif
237 
238  switch (new_mode) {
241  break;
242 
243 #if USE_STABILIZATION_RATE
246  break;
247 #endif
248 
251  /* Falls through. */
254 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
255  /* reset attitude stabilization if previous mode was not using it */
259 #endif
261  break;
262 
265 #if GUIDANCE_INDI
267 #endif
269 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
270  /* reset attitude stabilization if previous mode was not using it */
274 #endif
276  break;
277 
278 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
281  break;
282 #endif
283 
284  case GUIDANCE_H_MODE_NAV:
285 #if GUIDANCE_INDI
287 #endif
289 #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
290  /* reset attitude stabilization if previous mode was not using it */
294 #endif
296  break;
297 
300  break;
301 
302  default:
303  break;
304  }
305 
306  guidance_h.mode = new_mode;
307 
308 }
309 
310 
311 void guidance_h_read_rc(bool in_flight)
312 {
313 
314  switch (guidance_h.mode) {
315 
318  break;
319 
320 #if USE_STABILIZATION_RATE
322 #if SWITCH_STICKS_FOR_RATE_CONTROL
324 #else
326 #endif
327  break;
328 #endif
329 
332  break;
335  break;
338  break;
341 #if GUIDANCE_H_USE_SPEED_REF
343  /* enable x,y velocity setpoints */
344  SetBit(guidance_h.sp.mask, 5);
345 #endif
346  break;
347 
348 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
351  break;
352 #endif
353 
354  case GUIDANCE_H_MODE_NAV:
355  if (radio_control.status == RC_OK) {
357  } else {
359  }
360  break;
363  break;
364  default:
365  break;
366  }
367 
368 }
369 
370 void guidance_h_run(bool in_flight)
371 {
372  switch (guidance_h.mode) {
373 
375  stabilization_none_run(in_flight);
376  break;
377 
378 #if USE_STABILIZATION_RATE
380  stabilization_rate_run(in_flight);
381  break;
382 #endif
383 
386  transition_run(true);
387  }
388  /* Falls through. */
392  transition_run(false);
393  }
394  stabilization_attitude_run(in_flight);
395 #if (STABILIZATION_FILTER_CMD_ROLL_PITCH || STABILIZATION_FILTER_CMD_YAW)
396  if (in_flight) {
398  }
399 #endif
400 
401  break;
402 
404  /* set psi command from RC */
406  /* fall trough to GUIDED to update ref, run traj and set final attitude setpoint */
407 
408  /* Falls through. */
410  guidance_h_guided_run(in_flight);
411  break;
412 
413  case GUIDANCE_H_MODE_NAV:
414  guidance_h_from_nav(in_flight);
415  break;
416 
417 #if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
419  guidance_h_module_run(in_flight);
420  break;
421 #endif
422 
425  break;
426 
427  default:
428  break;
429  }
430 }
431 
432 
434 {
435  /* compute reference even if usage temporarily disabled via guidance_h_use_ref */
436 #if GUIDANCE_H_USE_REF
437  if (bit_is_set(guidance_h.sp.mask, 5)) {
438  struct FloatVect2 sp_speed;
439  sp_speed.x = SPEED_FLOAT_OF_BFP(guidance_h.sp.speed.x);
440  sp_speed.y = SPEED_FLOAT_OF_BFP(guidance_h.sp.speed.y);
441  gh_update_ref_from_speed_sp(sp_speed);
442  } else {
444  }
445 #endif
446 
447  /* either use the reference or simply copy the pos setpoint */
448  if (guidance_h.use_ref) {
449  /* convert our reference to generic representation */
455  } else {
459  }
460 
461 #if GUIDANCE_H_USE_SPEED_REF
463  VECT2_COPY(guidance_h.sp.pos, guidance_h.ref.pos); // for display only
464  }
465 #endif
466 
467  /* update heading setpoint from rate */
468  if (bit_is_set(guidance_h.sp.mask, 7)) {
469  guidance_h.sp.heading += guidance_h.sp.heading_rate / PERIODIC_FREQUENCY;
471  }
472 }
473 
474 #define MAX_POS_ERR POS_BFP_OF_REAL(16.)
475 #define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)
476 
477 #ifndef GUIDANCE_H_THRUST_CMD_FILTER
478 #define GUIDANCE_H_THRUST_CMD_FILTER 10
479 #endif
480 
481 /* with a pgain of 100 and a scale of 2,
482  * you get an angle of 5.6 degrees for 1m pos error */
483 #define GH_GAIN_SCALE 2
484 
485 #if !GUIDANCE_INDI
486 static void guidance_h_traj_run(bool in_flight)
487 {
488  /* maximum bank angle: default 20 deg, max 40 deg*/
489  static const int32_t traj_max_bank = Min(BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC),
490  BFP_OF_REAL(RadOfDeg(40), INT32_ANGLE_FRAC));
491  static const int32_t total_max_bank = BFP_OF_REAL(RadOfDeg(45), INT32_ANGLE_FRAC);
492 
493  /* compute position error */
495  /* saturate it */
497 
498  /* compute speed error */
500  /* saturate it */
502 
503  /* run PID */
504  int32_t pd_x =
507  int32_t pd_y =
510  guidance_h_cmd_earth.x = pd_x +
511  ((guidance_h.gains.v * guidance_h.ref.speed.x) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) + /* speed feedforward gain */
513  GH_GAIN_SCALE)); /* acceleration feedforward gain */
514  guidance_h_cmd_earth.y = pd_y +
515  ((guidance_h.gains.v * guidance_h.ref.speed.y) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) + /* speed feedforward gain */
517  GH_GAIN_SCALE)); /* acceleration feedforward gain */
518 
519  /* trim max bank angle from PD */
520  VECT2_STRIM(guidance_h_cmd_earth, -traj_max_bank, traj_max_bank);
521 
522  /* Update pos & speed error integral, zero it if not in_flight.
523  * Integrate twice as fast when not only POS but also SPEED are wrong,
524  * but do not integrate POS errors when the SPEED is already catching up.
525  */
526  if (in_flight) {
527  /* ANGLE_FRAC (12) * GAIN (8) * LOOP_FREQ (9) -> INTEGRATOR HIGH RES ANGLE_FRAX (28) */
530  /* saturate it */
532  (traj_max_bank << (INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2)));
533  /* add it to the command */
536  } else {
538  }
539 
540  /* compute a better approximation of force commands by taking thrust into account */
541  if (guidance_h.approx_force_by_thrust && in_flight) {
542  static int32_t thrust_cmd_filt;
543  int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC;
544  thrust_cmd_filt = (thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + vertical_thrust) /
547  thrust_cmd_filt));
549  thrust_cmd_filt));
550  }
551 
552  VECT2_STRIM(guidance_h_cmd_earth, -total_max_bank, total_max_bank);
553 }
554 #endif
555 
557 {
558  /* reset speed setting */
559  guidance_h.sp.speed.x = 0;
560  guidance_h.sp.speed.y = 0;
561 
562  /* set horizontal setpoint to current position */
566  /* reset guidance reference */
568 
569  /* set guidance to current heading and position */
572 }
573 
575 {
576  /* horizontal position setpoint from navigation/flightplan */
581  /* set nav_heading to current heading */
584 }
585 
586 void guidance_h_from_nav(bool in_flight)
587 {
588  if (!in_flight) {
590  }
591 
593  stabilization_cmd[COMMAND_ROLL] = nav_cmd_roll;
594  stabilization_cmd[COMMAND_PITCH] = nav_cmd_pitch;
595  stabilization_cmd[COMMAND_YAW] = nav_cmd_yaw;
597  struct Int32Eulers sp_cmd_i;
598  sp_cmd_i.phi = nav_roll;
599  sp_cmd_i.theta = nav_pitch;
600  sp_cmd_i.psi = nav_heading;
602  stabilization_attitude_run(in_flight);
603 
604 #if HYBRID_NAVIGATION
605  //make sure the heading is right before leaving horizontal_mode attitude
607 #endif
608  } else if (horizontal_mode == HORIZONTAL_MODE_GUIDED) {
609  guidance_h_guided_run(in_flight);
610  } else {
611 
612 #if HYBRID_NAVIGATION
615 #else
616  // set guidance in NED
621 
622 #if GUIDANCE_HEADING_IS_FREE
623  /* set psi command */
625 #endif
626 
627 #if GUIDANCE_INDI
629 #else
630  /* compute x,y earth commands */
631  guidance_h_traj_run(in_flight);
632  /* set final attitude setpoint */
635  heading_sp_i);
636 #endif
637 
638 #endif
639  stabilization_attitude_run(in_flight);
640  }
641 }
642 
643 static inline void transition_run(bool to_forward)
644 {
645  if (to_forward) {
646  //Add 0.00625%
648  } else {
649  //Subtract 0.00625%
651  }
652 
653 #ifdef TRANSITION_MAX_OFFSET
654  const int32_t max_offset = ANGLE_BFP_OF_REAL(TRANSITION_MAX_OFFSET);
656  max_offset, INT32_ANGLE_FRAC);
657 #endif
658 }
659 
661 static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight)
662 {
663  if (in_flight) {
664  // negative pitch is forward
665  int64_t rc_x = -radio_control.values[RADIO_PITCH];
666  int64_t rc_y = radio_control.values[RADIO_ROLL];
667  DeadBand(rc_x, MAX_PPRZ / 20);
668  DeadBand(rc_y, MAX_PPRZ / 20);
669 
670  // convert input from MAX_PPRZ range to SPEED_BFP
673  //int32_t rc_norm = sqrtf(rc_x * rc_x + rc_y * rc_y);
674  //int32_t max_pprz = rc_norm * MAX_PPRZ / Max(abs(rc_x), abs(rc_y);
675  rc_x = rc_x * max_speed / MAX_PPRZ;
676  rc_y = rc_y * max_speed / MAX_PPRZ;
677 
678  /* Rotate from body to NED frame by negative psi angle */
680  int32_t s_psi, c_psi;
681  PPRZ_ITRIG_SIN(s_psi, psi);
682  PPRZ_ITRIG_COS(c_psi, psi);
683  speed_sp->x = (int32_t)(((int64_t)c_psi * rc_x + (int64_t)s_psi * rc_y) >> INT32_TRIG_FRAC);
684  speed_sp->y = (int32_t)((-(int64_t)s_psi * rc_x + (int64_t)c_psi * rc_y) >> INT32_TRIG_FRAC);
685  } else {
686  speed_sp->x = 0;
687  speed_sp->y = 0;
688  }
689 }
690 
692 {
693  guidance_h.gains.i = igain;
695 }
696 
697 
698 void guidance_h_guided_run(bool in_flight)
699 {
700  /* guidance_h.sp.pos and guidance_h.sp.heading need to be set from external source */
701  if (!in_flight) {
703  }
704 
706 
707 #if GUIDANCE_INDI
709 #else
710  /* compute x,y earth commands */
711  guidance_h_traj_run(in_flight);
712  /* set final attitude setpoint */
715 #endif
716  stabilization_attitude_run(in_flight);
717 }
718 
719 void guidance_h_set_pos(float x, float y)
720 {
721  ClearBit(guidance_h.sp.mask, 5);
724 }
725 
727 {
728  ClearBit(guidance_h.sp.mask, 7);
731 }
732 
733 void guidance_h_set_body_vel(float vx, float vy)
734 {
735  float psi = stateGetNedToBodyEulers_f()->psi;
736  float newvx = cosf(-psi) * vx + sinf(-psi) * vy;
737  float newvy = -sinf(-psi) * vx + cosf(-psi) * vy;
738  guidance_h_set_vel(newvx, newvy);
739 }
740 
741 void guidance_h_set_vel(float vx, float vy)
742 {
743  SetBit(guidance_h.sp.mask, 5);
746 }
747 
749 {
750  SetBit(guidance_h.sp.mask, 7);
751  guidance_h.sp.heading_rate = rate;
752 }
753 
755 {
756  return &guidance_h_pos_err;
757 }
Int32Eulers::theta
int32_t theta
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:148
FLOAT_VECT2_ZERO
#define FLOAT_VECT2_ZERO(_v)
Definition: pprz_algebra_float.h:129
radio_control.h
guidance_hybrid_norm_ref_airspeed
int32_t guidance_hybrid_norm_ref_airspeed
Definition: guidance_hybrid.c:49
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
HorizontalGuidance::ref
struct HorizontalGuidanceReference ref
reference calculated from setpoints
Definition: guidance_h.h:104
INT32_VECT2_NED_OF_ENU
#define INT32_VECT2_NED_OF_ENU(_o, _i)
Definition: pprz_geodetic_int.h:214
stateGetPositionNed_f
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
GUIDANCE_H_USE_SPEED_REF
#define GUIDANCE_H_USE_SPEED_REF
Use horizontal guidance speed reference.
Definition: guidance_h.h:53
ANGLE_FLOAT_OF_BFP
#define ANGLE_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:212
transition_theta_offset
int32_t transition_theta_offset
Definition: stabilization_attitude_rc_setpoint.c:58
stabilization_attitude_read_rc
void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
Definition: stabilization_attitude_euler_float.c:131
FLOAT_ANGLE_NORMALIZE
#define FLOAT_ANGLE_NORMALIZE(_a)
Definition: pprz_algebra_float.h:99
HorizontalGuidanceSetpoint::mask
uint8_t mask
bit 5: vx & vy, bit 6: vz, bit 7: vyaw
Definition: guidance_h.h:78
HORIZONTAL_MODE_GUIDED
#define HORIZONTAL_MODE_GUIDED
Definition: navigation.h:65
stabilization_attitude.h
guidance_hybrid_reset_heading
void guidance_hybrid_reset_heading(struct Int32Eulers *sp_cmd)
Description.
Definition: guidance_hybrid.c:137
HorizontalGuidanceGains::i
int32_t i
Definition: guidance_h.h:90
gh_ref
struct GuidanceHRef gh_ref
Definition: guidance_h_ref.c:30
guidance_flip.h
stabilization_rate_read_rc
void stabilization_rate_read_rc(void)
Definition: stabilization_rate.c:137
GUIDANCE_H_MAX_BANK
#define GUIDANCE_H_MAX_BANK
Definition: guidance_h.c:70
INT32_SPEED_FRAC
#define INT32_SPEED_FRAC
Definition: pprz_algebra_int.h:73
EnuCoor_i::y
int32_t y
North.
Definition: pprz_geodetic_int.h:79
guidance_h_module_run
void guidance_h_module_run(bool in_flight)
Definition: ctrl_module_innerloop_demo.c:99
guidance_h_cmd_earth
struct Int32Vect2 guidance_h_cmd_earth
horizontal guidance command.
Definition: guidance_h.c:105
NedCoor_i::y
int32_t y
East.
Definition: pprz_geodetic_int.h:70
EnuCoor_i::x
int32_t x
East.
Definition: pprz_geodetic_int.h:78
GUIDANCE_H_MODE_ATTITUDE
#define GUIDANCE_H_MODE_ATTITUDE
Definition: guidance_h.h:58
GH_GAIN_SCALE
#define GH_GAIN_SCALE
Definition: guidance_h.c:483
guidance_h_mode_changed
void guidance_h_mode_changed(uint8_t new_mode)
Definition: guidance_h.c:228
guidance_h_module_read_rc
void guidance_h_module_read_rc(void)
Definition: ctrl_module_innerloop_demo.c:90
stabilization_filter_commands
void stabilization_filter_commands(void)
Definition: stabilization.c:75
guidance_h_update_reference
static void guidance_h_update_reference(void)
Definition: guidance_h.c:433
GUIDANCE_H_MODE_GUIDED
#define GUIDANCE_H_MODE_GUIDED
Definition: guidance_h.h:66
INT32_ANGLE_FRAC
#define INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:116
guidance_indi_hybrid.h
Int32Vect2::y
int32_t y
Definition: pprz_algebra_int.h:85
nav_cmd_yaw
int32_t nav_cmd_yaw
Definition: navigation.c:114
SPEED_BFP_OF_REAL
#define SPEED_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:219
GuidanceHRef::accel
struct FloatVect2 accel
Reference model acceleration.
Definition: guidance_h_ref.h:64
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
HorizontalGuidanceSetpoint::pos
struct Int32Vect2 pos
horizontal position setpoint in NED.
Definition: guidance_h.h:74
GUIDANCE_H_MODE_MODULE
#define GUIDANCE_H_MODE_MODULE
Definition: guidance_h.h:64
GUIDANCE_H_MODE_RC_DIRECT
#define GUIDANCE_H_MODE_RC_DIRECT
Definition: guidance_h.h:61
HorizontalGuidanceReference::accel
struct Int32Vect2 accel
with INT32_ACCEL_FRAC
Definition: guidance_h.h:84
PPRZ_ITRIG_COS
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:112
FLOAT_EULERS_ZERO
#define FLOAT_EULERS_ZERO(_e)
Definition: pprz_algebra_float.h:519
navigation_target
struct EnuCoor_i navigation_target
Definition: navigation.c:96
nav_roll
int32_t nav_roll
Definition: navigation.c:112
navigation_carrot
struct EnuCoor_i navigation_carrot
Definition: navigation.c:97
guidance_module.h
guidance_h_set_igain
void guidance_h_set_igain(uint32_t igain)
Definition: guidance_h.c:691
POS_FLOAT_OF_BFP
#define POS_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:218
HorizontalGuidanceSetpoint::heading
float heading
Definition: guidance_h.h:76
HorizontalGuidance
Definition: guidance_h.h:95
SPEED_FLOAT_OF_BFP
#define SPEED_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:220
stateGetPositionNed_i
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:665
guidance_v.h
nav_heading
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:113
HorizontalGuidanceGains::d
int32_t d
Definition: guidance_h.h:89
HorizontalGuidanceReference::speed
struct Int32Vect2 speed
with INT32_SPEED_FRAC
Definition: guidance_h.h:83
HorizontalGuidance::use_ref
bool use_ref
Definition: guidance_h.h:98
nav_cmd_pitch
int32_t nav_cmd_pitch
Definition: navigation.c:114
GUIDANCE_H_MODE_FLIP
#define GUIDANCE_H_MODE_FLIP
Definition: guidance_h.h:65
guidance_h_set_heading_rate
void guidance_h_set_heading_rate(float rate)
Set heading rate setpoint.
Definition: guidance_h.c:748
gh_set_ref
void gh_set_ref(struct Int32Vect2 pos, struct FloatVect2 speed, struct FloatVect2 accel)
Definition: guidance_h_ref.c:95
INT_VECT2_ZERO
#define INT_VECT2_ZERO(_v)
Definition: pprz_algebra_int.h:238
HORIZONTAL_MODE_MANUAL
#define HORIZONTAL_MODE_MANUAL
Definition: navigation.h:64
guidance_h_pos_err
struct Int32Vect2 guidance_h_pos_err
Definition: guidance_h.c:97
stabilization_attitude_read_rc_setpoint_eulers_f
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool in_flight, bool in_carefree, bool coordinated_turn)
Definition: stabilization_attitude_rc_setpoint.c:252
MAX_POS_ERR
#define MAX_POS_ERR
Definition: guidance_h.c:474
VECT2_DIFF
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
VECT2_COPY
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
VECT2_STRIM
#define VECT2_STRIM(_v, _min, _max)
Definition: pprz_algebra.h:110
guidance_h_traj_run
static void guidance_h_traj_run(bool in_flight)
Definition: guidance_h.c:486
INT32_ACCEL_FRAC
#define INT32_ACCEL_FRAC
Definition: pprz_algebra_int.h:78
FloatVect2
Definition: pprz_algebra_float.h:49
telemetry.h
nav_pitch
float nav_pitch
Definition: nav.c:311
Int32Vect2::x
int32_t x
Definition: pprz_algebra_int.h:84
reset_guidance_reference_from_current_position
static void reset_guidance_reference_from_current_position(void)
Definition: guidance_h.c:212
GUIDANCE_H_APPROX_FORCE_BY_THRUST
#define GUIDANCE_H_APPROX_FORCE_BY_THRUST
Definition: guidance_h.c:77
Int32Eulers::psi
int32_t psi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:149
Int32Eulers::phi
int32_t phi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:147
HorizontalGuidanceSetpoint::speed
struct Int32Vect2 speed
only used in HOVER mode if GUIDANCE_H_USE_SPEED_REF or in GUIDED mode
Definition: guidance_h.h:75
GUIDANCE_H_MODE_HOVER
#define GUIDANCE_H_MODE_HOVER
Definition: guidance_h.h:59
gh_update_ref_from_pos_sp
void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp)
Definition: guidance_h_ref.c:105
stateGetSpeedNed_i
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:863
guidance_h_run
void guidance_h_run(bool in_flight)
Definition: guidance_h.c:370
HorizontalGuidance::mode
uint8_t mode
Definition: guidance_h.h:96
GUIDANCE_H_MODE_FORWARD
#define GUIDANCE_H_MODE_FORWARD
Definition: guidance_h.h:63
guidance_h_set_vel
void guidance_h_set_vel(float vx, float vy)
Set horizontal velocity setpoint.
Definition: guidance_h.c:741
ANGLE_BFP_OF_REAL
#define ANGLE_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:211
NedCoor_i
vector in North East Down coordinates
Definition: pprz_geodetic_int.h:68
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
send_hover_loop
static void send_hover_loop(struct transport_tx *trans, struct link_device *dev)
Definition: guidance_h.c:126
guidance_h_trim_att_integrator
struct Int32Vect2 guidance_h_trim_att_integrator
Definition: guidance_h.c:99
guidance_indi_enter
void guidance_indi_enter(void)
Call upon entering indi guidance.
Definition: guidance_indi.c:159
guidance_flip_run
void guidance_flip_run(void)
Definition: guidance_flip.c:65
INT_MULT_RSHIFT
#define INT_MULT_RSHIFT(_a, _b, _r)
Definition: pprz_algebra_int.h:226
GUIDANCE_H_USE_REF
#define GUIDANCE_H_USE_REF
Use horizontal guidance reference trajectory.
Definition: guidance_h.h:45
Int32Vect2
Definition: pprz_algebra_int.h:83
stabilization_attitude_rc_setpoint.h
ACCEL_BFP_OF_REAL
#define ACCEL_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:221
HorizontalGuidance::approx_force_by_thrust
bool approx_force_by_thrust
Definition: guidance_h.h:99
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
GuidanceHRef::speed
struct FloatVect2 speed
Reference model speed.
Definition: guidance_h_ref.h:70
RadioControl::status
uint8_t status
Definition: radio_control.h:61
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
gh_ref_init
void gh_ref_init(void)
Definition: guidance_h_ref.c:51
stabilization_attitude_run
void stabilization_attitude_run(bool in_flight)
Definition: stabilization_attitude_euler_float.c:176
HorizontalGuidanceSetpoint::heading_rate
float heading_rate
Definition: guidance_h.h:77
guidance_flip_enter
void guidance_flip_enter(void)
Definition: guidance_flip.c:56
nav_cmd_roll
int32_t nav_cmd_roll
Definition: navigation.c:114
BFP_OF_REAL
#define BFP_OF_REAL(_vr, _frac)
Definition: pprz_algebra_int.h:206
guidance_hybrid_run
void guidance_hybrid_run(void)
Runs the Hybrid Guidance main functions.
Definition: guidance_hybrid.c:129
HorizontalGuidance::sp
struct HorizontalGuidanceSetpoint sp
setpoints
Definition: guidance_h.h:103
stabilization_rate_enter
void stabilization_rate_enter(void)
Definition: stabilization_rate.c:182
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
HorizontalGuidance::gains
struct HorizontalGuidanceGains gains
Definition: guidance_h.h:101
GUIDANCE_H_MODE_KILL
#define GUIDANCE_H_MODE_KILL
Definition: guidance_h.h:56
stabilization_rate.h
stabilization_attitude_reset_care_free_heading
void stabilization_attitude_reset_care_free_heading(void)
reset the heading for care-free mode to current heading
Definition: stabilization_attitude_rc_setpoint.c:122
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
HorizontalGuidanceGains::v
int32_t v
Definition: guidance_h.h:91
GUIDANCE_H_MODE_RATE
#define GUIDANCE_H_MODE_RATE
Definition: guidance_h.h:57
GUIDANCE_H_AGAIN
#define GUIDANCE_H_AGAIN
Definition: guidance_h.c:53
INT32_VECT2_RSHIFT
#define INT32_VECT2_RSHIFT(_o, _i, _r)
Definition: pprz_algebra_int.h:269
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
GUIDANCE_H_VGAIN
#define GUIDANCE_H_VGAIN
Definition: guidance_h.c:57
stabilization_attitude_enter
void stabilization_attitude_enter(void)
Definition: stabilization_attitude_euler_float.c:136
guidance_h_speed_err
struct Int32Vect2 guidance_h_speed_err
Definition: guidance_h.c:98
send_href
static void send_href(struct transport_tx *trans, struct link_device *dev)
Definition: guidance_h.c:148
RC_OK
#define RC_OK
Definition: radio_control.h:49
read_rc_setpoint_speed_i
static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight)
read speed setpoint from RC
Definition: guidance_h.c:661
guidance_h_read_rc
void guidance_h_read_rc(bool in_flight)
Definition: guidance_h.c:311
stabilization_none_enter
void stabilization_none_enter(void)
Definition: stabilization_none.c:51
guidance_hybrid.h
guidance_h_hover_enter
void guidance_h_hover_enter(void)
Definition: guidance_h.c:556
guidance_h_nav_enter
void guidance_h_nav_enter(void)
Definition: guidance_h.c:574
guidance_h_set_pos
void guidance_h_set_pos(float x, float y)
Set horizontal position setpoint.
Definition: guidance_h.c:719
transition_run
static void transition_run(bool to_forward)
Definition: guidance_h.c:643
navigation.h
send_tune_hover
static void send_tune_hover(struct transport_tx *trans, struct link_device *dev)
Definition: guidance_h.c:159
guidance_v_thrust_coeff
int32_t guidance_v_thrust_coeff
Definition: guidance_v.c:142
transition_percentage
int32_t transition_percentage
Definition: guidance_h.c:92
guidance_h_guided_run
void guidance_h_guided_run(bool in_flight)
Run GUIDED mode control.
Definition: guidance_h.c:698
stabilization_cmd
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
HorizontalGuidanceReference::pos
struct Int32Vect2 pos
with INT32_POS_FRAC
Definition: guidance_h.h:82
guidance_h_set_body_vel
void guidance_h_set_body_vel(float vx, float vy)
Set body relative horizontal velocity setpoint.
Definition: guidance_h.c:733
NedCoor_i::x
int32_t x
North.
Definition: pprz_geodetic_int.h:69
guidance_h
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:90
GH_POS_REF_FRAC
#define GH_POS_REF_FRAC
fixedpoint representation: Q26.37 will give a range of 67e3 km and a resolution of 1....
Definition: guidance_h_ref.h:58
INT32_ANGLE_PI_2
#define INT32_ANGLE_PI_2
Definition: pprz_algebra_int.h:119
stateGetAccelNed_i
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:1020
send_gh
static void send_gh(struct transport_tx *trans, struct link_device *dev)
Definition: guidance_h.c:117
guidance_h_from_nav
void guidance_h_from_nav(bool in_flight)
Set horizontal guidance from NAV and run control loop.
Definition: guidance_h.c:586
stabilization_rate_run
void stabilization_rate_run(bool in_flight)
Definition: stabilization_rate.c:187
stabilization_rate_read_rc_switched_sticks
void stabilization_rate_read_rc_switched_sticks(void)
Definition: stabilization_rate.c:160
guidance_indi.h
POS_BFP_OF_REAL
#define POS_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:217
stabilization_attitude_set_earth_cmd_i
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
Definition: stabilization_attitude_euler_float.c:159
stabilization_attitude_set_rpy_setpoint_i
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
Definition: stabilization_attitude_euler_float.c:154
guidance_h_module_enter
void guidance_h_module_enter(void)
Definition: ctrl_module_innerloop_demo.c:85
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
stateGetNedToBodyEulers_i
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
gh_update_ref_from_speed_sp
void gh_update_ref_from_speed_sp(struct FloatVect2 speed_sp)
Definition: guidance_h_ref.c:143
HorizontalGuidanceGains::p
int32_t p
Definition: guidance_h.h:88
guidance_h_module_init
void guidance_h_module_init(void)
Definition: ctrl_module_innerloop_demo.c:80
HorizontalGuidanceGains::a
int32_t a
Definition: guidance_h.h:92
stabilization.h
state.h
INT32_TRIG_FRAC
#define INT32_TRIG_FRAC
Definition: pprz_algebra_int.h:154
FALSE
#define FALSE
Definition: std.h:5
GUIDANCE_H_MODE_NAV
#define GUIDANCE_H_MODE_NAV
Definition: guidance_h.h:60
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
MAX_SPEED_ERR
#define MAX_SPEED_ERR
Definition: guidance_h.c:475
TRUE
#define TRUE
Definition: std.h:4
INT32_PERCENTAGE_FRAC
#define INT32_PERCENTAGE_FRAC
Definition: pprz_algebra_int.h:81
guidance_h.h
INT32_POS_FRAC
#define INT32_POS_FRAC
Definition: pprz_algebra_int.h:68
HORIZONTAL_MODE_ATTITUDE
#define HORIZONTAL_MODE_ATTITUDE
Definition: navigation.h:63
GuidanceHRef::pos
struct Int64Vect2 pos
Reference model position.
Definition: guidance_h_ref.h:76
horizontal_mode
uint8_t horizontal_mode
Definition: nav.c:73
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
stabilization_none.h
GUIDANCE_H_MODE_CARE_FREE
#define GUIDANCE_H_MODE_CARE_FREE
Definition: guidance_h.h:62
guidance_h_set_heading
void guidance_h_set_heading(float heading)
Set heading setpoint.
Definition: guidance_h.c:726
speed_sp
struct FloatVect3 speed_sp
Definition: guidance_indi.c:76
HorizontalGuidance::rc_sp
struct FloatEulers rc_sp
Definition: guidance_h.h:106
stabilization_none_run
void stabilization_none_run(bool in_flight)
Definition: stabilization_none.c:56
guidance_indi_run
void guidance_indi_run(float *heading_sp)
Definition: guidance_indi.c:179
radio_control
struct RadioControl radio_control
Definition: radio_control.c:33
guidance_h_get_pos_err
const struct Int32Vect2 * guidance_h_get_pos_err(void)
Gets the position error.
Definition: guidance_h.c:754
stabilization_none_read_rc
void stabilization_none_read_rc(void)
Definition: stabilization_none.c:43
guidance_h_init
void guidance_h_init(void)
Definition: guidance_h.c:176
Min
#define Min(x, y)
Definition: esc_dshot.c:85
RadioControl::values
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:67
GUIDANCE_H_THRUST_CMD_FILTER
#define GUIDANCE_H_THRUST_CMD_FILTER
Definition: guidance_h.c:478
heading
float heading
Definition: wedgebug.c:258
GUIDANCE_H_REF_MAX_SPEED
#define GUIDANCE_H_REF_MAX_SPEED
Default speed saturation.
Definition: guidance_h_ref.h:39
PPRZ_ITRIG_SIN
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:111