Paparazzi UAS  v5.18.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 "subsystems/gps.h" // needed by auto_nav from the flight plan
35 #include "subsystems/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 
54 #ifndef DEFAULT_CIRCLE_RADIUS
55 #define DEFAULT_CIRCLE_RADIUS 5.
56 #endif
57 
58 #ifndef NAV_CLIMB_VSPEED
59 #define NAV_CLIMB_VSPEED 0.5
60 #endif
61 
62 #ifndef NAV_DESCEND_VSPEED
63 #define NAV_DESCEND_VSPEED -0.8
64 #endif
65 
67 #ifndef ARRIVED_AT_WAYPOINT
68 #define ARRIVED_AT_WAYPOINT 3.0
69 #endif
70 
72 #ifndef FAILSAFE_MODE_DISTANCE
73 #define FAILSAFE_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)
74 #endif
75 
76 #define CLOSE_TO_WAYPOINT (15 << INT32_POS_FRAC)
77 #define CARROT_DIST (12 << INT32_POS_FRAC)
78 
79 bool force_forward = false;
80 
81 struct FloatVect2 line_vect, to_end_vect;
82 
83 const float max_dist_from_home = MAX_DIST_FROM_HOME;
84 const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME;
88 
90 
93 
95 
97 
98 bool exception_flag[10] = {0}; //exception flags that can be used in the flight plan
99 
101 
104 
106 
112 
117 
118 /* nav_circle variables */
121 
122 /* nav_route variables */
123 struct EnuCoor_i nav_segment_start, nav_segment_end;
124 
125 static inline void nav_set_altitude(void);
126 
127 #if PERIODIC_TELEMETRY
129 
131 {
132  exception_flag[flag_num] = 1;
133 }
134 
135 static void send_segment(struct transport_tx *trans, struct link_device *dev)
136 {
137  float sx = POS_FLOAT_OF_BFP(nav_segment_start.x);
138  float sy = POS_FLOAT_OF_BFP(nav_segment_start.y);
139  float ex = POS_FLOAT_OF_BFP(nav_segment_end.x);
140  float ey = POS_FLOAT_OF_BFP(nav_segment_end.y);
141  pprz_msg_send_SEGMENT(trans, dev, AC_ID, &sx, &sy, &ex, &ey);
142 }
143 
144 static void send_circle(struct transport_tx *trans, struct link_device *dev)
145 {
149  pprz_msg_send_CIRCLE(trans, dev, AC_ID, &cx, &cy, &r);
150 }
151 
152 static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
153 {
154  float dist_home = sqrtf(dist2_to_home);
155  float dist_wp = sqrtf(dist2_to_wp);
156  pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID,
158  &dist_home, &dist_wp,
159  &nav_block, &nav_stage,
160  &horizontal_mode);
162  send_segment(trans, dev);
163  } else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) {
164  send_circle(trans, dev);
165  }
166 }
167 
168 static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
169 {
170  static uint8_t i;
171  i++;
172  if (i >= nb_waypoint) { i = 0; }
173  pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID,
174  &i,
175  &(waypoints[i].enu_i.x),
176  &(waypoints[i].enu_i.y),
177  &(waypoints[i].enu_i.z));
178 }
179 #endif
180 
181 void nav_init(void)
182 {
183  waypoints_init();
184 
185  nav_block = 0;
186  nav_stage = 0;
187  nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
189  flight_altitude = SECURITY_ALT;
190  VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i);
191  VECT3_COPY(navigation_carrot, waypoints[WP_HOME].enu_i);
192 
195 
196  nav_roll = 0;
197  nav_pitch = 0;
198  nav_heading = 0;
199  nav_cmd_roll = 0;
200  nav_cmd_pitch = 0;
201  nav_cmd_yaw = 0;
205  nav_throttle = 0;
206  nav_climb = 0;
207  nav_leg_progress = 0;
208  nav_leg_length = 1;
209 
210  too_far_from_home = false;
211  dist2_to_home = 0;
212  dist2_to_wp = 0;
213 
214  FLOAT_VECT2_ZERO(line_vect);
216 
217 #if PERIODIC_TELEMETRY
218  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_NAV_STATUS, send_nav_status);
220 #endif
221 
222  // generated init function
223  auto_nav_init();
224 }
225 
226 static inline void UNUSED nav_advance_carrot(void)
227 {
228  struct EnuCoor_i *pos = stateGetPositionEnu_i();
229  /* compute a vector to the waypoint */
230  struct Int32Vect2 path_to_waypoint;
231  VECT2_DIFF(path_to_waypoint, navigation_target, *pos);
232 
233  /* saturate it */
234  VECT2_STRIM(path_to_waypoint, -(1 << 15), (1 << 15));
235 
236  int32_t dist_to_waypoint = int32_vect2_norm(&path_to_waypoint);
237 
238  if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
240  } else {
241  struct Int32Vect2 path_to_carrot;
242  VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST);
243  VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint);
244  VECT2_SUM(navigation_carrot, path_to_carrot, *pos);
245  }
246 }
247 
248 void nav_run(void)
249 {
250 
251 #if GUIDANCE_H_USE_REF
252  // if GUIDANCE_H_USE_REF, CARROT_DIST is not used
254 #else
256 #endif
257 
259 }
260 
261 
262 bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
263 {
264  float dist_to_point;
265  struct Int32Vect2 diff;
266  struct EnuCoor_i *pos = stateGetPositionEnu_i();
267 
268  /* if an approaching_time is given, estimate diff after approching_time secs */
269  if (approaching_time > 0) {
270  struct Int32Vect2 estimated_pos;
271  struct Int32Vect2 estimated_progress;
272  struct EnuCoor_i *speed = stateGetSpeedEnu_i();
273  VECT2_SMUL(estimated_progress, *speed, approaching_time);
274  INT32_VECT2_RSHIFT(estimated_progress, estimated_progress, (INT32_SPEED_FRAC - INT32_POS_FRAC));
275  VECT2_SUM(estimated_pos, *pos, estimated_progress);
276  VECT2_DIFF(diff, *wp, estimated_pos);
277  }
278  /* else use current position */
279  else {
280  VECT2_DIFF(diff, *wp, *pos);
281  }
282  /* compute distance of estimated/current pos to target wp
283  * POS_FRAC resolution
284  * convert to float to compute the norm without overflow in 32bit
285  */
286  struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
287  dist_to_point = float_vect2_norm(&diff_f);
288 
289  /* return TRUE if we have arrived */
290  if (dist_to_point < ARRIVED_AT_WAYPOINT) {
291  return true;
292  }
293 
294  /* if coming from a valid waypoint */
295  if (from != NULL) {
296  /* return TRUE if normal line at the end of the segment is crossed */
297  struct Int32Vect2 from_diff;
298  VECT2_DIFF(from_diff, *wp, *from);
299  struct FloatVect2 from_diff_f = {POS_FLOAT_OF_BFP(from_diff.x), POS_FLOAT_OF_BFP(from_diff.y)};
300  return (diff_f.x * from_diff_f.x + diff_f.y * from_diff_f.y < 0);
301  }
302 
303  return false;
304 }
305 
306 bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
307 {
308  uint16_t time_at_wp;
309  float dist_to_point;
310  static uint16_t wp_entry_time = 0;
311  static bool wp_reached = false;
312  static struct EnuCoor_i wp_last = { 0, 0, 0 };
313  struct Int32Vect2 diff;
314 
315  if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) {
316  wp_reached = false;
317  wp_last = *wp;
318  }
319 
320  VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
321  struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
322  dist_to_point = float_vect2_norm(&diff_f);
323  if (dist_to_point < ARRIVED_AT_WAYPOINT) {
324  if (!wp_reached) {
325  wp_reached = true;
326  wp_entry_time = autopilot.flight_time;
327  time_at_wp = 0;
328  } else {
329  time_at_wp = autopilot.flight_time - wp_entry_time;
330  }
331  } else {
332  time_at_wp = 0;
333  wp_reached = false;
334  }
335  if (time_at_wp > stay_time) {
336  INT_VECT3_ZERO(wp_last);
337  return true;
338  }
339  return false;
340 }
341 
342 static inline void nav_set_altitude(void)
343 {
344  static int32_t last_nav_alt = 0;
345  if (abs(nav_altitude - last_nav_alt) > (POS_BFP_OF_REAL(0.2))) {
347  last_nav_alt = nav_altitude;
348  }
349 }
350 
351 
354 {
356  /* update local ENU coordinates of global waypoints */
358 }
359 
360 void nav_reset_alt(void)
361 {
364 }
365 
366 void nav_init_stage(void)
367 {
369  stage_time = 0;
370  nav_circle_radians = 0;
371 }
372 
373 #include <stdio.h>
375 {
376  RunOnceEvery(NAV_FREQ, { stage_time++; block_time++; });
377 
378  nav_survey_active = false;
379 
380  dist2_to_wp = 0;
381 
382  /* from flight_plan.h */
383  auto_nav();
384 
385  /* run carrot loop */
386  nav_run();
387 }
388 
390 {
391  // MY_ASSERT(wp < nb_waypoint); FIXME
392  int32_t s_heading, c_heading;
393  PPRZ_ITRIG_SIN(s_heading, nav_heading);
394  PPRZ_ITRIG_COS(c_heading, nav_heading);
395  // FIXME : scale POS to SPEED
396  struct Int32Vect3 delta_pos;
397  VECT3_SDIV(delta_pos, speed_sp, NAV_FREQ); /* fixme :make sure the division is really a >> */
398  INT32_VECT3_RSHIFT(delta_pos, delta_pos, (INT32_SPEED_FRAC - INT32_POS_FRAC));
399  waypoints[wp].enu_i.x += (s_heading * delta_pos.x + c_heading * delta_pos.y) >> INT32_TRIG_FRAC;
400  waypoints[wp].enu_i.y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >> INT32_TRIG_FRAC;
401  waypoints[wp].enu_i.z += delta_pos.z;
402  int32_t delta_heading = heading_rate_sp / NAV_FREQ;
403  delta_heading = delta_heading >> (INT32_SPEED_FRAC - INT32_POS_FRAC);
404  nav_heading += delta_heading;
405 
407  RunOnceEvery(10, DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp,
408  &(waypoints[wp].enu_i.x),
409  &(waypoints[wp].enu_i.y),
410  &(waypoints[wp].enu_i.z)));
411 }
412 
414 {
415  if (!autopilot.ground_detected) { return false; }
416  autopilot.ground_detected = false;
417  return true;
418 }
419 
421 {
422  return autopilot_in_flight();
423 }
424 
426 void nav_home(void)
427 {
429  VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i);
430 
432  nav_altitude = waypoints[WP_HOME].enu_i.z;
434 
436 
437  /* run carrot loop */
438  nav_run();
439 }
440 
451 void nav_set_manual(int32_t roll, int32_t pitch, int32_t yaw)
452 {
454  nav_cmd_roll = roll;
455  nav_cmd_pitch = pitch;
456  nav_cmd_yaw = yaw;
457 }
458 
461 {
462  struct EnuCoor_f *pos = stateGetPositionEnu_f();
463  struct FloatVect2 pos_diff;
464  pos_diff.x = POS_FLOAT_OF_BFP(p->x) - pos->x;
465  pos_diff.y = POS_FLOAT_OF_BFP(p->y) - pos->y;
466  return pos_diff.x * pos_diff.x + pos_diff.y * pos_diff.y;
467 }
468 
471 {
472  return get_dist2_to_point(&waypoints[wp_id].enu_i);
473 }
474 
479 {
482 #ifdef InGeofenceSector
483  struct EnuCoor_f *pos = stateGetPositionEnu_f();
484  too_far_from_home = too_far_from_home || !(InGeofenceSector(pos->x, pos->y));
485 #endif
486 }
487 
489 void nav_set_heading_rad(float rad)
490 {
493 }
494 
496 void nav_set_heading_deg(float deg)
497 {
498  nav_set_heading_rad(RadOfDeg(deg));
499 }
500 
502 void nav_set_heading_towards(float x, float y)
503 {
504  struct FloatVect2 target = {x, y};
505  struct FloatVect2 pos_diff;
507  // don't change heading if closer than 0.5m to target
508  if (VECT2_NORM2(pos_diff) > 0.25) {
509  float heading_f = atan2f(pos_diff.x, pos_diff.y);
510  nav_heading = ANGLE_BFP_OF_REAL(heading_f);
511  }
512 }
513 
516 {
518 }
519 
522 {
525 }
526 
529 {
531 }
532 
534 {
536 }
537 
538 
539 /***********************************************************
540  * built in navigation routines
541  **********************************************************/
542 
543 void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
544 {
545  if (radius == 0) {
546  VECT2_COPY(navigation_target, *wp_center);
547  dist2_to_wp = get_dist2_to_point(wp_center);
548  } else {
549  struct Int32Vect2 pos_diff;
550  VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_center);
551  // go back to half metric precision or values are too large
552  //INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
553  // store last qdr
554  int32_t last_qdr = nav_circle_qdr;
555  // compute qdr
557  // increment circle radians
558  if (nav_circle_radians != 0) {
559  int32_t angle_diff = nav_circle_qdr - last_qdr;
560  INT32_ANGLE_NORMALIZE(angle_diff);
561  nav_circle_radians += angle_diff;
562  } else {
563  // Smallest angle to increment at next step
564  nav_circle_radians = 1;
565  }
566 
567  // direction of rotation
568  int8_t sign_radius = radius > 0 ? 1 : -1;
569  // absolute radius
570  int32_t abs_radius = abs(radius);
571  // carrot_angle
572  int32_t carrot_angle = ((CARROT_DIST << INT32_ANGLE_FRAC) / abs_radius);
573  Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
574  carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
575  int32_t s_carrot, c_carrot;
576  PPRZ_ITRIG_SIN(s_carrot, carrot_angle);
577  PPRZ_ITRIG_COS(c_carrot, carrot_angle);
578  // compute setpoint
579  VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot);
581  VECT2_SUM(navigation_target, *wp_center, pos_diff);
582  }
583  nav_circle_center = *wp_center;
584  nav_circle_radius = radius;
586 }
587 
588 
589 void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
590 {
591  struct Int32Vect2 wp_diff, pos_diff, wp_diff_prec;
592  VECT2_DIFF(wp_diff, *wp_end, *wp_start);
593  VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_start);
594  // go back to metric precision or values are too large
595  VECT2_COPY(wp_diff_prec, wp_diff);
596  INT32_VECT2_RSHIFT(wp_diff, wp_diff, INT32_POS_FRAC);
598  uint32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y), 1);
599  nav_leg_length = int32_sqrt(leg_length2);
600  nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / (int32_t)nav_leg_length;
601  int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
602  nav_leg_progress += progress;
603  int32_t prog_2 = nav_leg_length;
604  Bound(nav_leg_progress, 0, prog_2);
605  struct Int32Vect2 progress_pos;
606  VECT2_SMUL(progress_pos, wp_diff_prec, ((float)nav_leg_progress) / nav_leg_length);
607  VECT2_SUM(navigation_target, *wp_start, progress_pos);
608 
609  nav_segment_start = *wp_start;
610  nav_segment_end = *wp_end;
612 
614 }
615 
616 
617 /************** Oval Navigation **********************************************/
618 
619 #ifndef LINE_START_FUNCTION
620 #define LINE_START_FUNCTION {}
621 #endif
622 #ifndef LINE_STOP_FUNCTION
623 #define LINE_STOP_FUNCTION {}
624 #endif
625 
629 
630 void nav_oval_init(void)
631 {
632  oval_status = OC2;
633  nav_oval_count = 0;
634 }
635 
642 void nav_oval(uint8_t p1, uint8_t p2, float radius)
643 {
644  radius = - radius; /* Historical error ? */
645  int32_t alt = waypoints[p1].enu_i.z;
646  waypoints[p2].enu_i.z = alt;
647 
648  float p2_p1_x = waypoints[p1].enu_f.x - waypoints[p2].enu_f.x;
649  float p2_p1_y = waypoints[p1].enu_f.y - waypoints[p2].enu_f.y;
650  float d = sqrtf(p2_p1_x * p2_p1_x + p2_p1_y * p2_p1_y);
651 
652  /* Unit vector from p1 to p2 */
653  int32_t u_x = POS_BFP_OF_REAL(p2_p1_x / d);
654  int32_t u_y = POS_BFP_OF_REAL(p2_p1_y / d);
655 
656  /* The half circle centers and the other leg */
657  struct EnuCoor_i p1_center = { waypoints[p1].enu_i.x + radius * -u_y,
658  waypoints[p1].enu_i.y + radius * u_x,
659  alt
660  };
661  struct EnuCoor_i p1_out = { waypoints[p1].enu_i.x + 2 * radius * -u_y,
662  waypoints[p1].enu_i.y + 2 * radius * u_x,
663  alt
664  };
665 
666  struct EnuCoor_i p2_in = { waypoints[p2].enu_i.x + 2 * radius * -u_y,
667  waypoints[p2].enu_i.y + 2 * radius * u_x,
668  alt
669  };
670  struct EnuCoor_i p2_center = { waypoints[p2].enu_i.x + radius * -u_y,
671  waypoints[p2].enu_i.y + radius * u_x,
672  alt
673  };
674 
675  int32_t qdr_out_2 = INT32_ANGLE_PI - int32_atan2_2(u_y, u_x);
676  int32_t qdr_out_1 = qdr_out_2 + INT32_ANGLE_PI;
677  if (radius < 0) {
678  qdr_out_2 += INT32_ANGLE_PI;
679  qdr_out_1 += INT32_ANGLE_PI;
680  }
681  int32_t qdr_anticipation = ANGLE_BFP_OF_REAL(radius > 0 ? -15 : 15);
682 
683  switch (oval_status) {
684  case OC1 :
685  nav_circle(&p1_center, POS_BFP_OF_REAL(-radius));
686  if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_1) - qdr_anticipation)) {
687  oval_status = OR12;
688  InitStage();
690  }
691  return;
692 
693  case OR12:
694  nav_route(&p1_out, &p2_in);
695  if (nav_approaching_from(&p2_in, &p1_out, CARROT)) {
696  oval_status = OC2;
697  nav_oval_count++;
698  InitStage();
700  }
701  return;
702 
703  case OC2 :
704  nav_circle(&p2_center, POS_BFP_OF_REAL(-radius));
705  if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_2) - qdr_anticipation)) {
706  oval_status = OR21;
707  InitStage();
709  }
710  return;
711 
712  case OR21:
713  nav_route(&waypoints[p2].enu_i, &waypoints[p1].enu_i);
714  if (nav_approaching_from(&waypoints[p1].enu_i, &waypoints[p2].enu_i, CARROT)) {
715  oval_status = OC1;
716  InitStage();
718  }
719  return;
720 
721  default: /* Should not occur !!! Doing nothing */
722  return;
723  }
724 }
725 /*
726 #ifdef TRAFFIC_INFO
727 #include "modules/multi/traffic_info.h"
728 
729 void nav_follow(uint8_t ac_id, uint32_t distance, uint32_t height)
730 {
731  struct EnuCoor_i* target = acInfoGetPositionEnu_i(ac_id);
732 
733 
734  float alpha = M_PI / 2 - acInfoGetCourse(ac_id);
735  float ca = cosf(alpha), sa = sinf(alpha);
736  target->x += - distance * ca;
737  target->y += - distance * sa;
738  target->z = (Max(target->z + height, SECURITY_HEIGHT)); // todo add ground height to check
739 
740  ENU_OF_TO_NED(navigation_target, *target);
741 }
742 #else*/
743 void nav_follow(uint8_t __attribute__((unused)) _ac_id, uint32_t __attribute__((unused)) distance,
744  uint32_t __attribute__((unused)) height) {}
745 //#endif
int32_atan2
int32_t int32_atan2(int32_t y, int32_t x)
Definition: pprz_trig_int.c:899
speed_sp
struct FloatVect3 speed_sp
Definition: guidance_indi_hybrid.c:134
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:115
Int16Vect3
Definition: pprz_algebra_int.h:62
ins.h
LINE_START_FUNCTION
#define LINE_START_FUNCTION
Definition: navigation.c:620
uint16_t
unsigned short uint16_t
Definition: types.h:16
pprz_debug.h
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:168
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:119
InitStage
#define InitStage()
Definition: common_flight_plan.h:44
OC1
@ OC1
Definition: navigation.c:626
nav_climb_vspeed
float nav_climb_vspeed
Definition: navigation.c:111
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:115
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:86
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:478
ins_reset_altitude_ref
void ins_reset_altitude_ref(void)
INS altitude reference reset.
Definition: ins_mekf_wind_wrapper.c:649
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
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
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:306
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:135
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:262
nav_circle_radians
int32_t nav_circle_radians
Status on the current circle.
Definition: navigation.c:120
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:109
nav_circle
void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
Definition: navigation.c:543
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:502
nav_oval_init
void nav_oval_init(void)
Definition: navigation.c:630
PPRZ_ITRIG_COS
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:110
nav_follow
void nav_follow(uint8_t _ac_id, uint32_t distance, uint32_t height)
Definition: navigation.c:743
VECT3_SDIV
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:196
navigation_target
struct EnuCoor_i navigation_target
Definition: navigation.c:91
uint32_t
unsigned long uint32_t
Definition: types.h:18
UNUSED
uint8_t last_wp UNUSED
Definition: navigation.c:96
nav_roll
int32_t nav_roll
Definition: navigation.c:107
navigation_carrot
struct EnuCoor_i navigation_carrot
Definition: navigation.c:92
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:155
nav_reset_reference
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
Definition: navigation.c:353
nav_stage
uint8_t nav_stage
Definition: common_flight_plan.c:35
oval_status
oval_status
Definition: navigation.c:626
navigation_update_wp_from_speed
void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp)
Definition: navigation.c:389
get_dist2_to_waypoint
float get_dist2_to_waypoint(uint8_t wp_id)
Returns squared horizontal distance to given waypoint.
Definition: navigation.c:470
nav_heading
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:108
nav_radius
float nav_radius
Definition: navigation.c:110
int32_atan2_2
int32_t int32_atan2_2(int32_t y, int32_t x)
Definition: pprz_trig_int.c:921
nav_leg_progress
int32_t nav_leg_progress
Definition: navigation.c:102
nav_advance_carrot
static void UNUSED nav_advance_carrot(void)
Definition: navigation.c:226
nav_cmd_pitch
int32_t nav_cmd_pitch
Definition: navigation.c:109
waypoints
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
ins_reset_local_origin
void ins_reset_local_origin(void)
local implemetation of the ins_reset functions
Definition: ins_mekf_wind_wrapper.c:633
nav_pitch
int32_t nav_pitch
Definition: navigation.c:107
nav_set_heading_current
void nav_set_heading_current(void)
Set heading to the current yaw angle.
Definition: navigation.c:528
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
HORIZONTAL_MODE_MANUAL
#define HORIZONTAL_MODE_MANUAL
Definition: navigation.h:57
pprz_algebra_int.h
Paparazzi fixed point algebra.
nav_set_altitude
static void nav_set_altitude(void)
Definition: navigation.c:342
NAV_FREQ
#define NAV_FREQ
Definition: navigation.h:43
VECT2_DIFF
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
nav_reset_alt
void nav_reset_alt(void)
Definition: navigation.c:360
HORIZONTAL_MODE_WAYPOINT
#define HORIZONTAL_MODE_WAYPOINT
Definition: nav.h:85
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:181
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:257
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:111
telemetry.h
vertical_mode
uint8_t vertical_mode
Definition: navigation.c:113
last_wp
uint8_t last_wp
Index of last waypoint.
Definition: navigation.c:51
LINE_STOP_FUNCTION
#define LINE_STOP_FUNCTION
Definition: navigation.c:623
nav_circle_radius
int32_t nav_circle_radius
Definition: navigation.c:120
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:248
INT32_VECT3_RSHIFT
#define INT32_VECT3_RSHIFT(_o, _i, _r)
Definition: pprz_algebra_int.h:298
horizontal_mode
uint8_t horizontal_mode
Definition: navigation.c:100
DEFAULT_CIRCLE_RADIUS
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition: navigation.c:55
nb_waypoint
const uint8_t nb_waypoint
Definition: common_nav.c:37
CLOSE_TO_WAYPOINT
#define CLOSE_TO_WAYPOINT
Definition: navigation.c:76
nav_last_point
struct EnuCoor_i nav_last_point
Definition: navigation.c:94
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:116
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:460
CARROT_DIST
#define CARROT_DIST
Definition: navigation.c:77
nav_block
uint8_t nav_block
Definition: common_flight_plan.c:35
too_far_from_home
bool too_far_from_home
Definition: navigation.c:87
int16_t
signed short int16_t
Definition: types.h:17
Int32Vect2
Definition: pprz_algebra_int.h:83
Int32Vect3
Definition: pprz_algebra_int.h:88
to_end_vect
struct FloatVect2 line_vect to_end_vect
Definition: navigation.c:81
HORIZONTAL_MODE_CIRCLE
#define HORIZONTAL_MODE_CIRCLE
Definition: nav.h:87
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:589
nav_segment_end
struct EnuCoor_i nav_segment_start nav_segment_end
Definition: navigation.c:123
uint8_t
unsigned char uint8_t
Definition: types.h:14
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:642
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
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:85
nav_cmd_roll
int32_t nav_cmd_roll
Definition: navigation.c:109
VECT2_NORM2
#define VECT2_NORM2(_v)
Definition: pprz_algebra.h:118
force_forward
bool force_forward
Definition: navigation.c:79
autopilot.h
nav_home
void nav_home(void)
Home mode navigation.
Definition: navigation.c:426
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:144
oval_status
oval_status
Definition: nav.h:53
send_nav_status
static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
Definition: navigation.c:152
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
nav_detect_ground
bool nav_detect_ground(void)
Definition: navigation.c:413
set_exception_flag
void set_exception_flag(uint8_t flag_num)
Definition: navigation.c:130
nav_circle_qdr
int32_t nav_circle_qdr
Definition: navigation.c:120
VECT2_ASSIGN
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:62
waypoints_init
void waypoints_init(void)
initialize global and local waypoints
Definition: waypoints.c:35
waypoints_localize_all
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition: waypoints.c:293
FAILSAFE_MODE_DISTANCE
#define FAILSAFE_MODE_DISTANCE
Maximum distance from HOME waypoint before going into failsafe mode.
Definition: navigation.c:73
nav_survey_active
bool nav_survey_active
Definition: navigation.c:105
INT32_VECT2_RSHIFT
#define INT32_VECT2_RSHIFT(_o, _i, _r)
Definition: pprz_algebra_int.h:268
int8_t
signed char int8_t
Definition: types.h:15
nav_periodic_task
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
Definition: navigation.c:374
int32_t
signed long int32_t
Definition: types.h:19
NAV_CLIMB_VSPEED
#define NAV_CLIMB_VSPEED
Definition: navigation.c:59
VERTICAL_MODE_ALT
#define VERTICAL_MODE_ALT
Definition: navigation.h:75
OR12
@ OR12
Definition: navigation.c:626
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:68
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:161
navigation.h
point::y
float y
Definition: common_nav.h:41
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
OR21
@ OR21
Definition: navigation.c:626
nav_set_heading_deg
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition: navigation.c:496
OC2
@ OC2
Definition: navigation.c:626
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:366
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:451
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
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:63
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:515
nav_is_in_flight
bool nav_is_in_flight(void)
Definition: navigation.c:420
max_dist2_from_home
const float max_dist2_from_home
Definition: navigation.c:84
HORIZONTAL_MODE_ROUTE
#define HORIZONTAL_MODE_ROUTE
Definition: nav.h:86
nav_set_heading_rad
void nav_set_heading_rad(float rad)
Set nav_heading in radians.
Definition: navigation.c:489
nav_oval_count
uint8_t nav_oval_count
Navigation along a figure O.
Definition: navigation.c:628
nav_leg_length
uint32_t nav_leg_length
Definition: navigation.c:103
exception_flag
bool exception_flag[10]
Definition: navigation.c:98
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:89
nav_throttle
uint32_t nav_throttle
direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL
Definition: navigation.c:114
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:521
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:115
nav_set_failsafe
void nav_set_failsafe(void)
Definition: navigation.c:533
max_dist_from_home
const float max_dist_from_home
Definition: navigation.c:83
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:109