Paparazzi UAS  v6.1.0_stable
Paparazzi is a free software Unmanned Aircraft System.
navigation.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 
29 #define NAV_C
30 
32 
33 #include "pprz_debug.h"
34 #include "modules/gps/gps.h" // needed by auto_nav from the flight plan
35 #include "modules/ins/ins.h"
36 #include "state.h"
37 
38 #include "autopilot.h"
39 #include "generated/modules.h"
40 #include "generated/flight_plan.h"
41 
42 /* for default GUIDANCE_H_USE_REF */
44 
45 #include "math/pprz_algebra_int.h"
46 
48 #include "pprzlink/messages.h"
49 #include "mcu_periph/uart.h"
50 
51 
52 PRINT_CONFIG_VAR(NAVIGATION_FREQUENCY)
53 
54 
55 #ifndef DEFAULT_CIRCLE_RADIUS
56 #define DEFAULT_CIRCLE_RADIUS 5.
57 #endif
58 
59 #ifndef NAV_CLIMB_VSPEED
60 #define NAV_CLIMB_VSPEED 0.5
61 #endif
62 
63 #ifndef NAV_DESCEND_VSPEED
64 #define NAV_DESCEND_VSPEED -0.8
65 #endif
66 
68 #ifndef ARRIVED_AT_WAYPOINT
69 #define ARRIVED_AT_WAYPOINT 3.0
70 #endif
71 
73 #ifndef FAILSAFE_MODE_DISTANCE
74 #define FAILSAFE_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)
75 #endif
76 
77 #define CLOSE_TO_WAYPOINT (15 << INT32_POS_FRAC)
78 #define CARROT_DIST (12 << INT32_POS_FRAC)
79 
80 bool force_forward = false;
81 
82 struct FloatVect2 line_vect, to_end_vect;
83 
84 const float max_dist_from_home = MAX_DIST_FROM_HOME;
85 const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME;
89 
91 
94 
96 
98 
99 bool exception_flag[10] = {0}; //exception flags that can be used in the flight plan
100 
102 
105 
107 
113 
118 
119 /* nav_circle variables */
122 
123 /* nav_route variables */
124 struct EnuCoor_i nav_segment_start, nav_segment_end;
125 
126 static inline void nav_set_altitude(void);
127 
128 #if PERIODIC_TELEMETRY
130 
132 {
133  exception_flag[flag_num] = 1;
134 }
135 
136 static void send_segment(struct transport_tx *trans, struct link_device *dev)
137 {
138  float sx = POS_FLOAT_OF_BFP(nav_segment_start.x);
139  float sy = POS_FLOAT_OF_BFP(nav_segment_start.y);
140  float ex = POS_FLOAT_OF_BFP(nav_segment_end.x);
141  float ey = POS_FLOAT_OF_BFP(nav_segment_end.y);
142  pprz_msg_send_SEGMENT(trans, dev, AC_ID, &sx, &sy, &ex, &ey);
143 }
144 
145 static void send_circle(struct transport_tx *trans, struct link_device *dev)
146 {
150  pprz_msg_send_CIRCLE(trans, dev, AC_ID, &cx, &cy, &r);
151 }
152 
153 static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
154 {
155  float dist_home = sqrtf(dist2_to_home);
156  float dist_wp = sqrtf(dist2_to_wp);
157  pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID,
159  &dist_home, &dist_wp,
160  &nav_block, &nav_stage,
161  &horizontal_mode);
163  send_segment(trans, dev);
164  } else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) {
165  send_circle(trans, dev);
166  }
167 }
168 
169 static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
170 {
171  static uint8_t i;
172  i++;
173  if (i >= nb_waypoint) { i = 0; }
174  pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID,
175  &i,
176  &(waypoints[i].enu_i.x),
177  &(waypoints[i].enu_i.y),
178  &(waypoints[i].enu_i.z));
179 }
180 #endif
181 
182 void nav_init(void)
183 {
184  waypoints_init();
185 
186  nav_block = 0;
187  nav_stage = 0;
188  nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
190  flight_altitude = SECURITY_ALT;
191  VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i);
192  VECT3_COPY(navigation_carrot, waypoints[WP_HOME].enu_i);
193 
196 
197  nav_roll = 0;
198  nav_pitch = 0;
199  nav_heading = 0;
200  nav_cmd_roll = 0;
201  nav_cmd_pitch = 0;
202  nav_cmd_yaw = 0;
206  nav_throttle = 0;
207  nav_climb = 0;
208  nav_leg_progress = 0;
209  nav_leg_length = 1;
210 
211  too_far_from_home = false;
212  dist2_to_home = 0;
213  dist2_to_wp = 0;
214 
215  FLOAT_VECT2_ZERO(line_vect);
217 
218 #if PERIODIC_TELEMETRY
219  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_NAV_STATUS, send_nav_status);
221 #endif
222 
223  // generated init function
224  auto_nav_init();
225 }
226 
227 static inline void UNUSED nav_advance_carrot(void)
228 {
229  struct EnuCoor_i *pos = stateGetPositionEnu_i();
230  /* compute a vector to the waypoint */
231  struct Int32Vect2 path_to_waypoint;
232  VECT2_DIFF(path_to_waypoint, navigation_target, *pos);
233 
234  /* saturate it */
235  VECT2_STRIM(path_to_waypoint, -(1 << 15), (1 << 15));
236 
237  int32_t dist_to_waypoint = int32_vect2_norm(&path_to_waypoint);
238 
239  if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
241  } else {
242  struct Int32Vect2 path_to_carrot;
243  VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST);
244  VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint);
245  VECT2_SUM(navigation_carrot, path_to_carrot, *pos);
246  }
247 }
248 
249 void nav_run(void)
250 {
251 
252 #if GUIDANCE_H_USE_REF
253  // if GUIDANCE_H_USE_REF, CARROT_DIST is not used
255 #else
257 #endif
258 
260 }
261 
262 
263 bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
264 {
265  float dist_to_point;
266  struct Int32Vect2 diff;
267  struct EnuCoor_i *pos = stateGetPositionEnu_i();
268 
269  /* if an approaching_time is given, estimate diff after approching_time secs */
270  if (approaching_time > 0) {
271  struct Int32Vect2 estimated_pos;
272  struct Int32Vect2 estimated_progress;
273  struct EnuCoor_i *speed = stateGetSpeedEnu_i();
274  VECT2_SMUL(estimated_progress, *speed, approaching_time);
275  INT32_VECT2_RSHIFT(estimated_progress, estimated_progress, (INT32_SPEED_FRAC - INT32_POS_FRAC));
276  VECT2_SUM(estimated_pos, *pos, estimated_progress);
277  VECT2_DIFF(diff, *wp, estimated_pos);
278  }
279  /* else use current position */
280  else {
281  VECT2_DIFF(diff, *wp, *pos);
282  }
283  /* compute distance of estimated/current pos to target wp
284  * POS_FRAC resolution
285  * convert to float to compute the norm without overflow in 32bit
286  */
287  struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
288  dist_to_point = float_vect2_norm(&diff_f);
289 
290  /* return TRUE if we have arrived */
291  if (dist_to_point < ARRIVED_AT_WAYPOINT) {
292  return true;
293  }
294 
295  /* if coming from a valid waypoint */
296  if (from != NULL) {
297  /* return TRUE if normal line at the end of the segment is crossed */
298  struct Int32Vect2 from_diff;
299  VECT2_DIFF(from_diff, *wp, *from);
300  struct FloatVect2 from_diff_f = {POS_FLOAT_OF_BFP(from_diff.x), POS_FLOAT_OF_BFP(from_diff.y)};
301  return (diff_f.x * from_diff_f.x + diff_f.y * from_diff_f.y < 0);
302  }
303 
304  return false;
305 }
306 
307 bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
308 {
309  uint16_t time_at_wp;
310  float dist_to_point;
311  static uint16_t wp_entry_time = 0;
312  static bool wp_reached = false;
313  static struct EnuCoor_i wp_last = { 0, 0, 0 };
314  struct Int32Vect2 diff;
315 
316  if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) {
317  wp_reached = false;
318  wp_last = *wp;
319  }
320 
321  VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
322  struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
323  dist_to_point = float_vect2_norm(&diff_f);
324  if (dist_to_point < ARRIVED_AT_WAYPOINT) {
325  if (!wp_reached) {
326  wp_reached = true;
327  wp_entry_time = autopilot.flight_time;
328  time_at_wp = 0;
329  } else {
330  time_at_wp = autopilot.flight_time - wp_entry_time;
331  }
332  } else {
333  time_at_wp = 0;
334  wp_reached = false;
335  }
336  if (time_at_wp > stay_time) {
337  INT_VECT3_ZERO(wp_last);
338  return true;
339  }
340  return false;
341 }
342 
343 static inline void nav_set_altitude(void)
344 {
345  static int32_t last_nav_alt = 0;
346  if (abs(nav_altitude - last_nav_alt) > (POS_BFP_OF_REAL(0.2))) {
348  last_nav_alt = nav_altitude;
349  }
350 }
351 
352 
355 {
357  /* update local ENU coordinates of global waypoints */
359 }
360 
361 void nav_reset_alt(void)
362 {
365 }
366 
367 void nav_init_stage(void)
368 {
370  stage_time = 0;
371  nav_circle_radians = 0;
372 }
373 
374 #include <stdio.h>
376 {
377  RunOnceEvery(NAVIGATION_FREQUENCY, { stage_time++; block_time++; });
378 
379  nav_survey_active = false;
380 
381  dist2_to_wp = 0;
382 
383  /* from flight_plan.h */
384  auto_nav();
385 
386  /* run carrot loop */
387  nav_run();
388 }
389 
391 {
392  if (!autopilot.ground_detected) { return false; }
393  autopilot.ground_detected = false;
394  return true;
395 }
396 
398 {
399  return autopilot_in_flight();
400 }
401 
403 void nav_home(void)
404 {
406  VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i);
407 
409  nav_altitude = waypoints[WP_HOME].enu_i.z;
411 
413 
414  /* run carrot loop */
415  nav_run();
416 }
417 
428 void nav_set_manual(int32_t roll, int32_t pitch, int32_t yaw)
429 {
431  nav_cmd_roll = roll;
432  nav_cmd_pitch = pitch;
433  nav_cmd_yaw = yaw;
434 }
435 
438 {
439  struct EnuCoor_f *pos = stateGetPositionEnu_f();
440  struct FloatVect2 pos_diff;
441  pos_diff.x = POS_FLOAT_OF_BFP(p->x) - pos->x;
442  pos_diff.y = POS_FLOAT_OF_BFP(p->y) - pos->y;
443  return pos_diff.x * pos_diff.x + pos_diff.y * pos_diff.y;
444 }
445 
448 {
449  return get_dist2_to_point(&waypoints[wp_id].enu_i);
450 }
451 
456 {
459 #ifdef InGeofenceSector
460  struct EnuCoor_f *pos = stateGetPositionEnu_f();
461  too_far_from_home = too_far_from_home || !(InGeofenceSector(pos->x, pos->y));
462 #endif
463 }
464 
466 void nav_set_heading_rad(float rad)
467 {
470 }
471 
473 void nav_set_heading_deg(float deg)
474 {
475  nav_set_heading_rad(RadOfDeg(deg));
476 }
477 
479 void nav_set_heading_towards(float x, float y)
480 {
481  struct FloatVect2 target = {x, y};
482  struct FloatVect2 pos_diff;
484  // don't change heading if closer than 0.5m to target
485  if (VECT2_NORM2(pos_diff) > 0.25) {
486  float heading_f = atan2f(pos_diff.x, pos_diff.y);
487  nav_heading = ANGLE_BFP_OF_REAL(heading_f);
488  }
489 }
490 
493 {
495 }
496 
499 {
502 }
503 
506 {
508 }
509 
511 {
513 }
514 
515 
516 /***********************************************************
517  * built in navigation routines
518  **********************************************************/
519 
520 void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
521 {
522  if (radius == 0) {
523  VECT2_COPY(navigation_target, *wp_center);
524  dist2_to_wp = get_dist2_to_point(wp_center);
525  } else {
526  struct Int32Vect2 pos_diff;
527  VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_center);
528  // go back to half metric precision or values are too large
529  //INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
530  // store last qdr
531  int32_t last_qdr = nav_circle_qdr;
532  // compute qdr
534  // increment circle radians
535  if (nav_circle_radians != 0) {
536  int32_t angle_diff = nav_circle_qdr - last_qdr;
537  INT32_ANGLE_NORMALIZE(angle_diff);
538  nav_circle_radians += angle_diff;
539  } else {
540  // Smallest angle to increment at next step
541  nav_circle_radians = 1;
542  }
543 
544  // direction of rotation
545  int8_t sign_radius = radius > 0 ? 1 : -1;
546  // absolute radius
547  int32_t abs_radius = abs(radius);
548  // carrot_angle
549  int32_t carrot_angle = ((CARROT_DIST << INT32_ANGLE_FRAC) / abs_radius);
550  Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
551  carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
552  int32_t s_carrot, c_carrot;
553  PPRZ_ITRIG_SIN(s_carrot, carrot_angle);
554  PPRZ_ITRIG_COS(c_carrot, carrot_angle);
555  // compute setpoint
556  VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot);
558  VECT2_SUM(navigation_target, *wp_center, pos_diff);
559  }
560  nav_circle_center = *wp_center;
561  nav_circle_radius = radius;
563 }
564 
565 
566 void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
567 {
568  struct Int32Vect2 wp_diff, pos_diff, wp_diff_prec;
569  VECT2_DIFF(wp_diff, *wp_end, *wp_start);
570  VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_start);
571  // go back to metric precision or values are too large
572  VECT2_COPY(wp_diff_prec, wp_diff);
573  INT32_VECT2_RSHIFT(wp_diff, wp_diff, INT32_POS_FRAC);
575  uint32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y), 1);
576  nav_leg_length = int32_sqrt(leg_length2);
577  nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / (int32_t)nav_leg_length;
578  int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
579  nav_leg_progress += progress;
580  int32_t prog_2 = nav_leg_length;
581  Bound(nav_leg_progress, 0, prog_2);
582  struct Int32Vect2 progress_pos;
583  VECT2_SMUL(progress_pos, wp_diff_prec, ((float)nav_leg_progress) / nav_leg_length);
584  VECT2_SUM(navigation_target, *wp_start, progress_pos);
585 
586  nav_segment_start = *wp_start;
587  nav_segment_end = *wp_end;
589 
591 }
592 
593 
594 /************** Oval Navigation **********************************************/
595 
596 #ifndef LINE_START_FUNCTION
597 #define LINE_START_FUNCTION {}
598 #endif
599 #ifndef LINE_STOP_FUNCTION
600 #define LINE_STOP_FUNCTION {}
601 #endif
602 
606 
607 void nav_oval_init(void)
608 {
609  oval_status = OC2;
610  nav_oval_count = 0;
611 }
612 
619 void nav_oval(uint8_t p1, uint8_t p2, float radius)
620 {
621  radius = - radius; /* Historical error ? */
622  int32_t alt = waypoints[p1].enu_i.z;
623  waypoints[p2].enu_i.z = alt;
624 
625  float p2_p1_x = waypoints[p1].enu_f.x - waypoints[p2].enu_f.x;
626  float p2_p1_y = waypoints[p1].enu_f.y - waypoints[p2].enu_f.y;
627  float d = sqrtf(p2_p1_x * p2_p1_x + p2_p1_y * p2_p1_y);
628 
629  /* Unit vector from p1 to p2 */
630  int32_t u_x = POS_BFP_OF_REAL(p2_p1_x / d);
631  int32_t u_y = POS_BFP_OF_REAL(p2_p1_y / d);
632 
633  /* The half circle centers and the other leg */
634  struct EnuCoor_i p1_center = { waypoints[p1].enu_i.x + radius * -u_y,
635  waypoints[p1].enu_i.y + radius * u_x,
636  alt
637  };
638  struct EnuCoor_i p1_out = { waypoints[p1].enu_i.x + 2 * radius * -u_y,
639  waypoints[p1].enu_i.y + 2 * radius * u_x,
640  alt
641  };
642 
643  struct EnuCoor_i p2_in = { waypoints[p2].enu_i.x + 2 * radius * -u_y,
644  waypoints[p2].enu_i.y + 2 * radius * u_x,
645  alt
646  };
647  struct EnuCoor_i p2_center = { waypoints[p2].enu_i.x + radius * -u_y,
648  waypoints[p2].enu_i.y + radius * u_x,
649  alt
650  };
651 
652  int32_t qdr_out_2 = INT32_ANGLE_PI - int32_atan2_2(u_y, u_x);
653  int32_t qdr_out_1 = qdr_out_2 + INT32_ANGLE_PI;
654  if (radius < 0) {
655  qdr_out_2 += INT32_ANGLE_PI;
656  qdr_out_1 += INT32_ANGLE_PI;
657  }
658  int32_t qdr_anticipation = ANGLE_BFP_OF_REAL(radius > 0 ? -15 : 15);
659 
660  switch (oval_status) {
661  case OC1 :
662  nav_circle(&p1_center, POS_BFP_OF_REAL(-radius));
663  if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_1) - qdr_anticipation)) {
664  oval_status = OR12;
665  InitStage();
667  }
668  return;
669 
670  case OR12:
671  nav_route(&p1_out, &p2_in);
672  if (nav_approaching_from(&p2_in, &p1_out, CARROT)) {
673  oval_status = OC2;
674  nav_oval_count++;
675  InitStage();
677  }
678  return;
679 
680  case OC2 :
681  nav_circle(&p2_center, POS_BFP_OF_REAL(-radius));
682  if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_2) - qdr_anticipation)) {
683  oval_status = OR21;
684  InitStage();
686  }
687  return;
688 
689  case OR21:
690  nav_route(&waypoints[p2].enu_i, &waypoints[p1].enu_i);
691  if (nav_approaching_from(&waypoints[p1].enu_i, &waypoints[p2].enu_i, CARROT)) {
692  oval_status = OC1;
693  InitStage();
695  }
696  return;
697 
698  default: /* Should not occur !!! Doing nothing */
699  return;
700  }
701 }
702 /*
703 #ifdef TRAFFIC_INFO
704 #include "modules/multi/traffic_info.h"
705 
706 void nav_follow(uint8_t ac_id, uint32_t distance, uint32_t height)
707 {
708  struct EnuCoor_i* target = acInfoGetPositionEnu_i(ac_id);
709 
710 
711  float alpha = M_PI / 2 - acInfoGetCourse(ac_id);
712  float ca = cosf(alpha), sa = sinf(alpha);
713  target->x += - distance * ca;
714  target->y += - distance * sa;
715  target->z = (Max(target->z + height, SECURITY_HEIGHT)); // todo add ground height to check
716 
717  ENU_OF_TO_NED(navigation_target, *target);
718 }
719 #else*/
720 void nav_follow(uint8_t __attribute__((unused)) _ac_id, uint32_t __attribute__((unused)) distance,
721  uint32_t __attribute__((unused)) height) {}
722 //#endif
int32_atan2
int32_t int32_atan2(int32_t y, int32_t x)
Definition: pprz_trig_int.c:905
FLOAT_VECT2_ZERO
#define FLOAT_VECT2_ZERO(_v)
Definition: pprz_algebra_float.h:129
nav_flight_altitude
int32_t nav_flight_altitude
Definition: navigation.c:116
ins.h
LINE_START_FUNCTION
#define LINE_START_FUNCTION
Definition: navigation.c:597
int8_t
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
pprz_debug.h
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
block_time
uint16_t block_time
Definition: common_flight_plan.c:33
send_wp_moved
static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
Definition: navigation.c:169
VECT2_SMUL
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:98
nav_circle_center
struct EnuCoor_i nav_circle_center
Definition: navigation.c:120
InitStage
#define InitStage()
Definition: common_flight_plan.h:44
OC1
@ OC1
Definition: navigation.c:603
waypoints_localize_all
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition: waypoints.c:293
nav_climb_vspeed
float nav_climb_vspeed
Definition: navigation.c:112
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
nav_altitude
int32_t nav_altitude
Definition: navigation.c:116
EnuCoor_i::x
int32_t x
East.
Definition: pprz_geodetic_int.h:78
dist2_to_home
float dist2_to_home
squared distance to home waypoint
Definition: navigation.c:87
compute_dist2_to_home
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition: navigation.c:455
waypoints_init
void waypoints_init(void)
initialize global and local waypoints
Definition: waypoints.c:35
pprz_autopilot::flight_time
uint16_t flight_time
flight time in seconds
Definition: autopilot.h:65
WaypointY
#define WaypointY(_wp)
Definition: common_nav.h:46
nav_check_wp_time
bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp
Definition: navigation.c:307
AP_MODE_FAILSAFE
#define AP_MODE_FAILSAFE
Definition: autopilot_static.h:36
send_segment
static void send_segment(struct transport_tx *trans, struct link_device *dev)
Definition: navigation.c:136
INT32_ANGLE_FRAC
#define INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:116
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
Int32Vect2::y
int32_t y
Definition: pprz_algebra_int.h:85
nav_approaching_from
bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
Proximity tests on approaching a wp.
Definition: navigation.c:263
nav_circle_radians
int32_t nav_circle_radians
Status on the current circle.
Definition: navigation.c:121
stateGetPositionEnu_i
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:674
nav_cmd_yaw
int32_t nav_cmd_yaw
Definition: navigation.c:110
nav_circle
void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
Definition: navigation.c:520
WaypointX
#define WaypointX(_wp)
Definition: common_nav.h:45
nav_set_heading_towards
void nav_set_heading_towards(float x, float y)
Set heading to point towards x,y position in local coordinates.
Definition: navigation.c:479
nav_oval_init
void nav_oval_init(void)
Definition: navigation.c:607
PPRZ_ITRIG_COS
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:112
nav_follow
void nav_follow(uint8_t _ac_id, uint32_t distance, uint32_t height)
Definition: navigation.c:720
navigation_target
struct EnuCoor_i navigation_target
Definition: navigation.c:92
UNUSED
uint8_t last_wp UNUSED
Definition: navigation.c:97
nav_roll
int32_t nav_roll
Definition: navigation.c:108
navigation_carrot
struct EnuCoor_i navigation_carrot
Definition: navigation.c:93
POS_FLOAT_OF_BFP
#define POS_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:217
NavQdrCloseTo
#define NavQdrCloseTo(x)
True if x (in degrees) is close to the current QDR (less than 10 degrees)
Definition: nav.h:160
nav_reset_reference
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
Definition: navigation.c:354
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
nav_stage
uint8_t nav_stage
Definition: common_flight_plan.c:35
oval_status
oval_status
Definition: navigation.c:603
get_dist2_to_waypoint
float get_dist2_to_waypoint(uint8_t wp_id)
Returns squared horizontal distance to given waypoint.
Definition: navigation.c:447
nav_heading
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:109
nav_radius
float nav_radius
Definition: navigation.c:111
int32_atan2_2
int32_t int32_atan2_2(int32_t y, int32_t x)
Definition: pprz_trig_int.c:927
nav_leg_progress
int32_t nav_leg_progress
Definition: navigation.c:103
nav_advance_carrot
static void UNUSED nav_advance_carrot(void)
Definition: navigation.c:227
nav_cmd_pitch
int32_t nav_cmd_pitch
Definition: navigation.c:110
waypoints
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
nav_pitch
int32_t nav_pitch
Definition: navigation.c:108
nav_set_heading_current
void nav_set_heading_current(void)
Set heading to the current yaw angle.
Definition: navigation.c:505
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
HORIZONTAL_MODE_MANUAL
#define HORIZONTAL_MODE_MANUAL
Definition: navigation.h:64
pprz_algebra_int.h
Paparazzi fixed point algebra.
nav_set_altitude
static void nav_set_altitude(void)
Definition: navigation.c:343
VECT2_DIFF
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
nav_reset_alt
void nav_reset_alt(void)
Definition: navigation.c:361
HORIZONTAL_MODE_WAYPOINT
#define HORIZONTAL_MODE_WAYPOINT
Definition: nav.h:90
VECT2_COPY
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
VECT2_SDIV
#define VECT2_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:104
VECT2_STRIM
#define VECT2_STRIM(_v, _min, _max)
Definition: pprz_algebra.h:110
nav_init
void nav_init(void)
Navigation Initialisation.
Definition: navigation.c:182
CARROT
#define CARROT
default approaching_time for a wp
Definition: navigation.h:40
autopilot_in_flight
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:284
pos_diff
struct FloatVect2 pos_diff
Definition: obstacle_avoidance.c:75
FloatVect2
Definition: pprz_algebra_float.h:49
nav_descend_vspeed
float nav_descend_vspeed
Definition: navigation.c:112
telemetry.h
vertical_mode
uint8_t vertical_mode
Definition: navigation.c:114
last_wp
uint8_t last_wp
Index of last waypoint.
Definition: navigation.c:52
int16_t
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
LINE_STOP_FUNCTION
#define LINE_STOP_FUNCTION
Definition: navigation.c:600
nav_circle_radius
int32_t nav_circle_radius
Definition: navigation.c:121
float_vect2_norm
static float float_vect2_norm(struct FloatVect2 *v)
Definition: pprz_algebra_float.h:139
Int32Vect2::x
int32_t x
Definition: pprz_algebra_int.h:84
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
Int32Eulers::psi
int32_t psi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:149
stateGetSpeedEnu_i
static struct EnuCoor_i * stateGetSpeedEnu_i(void)
Get ground speed in local ENU coordinates (int).
Definition: state.h:872
gps.h
Device independent GPS code (interface)
autopilot
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
nav_run
void nav_run(void)
Definition: navigation.c:249
horizontal_mode
uint8_t horizontal_mode
Definition: navigation.c:101
DEFAULT_CIRCLE_RADIUS
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition: navigation.c:56
nb_waypoint
const uint8_t nb_waypoint
Definition: common_nav.c:38
CLOSE_TO_WAYPOINT
#define CLOSE_TO_WAYPOINT
Definition: navigation.c:77
nav_last_point
struct EnuCoor_i nav_last_point
Definition: navigation.c:95
ANGLE_BFP_OF_REAL
#define ANGLE_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:210
flight_altitude
float flight_altitude
Dynamically adjustable, reset to nav_altitude when it is changing.
Definition: navigation.c:117
get_dist2_to_point
float get_dist2_to_point(struct EnuCoor_i *p)
Returns squared horizontal distance to given point.
Definition: navigation.c:437
CARROT_DIST
#define CARROT_DIST
Definition: navigation.c:78
nav_block
uint8_t nav_block
Definition: common_flight_plan.c:35
too_far_from_home
bool too_far_from_home
Definition: navigation.c:88
Int32Vect2
Definition: pprz_algebra_int.h:83
to_end_vect
struct FloatVect2 line_vect to_end_vect
Definition: navigation.c:82
HORIZONTAL_MODE_CIRCLE
#define HORIZONTAL_MODE_CIRCLE
Definition: nav.h:92
int32_vect2_norm
static uint32_t int32_vect2_norm(struct Int32Vect2 *v)
return norm of 2D vector
Definition: pprz_algebra_int.h:249
nav_route
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
Definition: navigation.c:566
NAVIGATION_FREQUENCY
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition: nav.h:47
nav_segment_end
struct EnuCoor_i nav_segment_start nav_segment_end
Definition: navigation.c:124
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:46
point::x
float x
Definition: common_nav.h:40
nav_oval
void nav_oval(uint8_t p1, uint8_t p2, float radius)
Navigation along a figure O.
Definition: navigation.c:619
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
pprz_autopilot::ground_detected
bool ground_detected
automatic detection of landing
Definition: autopilot.h:74
int32_sqrt
uint32_t int32_sqrt(uint32_t in)
Definition: pprz_algebra_int.c:30
failsafe_mode_dist2
float failsafe_mode_dist2
maximum squared distance to home wp before going to failsafe mode
Definition: navigation.c:86
nav_cmd_roll
int32_t nav_cmd_roll
Definition: navigation.c:110
VECT2_NORM2
#define VECT2_NORM2(_v)
Definition: pprz_algebra.h:118
force_forward
bool force_forward
Definition: navigation.c:80
autopilot.h
nav_home
void nav_home(void)
Home mode navigation.
Definition: navigation.c:403
stage_time
uint16_t stage_time
In s.
Definition: common_flight_plan.c:33
send_circle
static void send_circle(struct transport_tx *trans, struct link_device *dev)
Definition: navigation.c:145
oval_status
oval_status
Definition: nav.h:58
send_nav_status
static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
Definition: navigation.c:153
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
nav_detect_ground
bool nav_detect_ground(void)
Definition: navigation.c:390
set_exception_flag
void set_exception_flag(uint8_t flag_num)
Definition: navigation.c:131
nav_circle_qdr
int32_t nav_circle_qdr
Definition: navigation.c:121
VECT2_ASSIGN
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:62
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
FAILSAFE_MODE_DISTANCE
#define FAILSAFE_MODE_DISTANCE
Maximum distance from HOME waypoint before going into failsafe mode.
Definition: navigation.c:74
nav_survey_active
bool nav_survey_active
Definition: navigation.c:106
INT32_VECT2_RSHIFT
#define INT32_VECT2_RSHIFT(_o, _i, _r)
Definition: pprz_algebra_int.h:268
nav_periodic_task
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
Definition: navigation.c:375
ins_reset_altitude_ref
void WEAK ins_reset_altitude_ref(void)
INS altitude reference reset.
Definition: ins.c:65
NAV_CLIMB_VSPEED
#define NAV_CLIMB_VSPEED
Definition: navigation.c:60
VERTICAL_MODE_ALT
#define VERTICAL_MODE_ALT
Definition: navigation.h:82
OR12
@ OR12
Definition: navigation.c:603
VECT2_SUM
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:86
ARRIVED_AT_WAYPOINT
#define ARRIVED_AT_WAYPOINT
minimum horizontal distance to waypoint to mark as arrived
Definition: navigation.c:69
INT32_DEG_OF_RAD
#define INT32_DEG_OF_RAD(_rad)
Definition: pprz_algebra_int.h:124
autopilot_set_mode
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:187
navigation.h
point::y
float y
Definition: common_nav.h:41
OR21
@ OR21
Definition: navigation.c:603
nav_set_heading_deg
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition: navigation.c:473
OC2
@ OC2
Definition: navigation.c:603
INT32_ANGLE_PI
#define INT32_ANGLE_PI
Definition: pprz_algebra_int.h:120
nav_init_stage
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition: navigation.c:367
nav_set_manual
void nav_set_manual(int32_t roll, int32_t pitch, int32_t yaw)
Set manual roll, pitch and yaw without stabilization.
Definition: navigation.c:428
POS_BFP_OF_REAL
#define POS_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:216
INT32_ANGLE_PI_4
#define INT32_ANGLE_PI_4
Definition: pprz_algebra_int.h:118
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
INT_VECT3_ZERO
#define INT_VECT3_ZERO(_v)
Definition: pprz_algebra_int.h:287
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
ins_reset_local_origin
void WEAK ins_reset_local_origin(void)
INS local origin reset.
Definition: ins.c:55
Max
#define Max(x, y)
Definition: main_fbw.c:53
state.h
INT32_TRIG_FRAC
#define INT32_TRIG_FRAC
Definition: pprz_algebra_int.h:154
NAV_DESCEND_VSPEED
#define NAV_DESCEND_VSPEED
Definition: navigation.c:64
nav_set_heading_towards_waypoint
void nav_set_heading_towards_waypoint(uint8_t wp)
Set heading in the direction of a waypoint.
Definition: navigation.c:492
nav_is_in_flight
bool nav_is_in_flight(void)
Definition: navigation.c:397
max_dist2_from_home
const float max_dist2_from_home
Definition: navigation.c:85
HORIZONTAL_MODE_ROUTE
#define HORIZONTAL_MODE_ROUTE
Definition: nav.h:91
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
nav_set_heading_rad
void nav_set_heading_rad(float rad)
Set nav_heading in radians.
Definition: navigation.c:466
nav_oval_count
uint8_t nav_oval_count
Navigation along a figure O.
Definition: navigation.c:605
nav_leg_length
uint32_t nav_leg_length
Definition: navigation.c:104
exception_flag
bool exception_flag[10]
Definition: navigation.c:99
guidance_h.h
INT32_POS_FRAC
#define INT32_POS_FRAC
Definition: pprz_algebra_int.h:68
dist2_to_wp
float dist2_to_wp
squared distance to next waypoint
Definition: navigation.c:90
nav_throttle
uint32_t nav_throttle
direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL
Definition: navigation.c:115
target
struct FloatVect2 target
Definition: obstacle_avoidance.c:78
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
EnuCoor_i
vector in East North Up coordinates
Definition: pprz_geodetic_int.h:77
nav_set_heading_towards_target
void nav_set_heading_towards_target(void)
Set heading in the direction of the target.
Definition: navigation.c:498
p
static float p[2][2]
Definition: ins_alt_float.c:268
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
nav_climb
int32_t nav_climb
Definition: navigation.c:116
nav_set_failsafe
void nav_set_failsafe(void)
Definition: navigation.c:510
max_dist_from_home
const float max_dist_from_home
Definition: navigation.c:84
INT32_COURSE_NORMALIZE
#define INT32_COURSE_NORMALIZE(_a)
Definition: pprz_algebra_int.h:131
INT32_ANGLE_NORMALIZE
#define INT32_ANGLE_NORMALIZE(_a)
Definition: pprz_algebra_int.h:126
PPRZ_ITRIG_SIN
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:111