Paparazzi UAS  v6.2_unstable
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 #ifndef NAV_CARROT_DIST
78 #define NAV_CARROT_DIST 12
79 #endif
80 
81 #define CLOSE_TO_WAYPOINT (15 << INT32_POS_FRAC)
82 #define CARROT_DIST ((int32_t) POS_BFP_OF_REAL(NAV_CARROT_DIST))
83 
84 bool force_forward = false;
85 
86 struct FloatVect2 line_vect, to_end_vect;
87 
88 const float max_dist_from_home = MAX_DIST_FROM_HOME;
89 const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME;
93 
95 
98 
100 
102 
103 bool exception_flag[10] = {0}; //exception flags that can be used in the flight plan
104 
106 
109 
111 
117 
122 
123 /* nav_circle variables */
126 
127 /* nav_route variables */
128 struct EnuCoor_i nav_segment_start, nav_segment_end;
129 
130 static inline void nav_set_altitude(void);
131 
132 #if PERIODIC_TELEMETRY
134 
136 {
137  exception_flag[flag_num] = 1;
138 }
139 
140 static void send_segment(struct transport_tx *trans, struct link_device *dev)
141 {
142  float sx = POS_FLOAT_OF_BFP(nav_segment_start.x);
143  float sy = POS_FLOAT_OF_BFP(nav_segment_start.y);
144  float ex = POS_FLOAT_OF_BFP(nav_segment_end.x);
145  float ey = POS_FLOAT_OF_BFP(nav_segment_end.y);
146  pprz_msg_send_SEGMENT(trans, dev, AC_ID, &sx, &sy, &ex, &ey);
147 }
148 
149 static void send_circle(struct transport_tx *trans, struct link_device *dev)
150 {
154  pprz_msg_send_CIRCLE(trans, dev, AC_ID, &cx, &cy, &r);
155 }
156 
157 static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
158 {
159  float dist_home = sqrtf(dist2_to_home);
160  float dist_wp = sqrtf(dist2_to_wp);
161  pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID,
163  &dist_home, &dist_wp,
164  &nav_block, &nav_stage,
165  &horizontal_mode);
167  send_segment(trans, dev);
168  } else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) {
169  send_circle(trans, dev);
170  }
171 }
172 
173 static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
174 {
175  static uint8_t i;
176  i++;
177  if (i >= nb_waypoint) { i = 0; }
178  pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID,
179  &i,
180  &(waypoints[i].enu_i.x),
181  &(waypoints[i].enu_i.y),
182  &(waypoints[i].enu_i.z));
183 }
184 #endif
185 
186 void nav_init(void)
187 {
188  waypoints_init();
189 
190  nav_block = 0;
191  nav_stage = 0;
192  nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
194  flight_altitude = SECURITY_ALT;
195  VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i);
196  VECT3_COPY(navigation_carrot, waypoints[WP_HOME].enu_i);
197 
200 
201  nav_roll = 0;
202  nav_pitch = 0;
203  nav_heading = 0;
204  nav_cmd_roll = 0;
205  nav_cmd_pitch = 0;
206  nav_cmd_yaw = 0;
210  nav_throttle = 0;
211  nav_climb = 0;
212  nav_leg_progress = 0;
213  nav_leg_length = 1;
214 
215  too_far_from_home = false;
216  dist2_to_home = 0;
217  dist2_to_wp = 0;
218 
219  FLOAT_VECT2_ZERO(line_vect);
221 
222 #if PERIODIC_TELEMETRY
223  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_NAV_STATUS, send_nav_status);
225 #endif
226 
227  // generated init function
228  auto_nav_init();
229 }
230 
232 {
233  if (DL_BLOCK_ac_id(buf) != AC_ID) { return; }
234  nav_goto_block(DL_BLOCK_block_id(buf));
235 }
236 
238 {
239  uint8_t ac_id = DL_MOVE_WP_ac_id(buf);
240  if (ac_id != AC_ID) { return; }
242  uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
243  struct LlaCoor_i lla;
244  lla.lat = DL_MOVE_WP_lat(buf);
245  lla.lon = DL_MOVE_WP_lon(buf);
246  /* WP_alt from message is alt above MSL in mm
247  * lla.alt is above ellipsoid in mm
248  */
249  lla.alt = DL_MOVE_WP_alt(buf) - state.ned_origin_i.hmsl +
251  waypoint_move_lla(wp_id, &lla);
252  }
253 }
254 
255 static inline void UNUSED nav_advance_carrot(void)
256 {
257  struct EnuCoor_i *pos = stateGetPositionEnu_i();
258  /* compute a vector to the waypoint */
259  struct Int32Vect2 path_to_waypoint;
260  VECT2_DIFF(path_to_waypoint, navigation_target, *pos);
261 
262  /* saturate it */
263  VECT2_STRIM(path_to_waypoint, -(1 << 15), (1 << 15));
264 
265  int32_t dist_to_waypoint = int32_vect2_norm(&path_to_waypoint);
266 
267  if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
269  } else {
270  struct Int32Vect2 path_to_carrot;
271  VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST);
272  VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint);
273  VECT2_SUM(navigation_carrot, path_to_carrot, *pos);
274  }
275 }
276 
277 void nav_run(void)
278 {
279 
280 #if GUIDANCE_H_USE_REF
281  // if GUIDANCE_H_USE_REF, CARROT_DIST is not used
283 #else
285 #endif
286 
288 }
289 
290 
291 bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
292 {
293  float dist_to_point;
294  struct Int32Vect2 diff;
295  struct EnuCoor_i *pos = stateGetPositionEnu_i();
296 
297  /* if an approaching_time is given, estimate diff after approching_time secs */
298  if (approaching_time > 0) {
299  struct Int32Vect2 estimated_pos;
300  struct Int32Vect2 estimated_progress;
301  struct EnuCoor_i *speed = stateGetSpeedEnu_i();
302  VECT2_SMUL(estimated_progress, *speed, approaching_time);
303  INT32_VECT2_RSHIFT(estimated_progress, estimated_progress, (INT32_SPEED_FRAC - INT32_POS_FRAC));
304  VECT2_SUM(estimated_pos, *pos, estimated_progress);
305  VECT2_DIFF(diff, *wp, estimated_pos);
306  }
307  /* else use current position */
308  else {
309  VECT2_DIFF(diff, *wp, *pos);
310  }
311  /* compute distance of estimated/current pos to target wp
312  * POS_FRAC resolution
313  * convert to float to compute the norm without overflow in 32bit
314  */
315  struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
316  dist_to_point = float_vect2_norm(&diff_f);
317 
318  /* return TRUE if we have arrived */
319  if (dist_to_point < ARRIVED_AT_WAYPOINT) {
320  return true;
321  }
322 
323  /* if coming from a valid waypoint */
324  if (from != NULL) {
325  /* return TRUE if normal line at the end of the segment is crossed */
326  struct Int32Vect2 from_diff;
327  VECT2_DIFF(from_diff, *wp, *from);
328  struct FloatVect2 from_diff_f = {POS_FLOAT_OF_BFP(from_diff.x), POS_FLOAT_OF_BFP(from_diff.y)};
329  return (diff_f.x * from_diff_f.x + diff_f.y * from_diff_f.y < 0);
330  }
331 
332  return false;
333 }
334 
335 bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
336 {
337  uint16_t time_at_wp;
338  float dist_to_point;
339  static uint16_t wp_entry_time = 0;
340  static bool wp_reached = false;
341  static struct EnuCoor_i wp_last = { 0, 0, 0 };
342  struct Int32Vect2 diff;
343 
344  if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) {
345  wp_reached = false;
346  wp_last = *wp;
347  }
348 
349  VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
350  struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
351  dist_to_point = float_vect2_norm(&diff_f);
352  if (dist_to_point < ARRIVED_AT_WAYPOINT) {
353  if (!wp_reached) {
354  wp_reached = true;
355  wp_entry_time = autopilot.flight_time;
356  time_at_wp = 0;
357  } else {
358  time_at_wp = autopilot.flight_time - wp_entry_time;
359  }
360  } else {
361  time_at_wp = 0;
362  wp_reached = false;
363  }
364  if (time_at_wp > stay_time) {
365  INT_VECT3_ZERO(wp_last);
366  return true;
367  }
368  return false;
369 }
370 
371 static inline void nav_set_altitude(void)
372 {
373  static int32_t last_nav_alt = 0;
374  if (abs(nav_altitude - last_nav_alt) > (POS_BFP_OF_REAL(0.2))) {
376  last_nav_alt = nav_altitude;
377  }
378 }
379 
380 
383 {
385  /* update local ENU coordinates of global waypoints */
387 }
388 
389 void nav_reset_alt(void)
390 {
393 }
394 
395 void nav_init_stage(void)
396 {
398  stage_time = 0;
399  nav_circle_radians = 0;
400 }
401 
402 #include <stdio.h>
404 {
405  RunOnceEvery(NAVIGATION_FREQUENCY, { stage_time++; block_time++; });
406 
407  nav_survey_active = false;
408 
409  dist2_to_wp = 0;
410 
411  /* from flight_plan.h */
412  auto_nav();
413 
414  /* run carrot loop */
415  nav_run();
416 }
417 
419 {
420  if (!autopilot.ground_detected) { return false; }
421  autopilot.ground_detected = false;
422  return true;
423 }
424 
426 {
427  return autopilot_in_flight();
428 }
429 
431 void nav_home(void)
432 {
434  VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i);
435 
437  nav_altitude = waypoints[WP_HOME].enu_i.z;
439 
441 
442  /* run carrot loop */
443  nav_run();
444 }
445 
456 void nav_set_manual(int32_t roll, int32_t pitch, int32_t yaw)
457 {
459  nav_cmd_roll = roll;
460  nav_cmd_pitch = pitch;
461  nav_cmd_yaw = yaw;
462 }
463 
466 {
467  struct EnuCoor_f *pos = stateGetPositionEnu_f();
468  struct FloatVect2 pos_diff;
469  pos_diff.x = POS_FLOAT_OF_BFP(p->x) - pos->x;
470  pos_diff.y = POS_FLOAT_OF_BFP(p->y) - pos->y;
471  return pos_diff.x * pos_diff.x + pos_diff.y * pos_diff.y;
472 }
473 
476 {
477  return get_dist2_to_point(&waypoints[wp_id].enu_i);
478 }
479 
484 {
487 #ifdef InGeofenceSector
488  struct EnuCoor_f *pos = stateGetPositionEnu_f();
489  too_far_from_home = too_far_from_home || !(InGeofenceSector(pos->x, pos->y));
490 #endif
491 }
492 
494 void nav_set_heading_rad(float rad)
495 {
498 }
499 
501 void nav_set_heading_deg(float deg)
502 {
503  nav_set_heading_rad(RadOfDeg(deg));
504 }
505 
507 void nav_set_heading_towards(float x, float y)
508 {
509  struct FloatVect2 target = {x, y};
510  struct FloatVect2 pos_diff;
512  // don't change heading if closer than 0.5m to target
513  if (VECT2_NORM2(pos_diff) > 0.25) {
514  float heading_f = atan2f(pos_diff.x, pos_diff.y);
515  nav_heading = ANGLE_BFP_OF_REAL(heading_f);
516  }
517 }
518 
521 {
523 }
524 
527 {
530 }
531 
534 {
536 }
537 
539 {
541 }
542 
543 
544 /***********************************************************
545  * built in navigation routines
546  **********************************************************/
547 
548 void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
549 {
550  if (radius == 0) {
551  VECT2_COPY(navigation_target, *wp_center);
552  dist2_to_wp = get_dist2_to_point(wp_center);
553  } else {
554  struct Int32Vect2 pos_diff;
555  VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_center);
556  // go back to half metric precision or values are too large
557  //INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
558  // store last qdr
559  int32_t last_qdr = nav_circle_qdr;
560  // compute qdr
562  // increment circle radians
563  if (nav_circle_radians != 0) {
564  int32_t angle_diff = nav_circle_qdr - last_qdr;
565  INT32_ANGLE_NORMALIZE(angle_diff);
566  nav_circle_radians += angle_diff;
567  } else {
568  // Smallest angle to increment at next step
569  nav_circle_radians = 1;
570  }
571 
572  // direction of rotation
573  int8_t sign_radius = radius > 0 ? 1 : -1;
574  // absolute radius
575  int32_t abs_radius = abs(radius);
576  // carrot_angle
577  int32_t carrot_angle = ((CARROT_DIST << INT32_ANGLE_FRAC) / abs_radius);
578  Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
579  carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
580  int32_t s_carrot, c_carrot;
581  PPRZ_ITRIG_SIN(s_carrot, carrot_angle);
582  PPRZ_ITRIG_COS(c_carrot, carrot_angle);
583  // compute setpoint
584  VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot);
586  VECT2_SUM(navigation_target, *wp_center, pos_diff);
587  }
588  nav_circle_center = *wp_center;
589  nav_circle_radius = radius;
591 }
592 
593 
594 void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
595 {
596  struct Int32Vect2 wp_diff, pos_diff, wp_diff_prec;
597  VECT2_DIFF(wp_diff, *wp_end, *wp_start);
598  VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_start);
599  // go back to metric precision or values are too large
600  VECT2_COPY(wp_diff_prec, wp_diff);
601  INT32_VECT2_RSHIFT(wp_diff, wp_diff, INT32_POS_FRAC);
603  uint32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y), 1);
604  nav_leg_length = int32_sqrt(leg_length2);
605  nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / (int32_t)nav_leg_length;
606  int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
607  nav_leg_progress += progress;
608  int32_t prog_2 = nav_leg_length;
609  Bound(nav_leg_progress, 0, prog_2);
610  struct Int32Vect2 progress_pos;
611  VECT2_SMUL(progress_pos, wp_diff_prec, ((float)nav_leg_progress) / nav_leg_length);
612  VECT2_SUM(navigation_target, *wp_start, progress_pos);
613 
614  nav_segment_start = *wp_start;
615  nav_segment_end = *wp_end;
617 
619 }
620 
621 
622 /************** Oval Navigation **********************************************/
623 
624 #ifndef LINE_START_FUNCTION
625 #define LINE_START_FUNCTION {}
626 #endif
627 #ifndef LINE_STOP_FUNCTION
628 #define LINE_STOP_FUNCTION {}
629 #endif
630 
634 
635 void nav_oval_init(void)
636 {
637  oval_status = OC2;
638  nav_oval_count = 0;
639 }
640 
647 void nav_oval(uint8_t p1, uint8_t p2, float radius)
648 {
649  radius = - radius; /* Historical error ? */
650  int32_t alt = waypoints[p1].enu_i.z;
651  waypoints[p2].enu_i.z = alt;
652 
653  float p2_p1_x = waypoints[p1].enu_f.x - waypoints[p2].enu_f.x;
654  float p2_p1_y = waypoints[p1].enu_f.y - waypoints[p2].enu_f.y;
655  float d = sqrtf(p2_p1_x * p2_p1_x + p2_p1_y * p2_p1_y);
656 
657  /* Unit vector from p1 to p2 */
658  int32_t u_x = POS_BFP_OF_REAL(p2_p1_x / d);
659  int32_t u_y = POS_BFP_OF_REAL(p2_p1_y / d);
660 
661  /* The half circle centers and the other leg */
662  struct EnuCoor_i p1_center = { waypoints[p1].enu_i.x + radius * -u_y,
663  waypoints[p1].enu_i.y + radius * u_x,
664  alt
665  };
666  struct EnuCoor_i p1_out = { waypoints[p1].enu_i.x + 2 * radius * -u_y,
667  waypoints[p1].enu_i.y + 2 * radius * u_x,
668  alt
669  };
670 
671  struct EnuCoor_i p2_in = { waypoints[p2].enu_i.x + 2 * radius * -u_y,
672  waypoints[p2].enu_i.y + 2 * radius * u_x,
673  alt
674  };
675  struct EnuCoor_i p2_center = { waypoints[p2].enu_i.x + radius * -u_y,
676  waypoints[p2].enu_i.y + radius * u_x,
677  alt
678  };
679 
680  int32_t qdr_out_2 = INT32_ANGLE_PI - int32_atan2_2(u_y, u_x);
681  int32_t qdr_out_1 = qdr_out_2 + INT32_ANGLE_PI;
682  if (radius < 0) {
683  qdr_out_2 += INT32_ANGLE_PI;
684  qdr_out_1 += INT32_ANGLE_PI;
685  }
686  int32_t qdr_anticipation = ANGLE_BFP_OF_REAL(radius > 0 ? -15 : 15);
687 
688  switch (oval_status) {
689  case OC1 :
690  nav_circle(&p1_center, POS_BFP_OF_REAL(-radius));
691  if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_1) - qdr_anticipation)) {
692  oval_status = OR12;
693  InitStage();
695  }
696  return;
697 
698  case OR12:
699  nav_route(&p1_out, &p2_in);
700  if (nav_approaching_from(&p2_in, &p1_out, CARROT)) {
701  oval_status = OC2;
702  nav_oval_count++;
703  InitStage();
705  }
706  return;
707 
708  case OC2 :
709  nav_circle(&p2_center, POS_BFP_OF_REAL(-radius));
710  if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_2) - qdr_anticipation)) {
711  oval_status = OR21;
712  InitStage();
714  }
715  return;
716 
717  case OR21:
718  nav_route(&waypoints[p2].enu_i, &waypoints[p1].enu_i);
719  if (nav_approaching_from(&waypoints[p1].enu_i, &waypoints[p2].enu_i, CARROT)) {
720  oval_status = OC1;
721  InitStage();
723  }
724  return;
725 
726  default: /* Should not occur !!! Doing nothing */
727  return;
728  }
729 }
730 /*
731 #ifdef TRAFFIC_INFO
732 #include "modules/multi/traffic_info.h"
733 
734 void nav_follow(uint8_t ac_id, uint32_t distance, uint32_t height)
735 {
736  struct EnuCoor_i* target = acInfoGetPositionEnu_i(ac_id);
737 
738 
739  float alpha = M_PI / 2 - acInfoGetCourse(ac_id);
740  float ca = cosf(alpha), sa = sinf(alpha);
741  target->x += - distance * ca;
742  target->y += - distance * sa;
743  target->z = (Max(target->z + height, SECURITY_HEIGHT)); // todo add ground height to check
744 
745  ENU_OF_TO_NED(navigation_target, *target);
746 }
747 #else*/
748 void nav_follow(uint8_t __attribute__((unused)) _ac_id, uint32_t __attribute__((unused)) distance,
749  uint32_t __attribute__((unused)) height) {}
750 //#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:120
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
nav_parse_BLOCK
void nav_parse_BLOCK(uint8_t *buf)
Definition: navigation.c:231
ins.h
LINE_START_FUNCTION
#define LINE_START_FUNCTION
Definition: navigation.c:625
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:173
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:124
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
InitStage
#define InitStage()
Definition: common_flight_plan.h:44
OC1
@ OC1
Definition: navigation.c:631
stateIsLocalCoordinateValid
static bool stateIsLocalCoordinateValid(void)
Test if local coordinates are valid.
Definition: state.h:508
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:116
State::ned_origin_i
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
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:120
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:91
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:483
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:335
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:140
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:291
nav_circle_radians
int32_t nav_circle_radians
Status on the current circle.
Definition: navigation.c:125
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:114
nav_circle
void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
Definition: navigation.c:548
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:507
nav_oval_init
void nav_oval_init(void)
Definition: navigation.c:635
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:748
navigation_target
struct EnuCoor_i navigation_target
Definition: navigation.c:96
UNUSED
uint8_t last_wp UNUSED
Definition: navigation.c:101
nav_roll
int32_t nav_roll
Definition: navigation.c:112
navigation_carrot
struct EnuCoor_i navigation_carrot
Definition: navigation.c:97
POS_FLOAT_OF_BFP
#define POS_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:218
NavQdrCloseTo
#define NavQdrCloseTo(x)
True if x (in degrees) is close to the current QDR (less than 10 degrees)
Definition: nav.h:164
nav_reset_reference
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
Definition: navigation.c:382
nav_stage
uint8_t nav_stage
Definition: common_flight_plan.c:35
oval_status
oval_status
Definition: navigation.c:631
get_dist2_to_waypoint
float get_dist2_to_waypoint(uint8_t wp_id)
Returns squared horizontal distance to given waypoint.
Definition: navigation.c:475
nav_heading
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:113
nav_radius
float nav_radius
Definition: navigation.c:115
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:107
nav_advance_carrot
static void UNUSED nav_advance_carrot(void)
Definition: navigation.c:255
nav_cmd_pitch
int32_t nav_cmd_pitch
Definition: navigation.c:114
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:112
nav_set_heading_current
void nav_set_heading_current(void)
Set heading to the current yaw angle.
Definition: navigation.c:533
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:371
nav_parse_MOVE_WP
void nav_parse_MOVE_WP(uint8_t *buf)
Definition: navigation.c:237
VECT2_DIFF
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
nav_reset_alt
void nav_reset_alt(void)
Definition: navigation.c:389
HORIZONTAL_MODE_WAYPOINT
#define HORIZONTAL_MODE_WAYPOINT
Definition: nav.h:92
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:186
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:289
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:116
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
telemetry.h
vertical_mode
uint8_t vertical_mode
Definition: navigation.c:118
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:628
nav_circle_radius
int32_t nav_circle_radius
Definition: navigation.c:125
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:48
nav_run
void nav_run(void)
Definition: navigation.c:277
waypoint_move_lla
void waypoint_move_lla(uint8_t wp_id, struct LlaCoor_i *lla)
Definition: waypoints.c:189
horizontal_mode
uint8_t horizontal_mode
Definition: navigation.c:105
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:81
nav_last_point
struct EnuCoor_i nav_last_point
Definition: navigation.c:99
ANGLE_BFP_OF_REAL
#define ANGLE_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:211
flight_altitude
float flight_altitude
Dynamically adjustable, reset to nav_altitude when it is changing.
Definition: navigation.c:121
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
get_dist2_to_point
float get_dist2_to_point(struct EnuCoor_i *p)
Returns squared horizontal distance to given point.
Definition: navigation.c:465
CARROT_DIST
#define CARROT_DIST
Definition: navigation.c:82
nav_block
uint8_t nav_block
Definition: common_flight_plan.c:35
too_far_from_home
bool too_far_from_home
Definition: navigation.c:92
Int32Vect2
Definition: pprz_algebra_int.h:83
to_end_vect
struct FloatVect2 line_vect to_end_vect
Definition: navigation.c:86
HORIZONTAL_MODE_CIRCLE
#define HORIZONTAL_MODE_CIRCLE
Definition: nav.h:94
int32_vect2_norm
static uint32_t int32_vect2_norm(struct Int32Vect2 *v)
return norm of 2D vector
Definition: pprz_algebra_int.h:250
nav_route
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
Definition: navigation.c:594
NAVIGATION_FREQUENCY
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition: nav.h:49
nav_segment_end
struct EnuCoor_i nav_segment_start nav_segment_end
Definition: navigation.c:128
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
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:647
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:73
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:90
nav_cmd_roll
int32_t nav_cmd_roll
Definition: navigation.c:114
VECT2_NORM2
#define VECT2_NORM2(_v)
Definition: pprz_algebra.h:118
force_forward
bool force_forward
Definition: navigation.c:84
autopilot.h
nav_home
void nav_home(void)
Home mode navigation.
Definition: navigation.c:431
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:149
oval_status
oval_status
Definition: nav.h:60
send_nav_status
static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
Definition: navigation.c:157
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
nav_detect_ground
bool nav_detect_ground(void)
Definition: navigation.c:418
set_exception_flag
void set_exception_flag(uint8_t flag_num)
Definition: navigation.c:135
nav_circle_qdr
int32_t nav_circle_qdr
Definition: navigation.c:125
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:110
INT32_VECT2_RSHIFT
#define INT32_VECT2_RSHIFT(_o, _i, _r)
Definition: pprz_algebra_int.h:269
nav_periodic_task
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
Definition: navigation.c:403
ins_reset_altitude_ref
void WEAK ins_reset_altitude_ref(void)
INS altitude reference reset.
Definition: ins.c:65
LlaCoor_i
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_int.h:59
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
NAV_CLIMB_VSPEED
#define NAV_CLIMB_VSPEED
Definition: navigation.c:60
VERTICAL_MODE_ALT
#define VERTICAL_MODE_ALT
Definition: navigation.h:83
OR12
@ OR12
Definition: navigation.c:631
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:192
navigation.h
point::y
float y
Definition: common_nav.h:41
OR21
@ OR21
Definition: navigation.c:631
nav_set_heading_deg
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition: navigation.c:501
OC2
@ OC2
Definition: navigation.c:631
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:395
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:456
POS_BFP_OF_REAL
#define POS_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:217
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:288
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
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:520
nav_is_in_flight
bool nav_is_in_flight(void)
Definition: navigation.c:425
max_dist2_from_home
const float max_dist2_from_home
Definition: navigation.c:89
ac_id
uint8_t ac_id
Definition: sim_ap.c:43
HORIZONTAL_MODE_ROUTE
#define HORIZONTAL_MODE_ROUTE
Definition: nav.h:93
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:494
state
struct State state
Definition: state.c:36
nav_oval_count
uint8_t nav_oval_count
Navigation along a figure O.
Definition: navigation.c:633
nav_leg_length
uint32_t nav_leg_length
Definition: navigation.c:108
exception_flag
bool exception_flag[10]
Definition: navigation.c:103
guidance_h.h
LtpDef_i::lla
struct LlaCoor_i lla
Reference point in lla.
Definition: pprz_geodetic_int.h:100
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:94
nav_throttle
uint32_t nav_throttle
direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL
Definition: navigation.c:119
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:526
nav_goto_block
void nav_goto_block(uint8_t b)
Definition: common_flight_plan.c:51
p
static float p[2][2]
Definition: ins_alt_float.c:257
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
nav_climb
int32_t nav_climb
Definition: navigation.c:120
nav_set_failsafe
void nav_set_failsafe(void)
Definition: navigation.c:538
max_dist_from_home
const float max_dist_from_home
Definition: navigation.c:88
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