Paparazzi UAS  v5.15_devel-88-gb3ad7fe
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 const float max_dist_from_home = MAX_DIST_FROM_HOME;
80 const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME;
84 
86 
89 
91 
93 
94 bool exception_flag[10] = {0}; //exception flags that can be used in the flight plan
95 
97 
100 
102 
108 
113 
114 /* nav_circle variables */
117 
118 /* nav_route variables */
119 struct EnuCoor_i nav_segment_start, nav_segment_end;
120 
121 
122 static inline void nav_set_altitude(void);
123 
124 
125 #if PERIODIC_TELEMETRY
127 
129 {
130  exception_flag[flag_num] = 1;
131 }
132 
133 static void send_segment(struct transport_tx *trans, struct link_device *dev)
134 {
135  float sx = POS_FLOAT_OF_BFP(nav_segment_start.x);
136  float sy = POS_FLOAT_OF_BFP(nav_segment_start.y);
137  float ex = POS_FLOAT_OF_BFP(nav_segment_end.x);
138  float ey = POS_FLOAT_OF_BFP(nav_segment_end.y);
139  pprz_msg_send_SEGMENT(trans, dev, AC_ID, &sx, &sy, &ex, &ey);
140 }
141 
142 static void send_circle(struct transport_tx *trans, struct link_device *dev)
143 {
147  pprz_msg_send_CIRCLE(trans, dev, AC_ID, &cx, &cy, &r);
148 }
149 
150 static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
151 {
152  float dist_home = sqrtf(dist2_to_home);
153  float dist_wp = sqrtf(dist2_to_wp);
154  pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID,
156  &dist_home, &dist_wp,
157  &nav_block, &nav_stage,
158  &horizontal_mode);
160  send_segment(trans, dev);
161  } else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) {
162  send_circle(trans, dev);
163  }
164 }
165 
166 static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
167 {
168  static uint8_t i;
169  i++;
170  if (i >= nb_waypoint) { i = 0; }
171  pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID,
172  &i,
173  &(waypoints[i].enu_i.x),
174  &(waypoints[i].enu_i.y),
175  &(waypoints[i].enu_i.z));
176 }
177 #endif
178 
179 void nav_init(void)
180 {
181  waypoints_init();
182 
183  nav_block = 0;
184  nav_stage = 0;
185  nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
187  flight_altitude = SECURITY_ALT;
188  VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i);
189  VECT3_COPY(navigation_carrot, waypoints[WP_HOME].enu_i);
190 
193 
194  nav_roll = 0;
195  nav_pitch = 0;
196  nav_heading = 0;
197  nav_cmd_roll = 0;
198  nav_cmd_pitch = 0;
199  nav_cmd_yaw = 0;
203  nav_throttle = 0;
204  nav_climb = 0;
205  nav_leg_progress = 0;
206  nav_leg_length = 1;
207 
208  too_far_from_home = false;
209  dist2_to_home = 0;
210  dist2_to_wp = 0;
211 
212 #if PERIODIC_TELEMETRY
213  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_NAV_STATUS, send_nav_status);
215 #endif
216 
217  // generated init function
218  auto_nav_init();
219 }
220 
221 static inline void UNUSED nav_advance_carrot(void)
222 {
223  struct EnuCoor_i *pos = stateGetPositionEnu_i();
224  /* compute a vector to the waypoint */
225  struct Int32Vect2 path_to_waypoint;
226  VECT2_DIFF(path_to_waypoint, navigation_target, *pos);
227 
228  /* saturate it */
229  VECT2_STRIM(path_to_waypoint, -(1 << 15), (1 << 15));
230 
231  int32_t dist_to_waypoint = int32_vect2_norm(&path_to_waypoint);
232 
233  if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
235  } else {
236  struct Int32Vect2 path_to_carrot;
237  VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST);
238  VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint);
239  VECT2_SUM(navigation_carrot, path_to_carrot, *pos);
240  }
241 }
242 
243 void nav_run(void)
244 {
245 
246 #if GUIDANCE_H_USE_REF
247  // if GUIDANCE_H_USE_REF, CARROT_DIST is not used
249 #else
251 #endif
252 
254 }
255 
256 
257 bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
258 {
259  float dist_to_point;
260  struct Int32Vect2 diff;
261  struct EnuCoor_i *pos = stateGetPositionEnu_i();
262 
263  /* if an approaching_time is given, estimate diff after approching_time secs */
264  if (approaching_time > 0) {
265  struct Int32Vect2 estimated_pos;
266  struct Int32Vect2 estimated_progress;
267  struct EnuCoor_i *speed = stateGetSpeedEnu_i();
268  VECT2_SMUL(estimated_progress, *speed, approaching_time);
269  INT32_VECT2_RSHIFT(estimated_progress, estimated_progress, (INT32_SPEED_FRAC - INT32_POS_FRAC));
270  VECT2_SUM(estimated_pos, *pos, estimated_progress);
271  VECT2_DIFF(diff, *wp, estimated_pos);
272  }
273  /* else use current position */
274  else {
275  VECT2_DIFF(diff, *wp, *pos);
276  }
277  /* compute distance of estimated/current pos to target wp
278  * POS_FRAC resolution
279  * convert to float to compute the norm without overflow in 32bit
280  */
281  struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
282  dist_to_point = float_vect2_norm(&diff_f);
283 
284  /* return TRUE if we have arrived */
285  if (dist_to_point < ARRIVED_AT_WAYPOINT) {
286  return true;
287  }
288 
289  /* if coming from a valid waypoint */
290  if (from != NULL) {
291  /* return TRUE if normal line at the end of the segment is crossed */
292  struct Int32Vect2 from_diff;
293  VECT2_DIFF(from_diff, *wp, *from);
294  struct FloatVect2 from_diff_f = {POS_FLOAT_OF_BFP(from_diff.x), POS_FLOAT_OF_BFP(from_diff.y)};
295  return (diff_f.x * from_diff_f.x + diff_f.y * from_diff_f.y < 0);
296  }
297 
298  return false;
299 }
300 
301 bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
302 {
303  uint16_t time_at_wp;
304  float dist_to_point;
305  static uint16_t wp_entry_time = 0;
306  static bool wp_reached = false;
307  static struct EnuCoor_i wp_last = { 0, 0, 0 };
308  struct Int32Vect2 diff;
309 
310  if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) {
311  wp_reached = false;
312  wp_last = *wp;
313  }
314 
315  VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
316  struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
317  dist_to_point = float_vect2_norm(&diff_f);
318  if (dist_to_point < ARRIVED_AT_WAYPOINT) {
319  if (!wp_reached) {
320  wp_reached = true;
321  wp_entry_time = autopilot.flight_time;
322  time_at_wp = 0;
323  } else {
324  time_at_wp = autopilot.flight_time - wp_entry_time;
325  }
326  } else {
327  time_at_wp = 0;
328  wp_reached = false;
329  }
330  if (time_at_wp > stay_time) {
331  INT_VECT3_ZERO(wp_last);
332  return true;
333  }
334  return false;
335 }
336 
337 static inline void nav_set_altitude(void)
338 {
339  static int32_t last_nav_alt = 0;
340  if (abs(nav_altitude - last_nav_alt) > (POS_BFP_OF_REAL(0.2))) {
342  last_nav_alt = nav_altitude;
343  }
344 }
345 
346 
349 {
351  /* update local ENU coordinates of global waypoints */
353 }
354 
355 void nav_reset_alt(void)
356 {
359 }
360 
361 void nav_init_stage(void)
362 {
364  stage_time = 0;
365  nav_circle_radians = 0;
366 }
367 
368 #include <stdio.h>
370 {
371  RunOnceEvery(NAV_FREQ, { stage_time++; block_time++; });
372 
373  nav_survey_active = false;
374 
375  dist2_to_wp = 0;
376 
377  /* from flight_plan.h */
378  auto_nav();
379 
380  /* run carrot loop */
381  nav_run();
382 }
383 
384 void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp)
385 {
386  // MY_ASSERT(wp < nb_waypoint); FIXME
387  int32_t s_heading, c_heading;
388  PPRZ_ITRIG_SIN(s_heading, nav_heading);
389  PPRZ_ITRIG_COS(c_heading, nav_heading);
390  // FIXME : scale POS to SPEED
391  struct Int32Vect3 delta_pos;
392  VECT3_SDIV(delta_pos, speed_sp, NAV_FREQ); /* fixme :make sure the division is really a >> */
393  INT32_VECT3_RSHIFT(delta_pos, delta_pos, (INT32_SPEED_FRAC - INT32_POS_FRAC));
394  waypoints[wp].enu_i.x += (s_heading * delta_pos.x + c_heading * delta_pos.y) >> INT32_TRIG_FRAC;
395  waypoints[wp].enu_i.y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >> INT32_TRIG_FRAC;
396  waypoints[wp].enu_i.z += delta_pos.z;
397  int32_t delta_heading = heading_rate_sp / NAV_FREQ;
398  delta_heading = delta_heading >> (INT32_SPEED_FRAC - INT32_POS_FRAC);
399  nav_heading += delta_heading;
400 
401  INT32_COURSE_NORMALIZE(nav_heading);
402  RunOnceEvery(10, DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp,
403  &(waypoints[wp].enu_i.x),
404  &(waypoints[wp].enu_i.y),
405  &(waypoints[wp].enu_i.z)));
406 }
407 
409 {
410  if (!autopilot.ground_detected) { return false; }
411  autopilot.ground_detected = false;
412  return true;
413 }
414 
416 {
417  return autopilot_in_flight();
418 }
419 
421 void nav_home(void)
422 {
424  VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i);
425 
427  nav_altitude = waypoints[WP_HOME].enu_i.z;
429 
431 
432  /* run carrot loop */
433  nav_run();
434 }
435 
446 void nav_set_manual(int32_t roll, int32_t pitch, int32_t yaw)
447 {
449  nav_cmd_roll = roll;
450  nav_cmd_pitch = pitch;
451  nav_cmd_yaw = yaw;
452 }
453 
456 {
457  struct EnuCoor_f *pos = stateGetPositionEnu_f();
458  struct FloatVect2 pos_diff;
459  pos_diff.x = POS_FLOAT_OF_BFP(p->x) - pos->x;
460  pos_diff.y = POS_FLOAT_OF_BFP(p->y) - pos->y;
461  return pos_diff.x * pos_diff.x + pos_diff.y * pos_diff.y;
462 }
463 
466 {
467  return get_dist2_to_point(&waypoints[wp_id].enu_i);
468 }
469 
474 {
477 #ifdef InGeofenceSector
478  struct EnuCoor_f *pos = stateGetPositionEnu_f();
479  too_far_from_home = too_far_from_home || !(InGeofenceSector(pos->x, pos->y));
480 #endif
481 }
482 
484 void nav_set_heading_rad(float rad)
485 {
488 }
489 
491 void nav_set_heading_deg(float deg)
492 {
493  nav_set_heading_rad(RadOfDeg(deg));
494 }
495 
497 void nav_set_heading_towards(float x, float y)
498 {
499  struct FloatVect2 target = {x, y};
500  struct FloatVect2 pos_diff;
501  VECT2_DIFF(pos_diff, target, *stateGetPositionEnu_f());
502  // don't change heading if closer than 0.5m to target
503  if (VECT2_NORM2(pos_diff) > 0.25) {
504  float heading_f = atan2f(pos_diff.x, pos_diff.y);
505  nav_heading = ANGLE_BFP_OF_REAL(heading_f);
506  }
507 }
508 
511 {
513 }
514 
517 {
520 }
521 
524 {
526 }
527 
529 {
531 }
532 
533 
534 /***********************************************************
535  * built in navigation routines
536  **********************************************************/
537 
538 void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
539 {
540  if (radius == 0) {
541  VECT2_COPY(navigation_target, *wp_center);
542  dist2_to_wp = get_dist2_to_point(wp_center);
543  } else {
544  struct Int32Vect2 pos_diff;
545  VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_center);
546  // go back to half metric precision or values are too large
547  //INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
548  // store last qdr
549  int32_t last_qdr = nav_circle_qdr;
550  // compute qdr
551  nav_circle_qdr = int32_atan2(pos_diff.y, pos_diff.x);
552  // increment circle radians
553  if (nav_circle_radians != 0) {
554  int32_t angle_diff = nav_circle_qdr - last_qdr;
555  INT32_ANGLE_NORMALIZE(angle_diff);
556  nav_circle_radians += angle_diff;
557  } else {
558  // Smallest angle to increment at next step
559  nav_circle_radians = 1;
560  }
561 
562  // direction of rotation
563  int8_t sign_radius = radius > 0 ? 1 : -1;
564  // absolute radius
565  int32_t abs_radius = abs(radius);
566  // carrot_angle
567  int32_t carrot_angle = ((CARROT_DIST << INT32_ANGLE_FRAC) / abs_radius);
568  Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
569  carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
570  int32_t s_carrot, c_carrot;
571  PPRZ_ITRIG_SIN(s_carrot, carrot_angle);
572  PPRZ_ITRIG_COS(c_carrot, carrot_angle);
573  // compute setpoint
574  VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot);
575  INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_TRIG_FRAC);
576  VECT2_SUM(navigation_target, *wp_center, pos_diff);
577  }
578  nav_circle_center = *wp_center;
579  nav_circle_radius = radius;
581 }
582 
583 
584 void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
585 {
586  struct Int32Vect2 wp_diff, pos_diff, wp_diff_prec;
587  VECT2_DIFF(wp_diff, *wp_end, *wp_start);
588  VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_start);
589  // go back to metric precision or values are too large
590  VECT2_COPY(wp_diff_prec, wp_diff);
591  INT32_VECT2_RSHIFT(wp_diff, wp_diff, INT32_POS_FRAC);
592  INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_POS_FRAC);
593  uint32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y), 1);
594  nav_leg_length = int32_sqrt(leg_length2);
595  nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_leg_length;
596  int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
597  nav_leg_progress += progress;
598  int32_t prog_2 = nav_leg_length;
599  Bound(nav_leg_progress, 0, prog_2);
600  struct Int32Vect2 progress_pos;
601  VECT2_SMUL(progress_pos, wp_diff_prec, ((float)nav_leg_progress) / nav_leg_length);
602  VECT2_SUM(navigation_target, *wp_start, progress_pos);
603 
604  nav_segment_start = *wp_start;
605  nav_segment_end = *wp_end;
607 
609 }
610 
611 
612 /************** Oval Navigation **********************************************/
613 
614 #ifndef LINE_START_FUNCTION
615 #define LINE_START_FUNCTION {}
616 #endif
617 #ifndef LINE_STOP_FUNCTION
618 #define LINE_STOP_FUNCTION {}
619 #endif
620 
624 
625 void nav_oval_init(void)
626 {
627  oval_status = OC2;
628  nav_oval_count = 0;
629 }
630 
637 void nav_oval(uint8_t p1, uint8_t p2, float radius)
638 {
639  radius = - radius; /* Historical error ? */
640  int32_t alt = waypoints[p1].enu_i.z;
641  waypoints[p2].enu_i.z = alt;
642 
643  float p2_p1_x = waypoints[p1].enu_f.x - waypoints[p2].enu_f.x;
644  float p2_p1_y = waypoints[p1].enu_f.y - waypoints[p2].enu_f.y;
645  float d = sqrtf(p2_p1_x * p2_p1_x + p2_p1_y * p2_p1_y);
646 
647  /* Unit vector from p1 to p2 */
648  int32_t u_x = POS_BFP_OF_REAL(p2_p1_x / d);
649  int32_t u_y = POS_BFP_OF_REAL(p2_p1_y / d);
650 
651  /* The half circle centers and the other leg */
652  struct EnuCoor_i p1_center = { waypoints[p1].enu_i.x + radius * -u_y,
653  waypoints[p1].enu_i.y + radius * u_x,
654  alt
655  };
656  struct EnuCoor_i p1_out = { waypoints[p1].enu_i.x + 2 * radius * -u_y,
657  waypoints[p1].enu_i.y + 2 * radius * u_x,
658  alt
659  };
660 
661  struct EnuCoor_i p2_in = { waypoints[p2].enu_i.x + 2 * radius * -u_y,
662  waypoints[p2].enu_i.y + 2 * radius * u_x,
663  alt
664  };
665  struct EnuCoor_i p2_center = { waypoints[p2].enu_i.x + radius * -u_y,
666  waypoints[p2].enu_i.y + radius * u_x,
667  alt
668  };
669 
670  int32_t qdr_out_2 = INT32_ANGLE_PI - int32_atan2_2(u_y, u_x);
671  int32_t qdr_out_1 = qdr_out_2 + INT32_ANGLE_PI;
672  if (radius < 0) {
673  qdr_out_2 += INT32_ANGLE_PI;
674  qdr_out_1 += INT32_ANGLE_PI;
675  }
676  int32_t qdr_anticipation = ANGLE_BFP_OF_REAL(radius > 0 ? -15 : 15);
677 
678  switch (oval_status) {
679  case OC1 :
680  nav_circle(&p1_center, POS_BFP_OF_REAL(-radius));
681  if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_1) - qdr_anticipation)) {
682  oval_status = OR12;
683  InitStage();
685  }
686  return;
687 
688  case OR12:
689  nav_route(&p1_out, &p2_in);
690  if (nav_approaching_from(&p2_in, &p1_out, CARROT)) {
691  oval_status = OC2;
692  nav_oval_count++;
693  InitStage();
695  }
696  return;
697 
698  case OC2 :
699  nav_circle(&p2_center, POS_BFP_OF_REAL(-radius));
700  if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_2) - qdr_anticipation)) {
701  oval_status = OR21;
702  InitStage();
704  }
705  return;
706 
707  case OR21:
708  nav_route(&waypoints[p2].enu_i, &waypoints[p1].enu_i);
709  if (nav_approaching_from(&waypoints[p1].enu_i, &waypoints[p2].enu_i, CARROT)) {
710  oval_status = OC1;
711  InitStage();
713  }
714  return;
715 
716  default: /* Should not occur !!! Doing nothing */
717  return;
718  }
719 }
720 /*
721 #ifdef TRAFFIC_INFO
722 #include "modules/multi/traffic_info.h"
723 
724 void nav_follow(uint8_t ac_id, uint32_t distance, uint32_t height)
725 {
726  struct EnuCoor_i* target = acInfoGetPositionEnu_i(ac_id);
727 
728 
729  float alpha = M_PI / 2 - acInfoGetCourse(ac_id);
730  float ca = cosf(alpha), sa = sinf(alpha);
731  target->x += - distance * ca;
732  target->y += - distance * sa;
733  target->z = (Max(target->z + height, SECURITY_HEIGHT)); // todo add ground height to check
734 
735  ENU_OF_TO_NED(navigation_target, *target);
736 }
737 #else*/
738 void nav_follow(uint8_t __attribute__((unused)) _ac_id, uint32_t __attribute__((unused)) distance,
739  uint32_t __attribute__((unused)) height) {}
740 //#endif
void nav_set_manual(int32_t roll, int32_t pitch, int32_t yaw)
Set manual roll, pitch and yaw without stabilization.
Definition: navigation.c:446
const float max_dist2_from_home
Definition: navigation.c:80
int32_t psi
in rad with INT32_ANGLE_FRAC
struct EnuCoor_i navigation_target
Definition: navigation.c:87
uint16_t block_time
unsigned short uint16_t
Definition: types.h:16
struct EnuCoor_i nav_last_point
Definition: navigation.c:90
float nav_climb_vspeed
Definition: navigation.c:107
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int32_t nav_cmd_pitch
Definition: navigation.c:105
float x
Definition: common_nav.h:40
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
Definition: navigation.c:348
int32_t y
North.
int32_t x
East.
#define FAILSAFE_MODE_DISTANCE
Maximum distance from HOME waypoint before going into failsafe mode.
Definition: navigation.c:73
void nav_oval_init(void)
Definition: navigation.c:625
#define INT32_SPEED_FRAC
#define INT32_DEG_OF_RAD(_rad)
float nav_descend_vspeed
Definition: navigation.c:107
Periodic telemetry system header (includes downlink utility and generated code).
int32_t nav_cmd_roll
Definition: navigation.c:105
void ins_reset_local_origin(void)
local implemetation of the ins_reset functions
void set_exception_flag(uint8_t flag_num)
Definition: navigation.c:128
int32_t nav_climb
Definition: navigation.c:111
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:86
#define INT32_ANGLE_FRAC
#define HORIZONTAL_MODE_MANUAL
Definition: navigation.h:57
#define VECT2_STRIM(_v, _min, _max)
Definition: pprz_algebra.h:110
vector in East North Up coordinates Units: meters
#define POS_BFP_OF_REAL(_af)
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
uint8_t last_wp UNUSED
Definition: navigation.c:92
int32_t nav_flight_altitude
Definition: navigation.c:111
float flight_altitude
Dynamically adjustable, reset to nav_altitude when it is changing.
Definition: navigation.c:112
uint16_t flight_time
flight time in seconds
Definition: autopilot.h:65
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
static void send_circle(struct transport_tx *trans, struct link_device *dev)
Definition: navigation.c:142
void nav_set_heading_towards_target(void)
Set heading in the direction of the target.
Definition: navigation.c:516
float nav_radius
Definition: navigation.c:106
#define InitStage()
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
uint8_t nav_block
bool nav_survey_active
Definition: navigation.c:101
Integrated Navigation System interface.
void nav_home(void)
Home mode navigation.
Definition: navigation.c:421
#define INT32_VECT3_RSHIFT(_o, _i, _r)
int32_t nav_altitude
Definition: navigation.c:111
#define DEFAULT_CIRCLE_RADIUS
default nav_circle_radius in meters
Definition: navigation.c:55
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:62
#define CLOSE_TO_WAYPOINT
Definition: navigation.c:76
uint8_t nav_oval_count
Navigation along a figure O.
Definition: navigation.c:623
void nav_set_heading_current(void)
Set heading to the current yaw angle.
Definition: navigation.c:523
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:161
float dist2_to_wp
squared distance to next waypoint
Definition: navigation.c:85
static uint32_t int32_vect2_norm(struct Int32Vect2 *v)
return norm of 2D vector
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
const float max_dist_from_home
Definition: navigation.c:79
struct EnuCoor_i nav_segment_start nav_segment_end
Definition: navigation.c:119
float dist2_to_home
squared distance to home waypoint
Definition: navigation.c:82
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
Definition: navigation.c:369
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
int32_t nav_circle_qdr
Definition: navigation.c:116
bool too_far_from_home
Definition: navigation.c:83
#define PPRZ_ITRIG_SIN(_s, _a)
float y
Definition: common_nav.h:41
void nav_run(void)
Definition: navigation.c:243
#define WaypointX(_wp)
Definition: common_nav.h:45
static void nav_set_altitude(void)
Definition: navigation.c:337
static float float_vect2_norm(struct FloatVect2 *v)
float x
in meters
bool exception_flag[10]
Definition: navigation.c:94
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:257
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:104
uint8_t last_wp
Index of last waypoint.
Definition: navigation.c:51
int32_t nav_pitch
with INT32_ANGLE_FRAC
Definition: navigation.c:103
Device independent GPS code (interface)
struct EnuCoor_i nav_circle_center
Definition: navigation.c:115
#define INT32_ANGLE_NORMALIZE(_a)
unsigned long uint32_t
Definition: types.h:18
float get_dist2_to_point(struct EnuCoor_i *p)
Returns squared horizontal distance to given point.
Definition: navigation.c:455
bool nav_is_in_flight(void)
Definition: navigation.c:415
signed short int16_t
Definition: types.h:17
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:196
uint32_t int32_sqrt(uint32_t in)
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
void nav_set_heading_rad(float rad)
Set nav_heading in radians.
Definition: navigation.c:484
#define VECT2_NORM2(_v)
Definition: pprz_algebra.h:118
uint32_t nav_leg_length
Definition: navigation.c:99
bool nav_detect_ground(void)
Definition: navigation.c:408
#define INT32_POS_FRAC
float get_dist2_to_waypoint(uint8_t wp_id)
Returns squared horizontal distance to given waypoint.
Definition: navigation.c:465
static void send_segment(struct transport_tx *trans, struct link_device *dev)
Definition: navigation.c:133
uint8_t vertical_mode
Definition: navigation.c:109
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition: navigation.c:473
#define Max(x, y)
Definition: main_fbw.c:53
uint16_t stage_time
In s.
#define WaypointY(_wp)
Definition: common_nav.h:46
int32_t nav_leg_progress
Definition: navigation.c:98
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:98
const uint8_t nb_waypoint
Definition: common_nav.c:37
signed long int32_t
Definition: types.h:19
struct EnuCoor_i navigation_carrot
Definition: navigation.c:88
static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
Definition: navigation.c:166
#define ANGLE_BFP_OF_REAL(_af)
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
uint8_t nav_stage
#define INT32_TRIG_FRAC
Core autopilot interface common to all firmwares.
#define VECT2_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:104
Rotorcraft navigation functions.
static void UNUSED nav_advance_carrot(void)
Definition: navigation.c:221
#define INT_VECT3_ZERO(_v)
#define INT32_ANGLE_PI_4
#define ARRIVED_AT_WAYPOINT
minimum horizontal distance to waypoint to mark as arrived
Definition: navigation.c:68
vector in East North Up coordinates
unsigned char uint8_t
Definition: types.h:14
void nav_oval(uint8_t p1, uint8_t p2, float radius)
Navigation along a figure O.
Definition: navigation.c:637
API to get/set the generic vehicle states.
int32_t nav_circle_radius
Definition: navigation.c:116
oval_status
Definition: navigation.c:621
int32_t int32_atan2_2(int32_t y, int32_t x)
#define CARROT_DIST
Definition: navigation.c:77
#define LINE_STOP_FUNCTION
Definition: navigation.c:618
#define NAV_DESCEND_VSPEED
Definition: navigation.c:63
static struct EnuCoor_i * stateGetSpeedEnu_i(void)
Get ground speed in local ENU coordinates (int).
Definition: state.h:872
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp)
Definition: navigation.c:384
#define INT32_COURSE_NORMALIZE(_a)
void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
Definition: navigation.c:538
void nav_set_heading_deg(float deg)
Set nav_heading in degrees.
Definition: navigation.c:491
uint8_t horizontal_mode
Definition: navigation.c:96
#define INT32_ANGLE_PI
#define VERTICAL_MODE_ALT
Definition: navigation.h:75
int32_t nav_roll
Definition: navigation.c:103
static float p[2][2]
void nav_set_heading_towards_waypoint(uint8_t wp)
Set heading in the direction of a waypoint.
Definition: navigation.c:510
uint32_t nav_throttle
direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL
Definition: navigation.c:110
int32_t nav_circle_radians
Status on the current circle.
Definition: navigation.c:116
#define POS_FLOAT_OF_BFP(_ai)
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
Definition: navigation.c:584
float failsafe_mode_dist2
maximum squared distance to home wp before going to failsafe mode
Definition: navigation.c:81
int32_t nav_cmd_yaw
Definition: navigation.c:105
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:674
#define LINE_START_FUNCTION
Definition: navigation.c:615
Horizontal guidance for rotorcrafts.
#define INT32_VECT2_RSHIFT(_o, _i, _r)
signed char int8_t
Definition: types.h:15
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition: waypoints.c:293
bool ground_detected
automatic detection of landing
Definition: autopilot.h:74
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
#define AP_MODE_FAILSAFE
void nav_init(void)
Navigation Initialisation.
Definition: navigation.c:179
void nav_set_failsafe(void)
Definition: navigation.c:528
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:301
void ins_reset_altitude_ref(void)
INS altitude reference reset.
void nav_set_heading_towards(float x, float y)
Set heading to point towards x,y position in local coordinates.
Definition: navigation.c:497
void nav_reset_alt(void)
Definition: navigation.c:355
float y
in meters
#define NAV_CLIMB_VSPEED
Definition: navigation.c:59
#define NAV_FREQ
Definition: navigation.h:43
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:257
void waypoints_init(void)
initialize global and local waypoints
Definition: waypoints.c:35
static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
Definition: navigation.c:150
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
void nav_follow(uint8_t _ac_id, uint32_t distance, uint32_t height)
Definition: navigation.c:738
#define PPRZ_ITRIG_COS(_c, _a)
int32_t int32_atan2(int32_t y, int32_t x)
Paparazzi fixed point algebra.
#define CARROT
default approaching_time for a wp
Definition: navigation.h:40
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition: navigation.c:361