Paparazzi UAS  v6.1.0_stable
Paparazzi is a free software Unmanned Aircraft System.
nav.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2003-2005 Pascal Brisset, Antoine Drouin
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 
28 #include <math.h>
29 #include "std.h"
30 
31 static unit_t unit __attribute__((unused));
32 
33 #define NAV_C
36 #include "autopilot.h"
38 #include "modules/gps/gps.h"
39 
40 #include "generated/flight_plan.h"
41 
42 
44 
45 float last_x, last_y;
46 
48 uint8_t last_wp __attribute__((unused));
49 
50 float rc_pitch;
52 
54 float nav_circle_radians; /* Cumulated */
55 float nav_circle_radians_no_rewind; /* Cumulated */
56 float nav_circle_trigo_qdr; /* Angle from center to mobile */
58 
59 
61 static float nav_leg_progress;
63 
65 static float nav_leg_length;
66 
67 bool nav_in_circle = false;
68 bool nav_in_segment = false;
72 float circle_bank = 0;
73 
76 
78 #ifndef NAV_GLIDE_PITCH_TRIM
79 #define NAV_GLIDE_PITCH_TRIM 0.
80 #endif
81 
82 
83 
85 
86 /* Used in nav_survey_rectangle. Defined here for downlink and uplink */
90 
92 
93 void nav_init_stage(void)
94 {
97  stage_time = 0;
100  nav_in_circle = false;
101  nav_in_segment = false;
102  nav_shift = 0;
103 }
104 
105 #define MIN_DX ((int16_t)(MAX_PPRZ * 0.05))
106 
107 
109 void nav_circle_XY(float x, float y, float radius)
110 {
111  struct EnuCoor_f *pos = stateGetPositionEnu_f();
112  float last_trigo_qdr = nav_circle_trigo_qdr;
113  nav_circle_trigo_qdr = atan2f(pos->y - y, pos->x - x);
114  float sign_radius = radius > 0 ? 1 : -1;
115 
116  if (nav_in_circle) {
117  float trigo_diff = nav_circle_trigo_qdr - last_trigo_qdr;
118  NormRadAngle(trigo_diff);
119  nav_circle_radians += trigo_diff;
120  trigo_diff *= - sign_radius;
121  if (trigo_diff > 0) { // do not rewind if the change in angle is in the opposite sense than nav_radius
122  nav_circle_radians_no_rewind += trigo_diff;
123  }
124  }
125 
126  float dist2_center = DistanceSquare(pos->x, pos->y, x, y);
127  float dist_carrot = CARROT * NOMINAL_AIRSPEED;
128 
129  radius += -nav_shift;
130 
131  float abs_radius = fabs(radius);
132 
134  circle_bank =
135  (dist2_center > Square(abs_radius + dist_carrot)
136  || dist2_center < Square(abs_radius - dist_carrot)) ?
137  0 :
139 
140  float carrot_angle = dist_carrot / abs_radius;
141  carrot_angle = Min(carrot_angle, M_PI / 4);
142  carrot_angle = Max(carrot_angle, M_PI / 16);
143  float alpha_carrot = nav_circle_trigo_qdr - sign_radius * carrot_angle;
145  float radius_carrot = abs_radius;
146  if (nav_mode == NAV_MODE_COURSE) {
147  radius_carrot += (abs_radius / cosf(carrot_angle) - abs_radius);
148  }
149  fly_to_xy(x + cosf(alpha_carrot)*radius_carrot,
150  y + sinf(alpha_carrot)*radius_carrot);
151  nav_in_circle = true;
152  nav_in_segment = false;
153  nav_circle_x = x;
154  nav_circle_y = y;
155  nav_circle_radius = radius;
156 }
157 
158 
159 void nav_glide(uint8_t start_wp, uint8_t wp)
160 {
161  float start_alt = waypoints[start_wp].a;
162  float diff_alt = waypoints[wp].a - start_alt;
163  float alt = start_alt + nav_leg_progress * diff_alt;
164  float pre_climb = stateGetHorizontalSpeedNorm_f() * diff_alt / nav_leg_length;
165  NavVerticalAltitudeMode(alt, pre_climb);
166 }
167 
168 
169 #define MAX_DIST_CARROT 250.
170 #define MIN_HEIGHT_CARROT 50.
171 #define MAX_HEIGHT_CARROT 150.
172 
173 #define Goto3D(radius) { \
174  if (autopilot_get_mode() == AP_MODE_AUTO2) { \
175  int16_t yaw = imcu_get_radio(RADIO_YAW); \
176  if (yaw > MIN_DX || yaw < -MIN_DX) { \
177  carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.); \
178  carrot_x = Min(carrot_x, MAX_DIST_CARROT); \
179  carrot_x = Max(carrot_x, -MAX_DIST_CARROT); \
180  } \
181  int16_t pitch = imcu_get_radio(RADIO_PITCH); \
182  if (pitch > MIN_DX || pitch < -MIN_DX) { \
183  carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); \
184  carrot_y = Min(carrot_y, MAX_DIST_CARROT); \
185  carrot_y = Max(carrot_y, -MAX_DIST_CARROT); \
186  } \
187  v_ctl_mode = V_CTL_MODE_AUTO_ALT; \
188  int16_t roll = imcu_get_radio(RADIO_ROLL); \
189  if (roll > MIN_DX || roll < -MIN_DX) { \
190  nav_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); \
191  nav_altitude = Max(nav_altitude, MIN_HEIGHT_CARROT+ground_alt); \
192  nav_altitude = Min(nav_altitude, MAX_HEIGHT_CARROT+ground_alt); \
193  } \
194  } \
195  nav_circle_XY(carrot_x, carrot_y, radius); \
196  }
197 
198 
199 
200 #ifdef NAV_GROUND_SPEED_PGAIN
201 
203 static void nav_ground_speed_loop(void)
204 {
205  if (MINIMUM_AIRSPEED < nav_ground_speed_setpoint
206  && nav_ground_speed_setpoint < MAXIMUM_AIRSPEED) {
211  } else {
212  /* Reset cruise throttle to nominal value */
213  v_ctl_auto_throttle_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
214  }
215 }
216 #endif
217 
219 void nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius)
220 {
221  nav_radius = radius;
222 
223  float x_0 = waypoints[wp_td].x - waypoints[wp_af].x;
224  float y_0 = waypoints[wp_td].y - waypoints[wp_af].y;
225 
226  /* Unit vector from AF to TD */
227  float d = sqrtf(x_0 * x_0 + y_0 * y_0);
228  float x_1 = x_0 / d;
229  float y_1 = y_0 / d;
230 
231  waypoints[wp_baseleg].x = waypoints[wp_af].x + y_1 * nav_radius;
232  waypoints[wp_baseleg].y = waypoints[wp_af].y - x_1 * nav_radius;
233  waypoints[wp_baseleg].a = waypoints[wp_af].a;
234  baseleg_out_qdr = M_PI - atan2f(-y_1, -x_1);
235  if (nav_radius < 0) {
236  baseleg_out_qdr += M_PI;
237  }
238 }
239 
240 void nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide)
241 {
242 
243  float x_0 = waypoints[wp_td].x - waypoints[wp_af].x;
244  float y_0 = waypoints[wp_td].y - waypoints[wp_af].y;
245  float h_0 = waypoints[wp_td].a - waypoints[wp_af].a;
246 
247  /* Unit vector from AF to TD */
248  float d = sqrtf(x_0 * x_0 + y_0 * y_0);
249  float x_1 = x_0 / d;
250  float y_1 = y_0 / d;
251 
252  waypoints[wp_af].x = waypoints[wp_td].x + x_1 * h_0 * glide;
253  waypoints[wp_af].y = waypoints[wp_td].y + y_1 * h_0 * glide;
254 }
255 
256 
257 /* For a landing UPWIND.
258  Computes Top Of Descent waypoint from Touch Down and Approach Fix
259  waypoints, using glide airspeed, glide vertical speed and wind */
260 static inline void compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed)
261 {
262  struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
263  float td_af_x = WaypointX(_af) - WaypointX(_td);
264  float td_af_y = WaypointY(_af) - WaypointY(_td);
265  float td_af = sqrtf(td_af_x * td_af_x + td_af_y * td_af_y);
266  float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrtf(
267  wind->x * wind->x + wind->y * wind->y));
268  WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
269  WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
270  WaypointAlt(_tod) = WaypointAlt(_af);
271 }
272 
273 
274 #ifndef LINE_START_FUNCTION
275 #define LINE_START_FUNCTION {}
276 #endif
277 #ifndef LINE_STOP_FUNCTION
278 #define LINE_STOP_FUNCTION {}
279 #endif
280 
281 #ifdef TRAFFIC_INFO
283 
284 void nav_follow(uint8_t ac_id, float distance, float height)
285 {
286  struct EnuCoor_f *ac = acInfoGetPositionEnu_f(ac_id);
288  NavVerticalAltitudeMode(Max(ac->z + height, ground_alt + SECURITY_HEIGHT), 0.);
289  float alpha = M_PI / 2 - acInfoGetCourse(ac_id);
290  float ca = cosf(alpha), sa = sinf(alpha);
291  float x = ac->x - distance * ca;
292  float y = ac->y - distance * sa;
293  fly_to_xy(x, y);
294 #ifdef NAV_FOLLOW_PGAIN
295  float s = (stateGetPositionEnu_f()->x - x) * ca + (stateGetPositionEnu_f()->y - y) * sa;
296  nav_ground_speed_setpoint = acInfoGetGspeed(ac_id) + NAV_FOLLOW_PGAIN * s;
297  nav_ground_speed_loop();
298 #endif
299 }
300 #else
301 void nav_follow(uint8_t __attribute__((unused)) _ac_id, float __attribute__((unused)) distance,
302  float __attribute__((unused)) height) {}
303 #endif // TRAFFIC_INFO
304 
305 
306 float nav_altitude = GROUND_ALT + MIN_HEIGHT_CARROT;
309 float nav_pitch; /* Rad */
310 float fp_pitch; /* deg */
311 float fp_throttle; /* [0-1] */
312 float fp_climb; /* m/s */
313 
314 
324 bool nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time)
325 {
327  float pw_x = x - stateGetPositionEnu_f()->x;
329  float pw_y = y - stateGetPositionEnu_f()->y;
330 
331  if (approaching_time < 0.) {
332  // fly after the destination waypoint
333  float leg_x = x - from_x;
334  float leg_y = y - from_y;
335  float leg = sqrtf(Max(leg_x * leg_x + leg_y * leg_y, 1.));
336  float exceed_dist = approaching_time * stateGetHorizontalSpeedNorm_f(); // negative value
337  float scal_prod = (leg_x * pw_x + leg_y * pw_y) / leg;
338  return (scal_prod < exceed_dist);
339  } else {
340  // fly close enough of the waypoint or cross it
341  dist2_to_wp = pw_x * pw_x + pw_y * pw_y;
342  float min_dist = approaching_time * stateGetHorizontalSpeedNorm_f();
343  if (dist2_to_wp < min_dist * min_dist) {
344  return true;
345  }
346  float scal_prod = (x - from_x) * pw_x + (y - from_y) * pw_y;
347  return (scal_prod < 0.);
348  }
349 }
350 
354 //static inline void fly_to_xy(float x, float y) {
355 void fly_to_xy(float x, float y)
356 {
357  struct EnuCoor_f *pos = stateGetPositionEnu_f();
358  desired_x = x;
359  desired_y = y;
360  if (nav_mode == NAV_MODE_COURSE) {
361  h_ctl_course_setpoint = atan2f(x - pos->x, y - pos->y);
362  if (h_ctl_course_setpoint < 0.) {
363  h_ctl_course_setpoint += 2 * M_PI;
364  }
366  } else {
367  float diff = atan2f(x - pos->x, y - pos->y) - stateGetHorizontalSpeedDir_f();
368  NormRadAngle(diff);
369  BoundAbs(diff, M_PI / 2.);
370  float s = sinf(diff);
371  float speed = stateGetHorizontalSpeedNorm_f();
372  h_ctl_roll_setpoint = atanf(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81));
375  }
376 }
377 
381 void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y)
382 {
383  float leg_x = wp_x - last_wp_x;
384  float leg_y = wp_y - last_wp_y;
385  float leg2 = Max(leg_x * leg_x + leg_y * leg_y, 1.);
386  nav_leg_progress = ((stateGetPositionEnu_f()->x - last_wp_x) * leg_x + (stateGetPositionEnu_f()->y - last_wp_y) *
387  leg_y) / leg2;
388  nav_leg_length = sqrtf(leg2);
389 
391  float carrot = CARROT * NOMINAL_AIRSPEED;
392 
394  nav_in_circle = false;
395  nav_in_segment = true;
396  nav_segment_x_1 = last_wp_x;
397  nav_segment_y_1 = last_wp_y;
398  nav_segment_x_2 = wp_x;
399  nav_segment_y_2 = wp_y;
401 
402  fly_to_xy(last_wp_x + nav_carrot_leg_progress * leg_x + nav_shift * leg_y / nav_leg_length,
403  last_wp_y + nav_carrot_leg_progress * leg_y - nav_shift * leg_x / nav_leg_length);
404 }
405 
406 #include "modules/nav/common_nav.c"
407 
408 #ifndef FAILSAFE_HOME_RADIUS
409 #define FAILSAFE_HOME_RADIUS DEFAULT_CIRCLE_RADIUS
410 #endif
411 
412 static void nav_set_altitude(void)
413 {
414  static float last_nav_altitude;
415  if (fabs(nav_altitude - last_nav_altitude) > 1.) {
417  last_nav_altitude = nav_altitude;
418  }
420 }
421 
423 void nav_home(void)
424 {
427  nav_pitch = 0.;
428  if (autopilot.launch) {
430  } else {
433  }
434  nav_altitude = ground_alt + HOME_MODE_HEIGHT;
438 }
439 
445 {
446  nav_survey_active = false;
447 
449  dist2_to_wp = 0.;
450 
451  auto_nav(); /* From flight_plan.h */
452 
454 
455 #ifdef AGR_CLIMB
458  }
459 #endif
460 
462 }
463 
467 #if PERIODIC_TELEMETRY
469 
470 static void send_nav_ref(struct transport_tx *trans, struct link_device *dev)
471 {
472  pprz_msg_send_NAVIGATION_REF(trans, dev, AC_ID,
474 }
475 
476 static void send_nav(struct transport_tx *trans, struct link_device *dev)
477 {
478  SEND_NAVIGATION(trans, dev);
479 }
480 
481 static void DownlinkSendWp(struct transport_tx *trans, struct link_device *dev, uint8_t _wp)
482 {
483  float x = nav_utm_east0 + waypoints[_wp].x;
484  float y = nav_utm_north0 + waypoints[_wp].y;
485  pprz_msg_send_WP_MOVED(trans, dev, AC_ID, &_wp, &x, &y, &(waypoints[_wp].a), &nav_utm_zone0);
486 }
487 
488 static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
489 {
490  static uint8_t i;
491  i++;
492  if (i >= nb_waypoint) { i = 0; }
493  DownlinkSendWp(trans, dev, i);
494 }
495 
497 {
498  if (_wp >= nb_waypoint) return;
499  DownlinkSendWp(&(DefaultChannel).trans_tx, &(DefaultDevice).device, _wp);
500 }
501 
502 
503 static void send_circle(struct transport_tx *trans, struct link_device *dev)
504 {
505  if (nav_in_circle) {
506  pprz_msg_send_CIRCLE(trans, dev, AC_ID,
508  }
509 }
510 
511 static void send_segment(struct transport_tx *trans, struct link_device *dev)
512 {
513  if (nav_in_segment) {
514  pprz_msg_send_SEGMENT(trans, dev, AC_ID,
516  }
517 }
518 
519 static void send_survey(struct transport_tx *trans, struct link_device *dev)
520 {
521  if (nav_survey_active) {
522  pprz_msg_send_SURVEY(trans, dev, AC_ID,
524  }
525 }
526 #endif
527 
531 void nav_init(void)
532 {
533  nav_block = 0;
534  nav_stage = 0;
535  ground_alt = GROUND_ALT;
540 
541  fp_pitch = 0.f;
542  fp_throttle = 0.f;
543  fp_climb = 0.f;
544 
545 #ifdef NAV_GROUND_SPEED_PGAIN
546  nav_ground_speed_pgain = ABS(NAV_GROUND_SPEED_PGAIN);
547  nav_ground_speed_setpoint = NOMINAL_AIRSPEED;
548 #endif
549 
550 #if PERIODIC_TELEMETRY
551  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_NAVIGATION_REF, send_nav_ref);
552  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_NAVIGATION, send_nav);
557 #endif
558 
559  // generated init function
560  auto_nav_init();
561 }
562 
569 void nav_without_gps(void)
570 {
573 
574 #ifdef SECTION_FAILSAFE
575  h_ctl_roll_setpoint = FAILSAFE_DEFAULT_ROLL;
576  nav_pitch = FAILSAFE_DEFAULT_PITCH;
577  nav_throttle_setpoint = TRIM_UPPRZ((FAILSAFE_DEFAULT_THROTTLE) * MAX_PPRZ);
578 #else
580  nav_pitch = 0;
581  nav_throttle_setpoint = TRIM_UPPRZ((V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE) * MAX_PPRZ);
582 #endif
583 }
584 
585 
586 /**************** 8 Navigation **********************************************/
587 
588 
589 enum eight_status { R1T, RT2, C2, R2T, RT1, C1 };
590 
592 void nav_eight_init(void)
593 {
594  eight_status = C1;
595 }
596 
605 void nav_eight(uint8_t target, uint8_t c1, float radius)
606 {
607  float aradius = fabs(radius);
608  float alt = waypoints[target].a;
609  waypoints[c1].a = alt;
610 
611  float target_c1_x = waypoints[c1].x - waypoints[target].x;
612  float target_c1_y = waypoints[c1].y - waypoints[target].y;
613  float d = sqrtf(target_c1_x * target_c1_x + target_c1_y * target_c1_y);
614  d = Max(d, 1.); /* To prevent a division by zero */
615 
616  /* Unit vector from target to c1 */
617  float u_x = target_c1_x / d;
618  float u_y = target_c1_y / d;
619 
620  /* Move [c1] closer if needed */
621  if (d > 2 * aradius) {
622  d = 2 * aradius;
623  waypoints[c1].x = waypoints[target].x + d * u_x;
624  waypoints[c1].y = waypoints[target].y + d * u_y;
625  }
626 
627  /* The other center */
628  struct point c2 = {
629  waypoints[target].x - d * u_x,
630  waypoints[target].y - d * u_y,
631  alt
632  };
633 
634  struct point c1_in = {
635  waypoints[c1].x + radius * -u_y,
636  waypoints[c1].y + radius * u_x,
637  alt
638  };
639  struct point c1_out = {
640  waypoints[c1].x - radius * -u_y,
641  waypoints[c1].y - radius * u_x,
642  alt
643  };
644 
645  struct point c2_in = {
646  c2.x + radius * -u_y,
647  c2.y + radius * u_x,
648  alt
649  };
650  struct point c2_out = {
651  c2.x - radius * -u_y,
652  c2.y - radius * u_x,
653  alt
654  };
655 
656  float qdr_out = M_PI - atan2f(u_y, u_x);
657  if (radius < 0) {
658  qdr_out += M_PI;
659  }
660 
661  switch (eight_status) {
662  case C1 :
663  NavCircleWaypoint(c1, radius);
664  if (NavQdrCloseTo(DegOfRad(qdr_out) - 10)) {
665  eight_status = R1T;
666  InitStage();
667  }
668  return;
669 
670  case R1T:
671  nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y);
672  if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c1_out.x, c1_out.y, 0)) {
673  eight_status = RT2;
674  InitStage();
675  }
676  return;
677 
678  case RT2:
679  nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y);
680  if (nav_approaching_xy(c2_in.x, c2_in.y, c1_out.x, c1_out.y, CARROT)) {
681  eight_status = C2;
682  InitStage();
683  }
684  return;
685 
686  case C2 :
687  nav_circle_XY(c2.x, c2.y, -radius);
688  if (NavQdrCloseTo(DegOfRad(qdr_out) + 10)) {
689  eight_status = R2T;
690  InitStage();
691  }
692  return;
693 
694  case R2T:
695  nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y);
696  if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c2_out.x, c2_out.y, 0)) {
697  eight_status = RT1;
698  InitStage();
699  }
700  return;
701 
702  case RT1:
703  nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y);
704  if (nav_approaching_xy(c1_in.x, c1_in.y, c2_out.x, c2_out.y, CARROT)) {
705  eight_status = C1;
706  InitStage();
707  }
708  return;
709 
710  default:/* Should not occur !!! Doing nothing */
711  return;
712  } /* switch */
713 }
714 
715 /************** Oval Navigation **********************************************/
716 
727 
728 void nav_oval_init(void)
729 {
730  oval_status = OC2;
731  nav_oval_count = 0;
732 }
733 
734 void nav_oval(uint8_t p1, uint8_t p2, float radius)
735 {
736  radius = - radius; /* Historical error ? */
737 
738  float alt = waypoints[p1].a;
739  waypoints[p2].a = alt;
740 
741  float p2_p1_x = waypoints[p1].x - waypoints[p2].x;
742  float p2_p1_y = waypoints[p1].y - waypoints[p2].y;
743  float d = sqrtf(p2_p1_x * p2_p1_x + p2_p1_y * p2_p1_y);
744 
745  /* Unit vector from p1 to p2 */
746  float u_x = p2_p1_x / d;
747  float u_y = p2_p1_y / d;
748 
749  /* The half circle centers and the other leg */
750  struct point p1_center = { waypoints[p1].x + radius * -u_y,
751  waypoints[p1].y + radius * u_x,
752  alt
753  };
754  struct point p1_out = { waypoints[p1].x + 2 * radius * -u_y,
755  waypoints[p1].y + 2 * radius * u_x,
756  alt
757  };
758 
759  struct point p2_in = { waypoints[p2].x + 2 * radius * -u_y,
760  waypoints[p2].y + 2 * radius * u_x,
761  alt
762  };
763  struct point p2_center = { waypoints[p2].x + radius * -u_y,
764  waypoints[p2].y + radius * u_x,
765  alt
766  };
767 
768  float qdr_out_2 = M_PI - atan2f(u_y, u_x);
769  float qdr_out_1 = qdr_out_2 + M_PI;
770  if (radius < 0) {
771  qdr_out_2 += M_PI;
772  qdr_out_1 += M_PI;
773  }
774  float qdr_anticipation = (radius > 0 ? -15 : 15);
775 
776  switch (oval_status) {
777  case OC1 :
778  nav_circle_XY(p1_center.x, p1_center.y, -radius);
779  if (NavQdrCloseTo(DegOfRad(qdr_out_1) - qdr_anticipation)) {
780  oval_status = OR12;
781  InitStage();
783  }
784  return;
785 
786  case OR12:
787  nav_route_xy(p1_out.x, p1_out.y, p2_in.x, p2_in.y);
788  if (nav_approaching_xy(p2_in.x, p2_in.y, p1_out.x, p1_out.y, CARROT)) {
789  oval_status = OC2;
790  nav_oval_count++;
791  InitStage();
793  }
794  return;
795 
796  case OC2 :
797  nav_circle_XY(p2_center.x, p2_center.y, -radius);
798  if (NavQdrCloseTo(DegOfRad(qdr_out_2) - qdr_anticipation)) {
799  oval_status = OR21;
800  InitStage();
802  }
803  return;
804 
805  case OR21:
806  nav_route_xy(waypoints[p2].x, waypoints[p2].y, waypoints[p1].x, waypoints[p1].y);
807  if (nav_approaching_xy(waypoints[p1].x, waypoints[p1].y, waypoints[p2].x, waypoints[p2].y, CARROT)) {
808  oval_status = OC1;
809  InitStage();
811  }
812  return;
813 
814  default: /* Should not occur !!! Doing nothing */
815  return;
816  }
817 }
nav_climb
float nav_climb
Definition: nav.c:57
point::a
float a
Definition: common_nav.h:42
carrot_y
float carrot_y
Definition: nav.c:51
send_circle
static void send_circle(struct transport_tx *trans, struct link_device *dev)
Definition: nav.c:503
traffic_info.h
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
nav_follow
void nav_follow(uint8_t _ac_id, float distance, float height)
Definition: nav.c:301
LINE_START_FUNCTION
#define LINE_START_FUNCTION
Definition: nav.c:275
acInfoGetGspeed
static float acInfoGetGspeed(uint8_t ac_id)
Get vehicle ground speed (float).
Definition: traffic_info.h:424
nav_circle_trigo_qdr
float nav_circle_trigo_qdr
Definition: nav.c:56
last_x
float last_x
Definition: nav.c:45
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
stateGetHorizontalSpeedDir_f
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
nav_in_circle
bool nav_in_circle
Definition: nav.c:67
v_ctl_mode
uint8_t v_ctl_mode
Definition: energy_ctrl.c:74
V_CTL_MODE_AUTO_ALT
#define V_CTL_MODE_AUTO_ALT
Definition: guidance_common.h:39
oval_status
enum oval_status oval_status
Definition: nav.c:43
InitStage
#define InitStage()
Definition: common_flight_plan.h:44
nav_circle_x
float nav_circle_x
Definition: nav.c:69
nav_leg_progress
static float nav_leg_progress
Status on the current leg (percentage, 0.
Definition: nav.c:61
nav_mode
int nav_mode
Definition: nav.c:91
baseleg_out_qdr
float baseleg_out_qdr
Definition: nav.c:218
nav_circle_y
float nav_circle_y
Definition: nav.c:69
nav_set_altitude
static void nav_set_altitude(void)
Definition: nav.c:412
nav_utm_east0
int32_t nav_utm_east0
Definition: common_nav.c:43
nav_ground_speed_pgain
float nav_ground_speed_pgain
Definition: nav.c:84
send_nav_ref
static void send_nav_ref(struct transport_tx *trans, struct link_device *dev)
Periodic telemetry.
Definition: nav.c:470
fp_climb
float fp_climb
Definition: nav.c:312
dist2_to_home
float dist2_to_home
squared distance to home waypoint
Definition: navigation.c:87
compute_dist2_to_home
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
Definition: navigation.c:455
WaypointY
#define WaypointY(_wp)
Definition: common_nav.h:46
v_ctl_auto_throttle_max_cruise_throttle
float v_ctl_auto_throttle_max_cruise_throttle
Definition: guidance_v.c:59
LATERAL_MODE_ROLL
#define LATERAL_MODE_ROLL
Definition: autopilot_firmware.h:38
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
s
static uint32_t s
Definition: light_scheduler.c:33
inter_mcu.h
WaypointX
#define WaypointX(_wp)
Definition: common_nav.h:45
nav_circle_XY
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition: nav.c:109
acInfoGetCourse
static float acInfoGetCourse(uint8_t ac_id)
Get vehicle course (float).
Definition: traffic_info.h:416
h_ctl_roll_max_setpoint
float h_ctl_roll_max_setpoint
Definition: stabilization_adaptive.c:85
Square
#define Square(_x)
Definition: nav.h:51
nav_survey_west
float nav_survey_west
Definition: nav.c:88
NAV_GLIDE_PITCH_TRIM
#define NAV_GLIDE_PITCH_TRIM
Definition: nav.c:79
nav_oval_init
void nav_oval_init(void)
Definition: nav.c:728
carrot_x
float carrot_x
Definition: nav.c:51
NavQdrCloseTo
#define NavQdrCloseTo(x)
True if x (in degrees) is close to the current QDR (less than 10 degrees)
Definition: nav.h:160
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
alpha
float alpha
Definition: textons.c:107
nav_stage
uint8_t nav_stage
Definition: common_flight_plan.c:35
stabilization_attitude.h
OR12
@ OR12
Definition: nav.h:58
nav_shift
float nav_shift
Definition: nav.c:57
nav_circle_radians
float nav_circle_radians
Status on the current circle.
Definition: nav.c:54
nav_init
void nav_init(void)
Navigation Initialisation.
Definition: nav.c:531
nav_home
void nav_home(void)
Home mode navigation (circle around HOME)
Definition: nav.c:423
point
Definition: common_nav.h:39
waypoints
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
v_ctl_auto_throttle_min_cruise_throttle
float v_ctl_auto_throttle_min_cruise_throttle
Definition: guidance_v.c:58
nav_utm_north0
int32_t nav_utm_north0
Definition: common_nav.c:44
nav_glide
void nav_glide(uint8_t start_wp, uint8_t wp)
Definition: nav.c:159
h_ctl_course_pgain
float h_ctl_course_pgain
Definition: stabilization_adaptive.c:83
nav_ground_speed_setpoint
float nav_ground_speed_setpoint
Definition: nav.c:84
h_ctl_roll_setpoint
float h_ctl_roll_setpoint
Definition: stabilization_adaptive.c:157
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
v_ctl_auto_throttle_cruise_throttle
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:103
EnuCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:75
NavVerticalAutoThrottleMode
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:176
nav_circle_radius
float nav_circle_radius
Definition: nav.c:69
rc_pitch
float rc_pitch
Definition: nav.c:50
NavCircleWaypoint
#define NavCircleWaypoint(wp, radius)
Definition: nav.h:143
acInfoGetPositionEnu_f
static struct EnuCoor_f * acInfoGetPositionEnu_f(uint8_t ac_id)
Get position in local ENU coordinates (float).
Definition: traffic_info.h:383
CARROT
#define CARROT
default approaching_time for a wp
Definition: navigation.h:40
nav_survey_east
float nav_survey_east
Definition: nav.c:88
flight_altitude
float flight_altitude
Dynamically adjustable, reset to nav_altitude when it is changing.
Definition: nav.c:75
FloatVect2
Definition: pprz_algebra_float.h:49
telemetry.h
nav_pitch
float nav_pitch
Definition: nav.c:309
std.h
nav_route_xy
void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y)
Computes the carrot position along the desired segment.
Definition: nav.c:381
OC1
@ OC1
Definition: nav.h:58
last_y
float last_y
Definition: nav.c:45
pprz_t
int16_t pprz_t
Definition: paparazzi.h:6
gps.h
Device independent GPS code (interface)
autopilot
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
desired_x
float desired_x
Definition: nav.c:307
nav_leg_length
static float nav_leg_length
length of the current leg (m)
Definition: nav.c:65
lateral_mode
uint8_t lateral_mode
Definition: autopilot_firmware.c:38
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
desired_y
float desired_y
Definition: nav.c:307
nav_eight
void nav_eight(uint8_t target, uint8_t c1, float radius)
Navigation along a figure 8.
Definition: nav.c:605
last_wp
uint8_t last_wp
Index of last waypoint.
Definition: nav.c:48
OR21
@ OR21
Definition: nav.h:58
nav_compute_final_from_glide
void nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide)
Definition: nav.c:240
nav_init_stage
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition: nav.c:93
DownlinkSendWpNr
void DownlinkSendWpNr(uint8_t _wp)
Definition: nav.c:496
C2
@ C2
Definition: nav.c:589
TRIM_UPPRZ
#define TRIM_UPPRZ(pprz)
Definition: paparazzi.h:14
nav_block
uint8_t nav_block
Definition: common_flight_plan.c:35
nav_in_segment
bool nav_in_segment
Definition: nav.c:68
NAV_MODE_COURSE
#define NAV_MODE_COURSE
Definition: nav.h:86
RT1
@ RT1
Definition: nav.c:589
HORIZONTAL_MODE_CIRCLE
#define HORIZONTAL_MODE_CIRCLE
Definition: nav.h:92
nav_compute_baseleg
void nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius)
Definition: nav.c:219
nav_segment_x_2
float nav_segment_x_2
Definition: nav.c:70
nav_course
float nav_course
Definition: nav.c:57
RT2
@ RT2
Definition: nav.c:589
NAV_GRAVITY
#define NAV_GRAVITY
Definition: nav.h:50
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
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
FAILSAFE_HOME_RADIUS
#define FAILSAFE_HOME_RADIUS
Definition: nav.c:409
nav_oval
void nav_oval(uint8_t p1, uint8_t p2, float radius)
Definition: nav.c:734
V_CTL_AUTO_THROTTLE_STANDARD
#define V_CTL_AUTO_THROTTLE_STANDARD
Definition: guidance_common.h:51
autopilot.h
common_nav.c
stage_time
uint16_t stage_time
In s.
Definition: common_flight_plan.c:33
oval_status
oval_status
Definition: nav.h:58
c1
static uint16_t c1
Definition: baro_MS5534A.c:203
nav_glide_pitch_trim
float nav_glide_pitch_trim
Definition: nav.c:77
DownlinkSendWp
static void DownlinkSendWp(struct transport_tx *trans, struct link_device *dev, uint8_t _wp)
Definition: nav.c:481
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
V_CTL_MODE_AUTO_THROTTLE
#define V_CTL_MODE_AUTO_THROTTLE
Definition: guidance_common.h:37
stateGetHorizontalWindspeed_f
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1377
h_ctl_course_pre_bank
float h_ctl_course_pre_bank
Definition: stabilization_adaptive.c:81
circle_bank
float circle_bank
Definition: nav.c:72
v_ctl_throttle_setpoint
pprz_t v_ctl_throttle_setpoint
Definition: energy_ctrl.c:131
nav_radius
float nav_radius
Definition: nav.c:57
nav.h
R1T
@ R1T
Definition: nav.c:589
WaypointAlt
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition: common_nav.h:48
nav_survey_north
float nav_survey_north
Definition: nav.c:88
stateGetHorizontalSpeedNorm_f
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
c2
static uint16_t c2
Definition: baro_MS5534A.c:203
DistanceSquare
#define DistanceSquare(p1_x, p1_y, p2_x, p2_y)
Definition: nav.h:52
ground_alt
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:41
nav_circle_radians_no_rewind
float nav_circle_radians_no_rewind
Definition: nav.c:55
nav_survey_shift
float nav_survey_shift
Definition: nav.c:87
SEND_NAVIGATION
#define SEND_NAVIGATION(_trans, _dev)
Definition: nav.h:245
v_ctl_altitude_setpoint
float v_ctl_altitude_setpoint
in meters above MSL
Definition: energy_ctrl.c:88
nav_approaching_xy
bool nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time)
Decide if the UAV is approaching the current waypoint.
Definition: nav.c:324
send_nav
static void send_nav(struct transport_tx *trans, struct link_device *dev)
Definition: nav.c:476
MIN_HEIGHT_CARROT
#define MIN_HEIGHT_CARROT
Definition: nav.c:170
nav_survey_active
bool nav_survey_active
Definition: nav.c:89
LATERAL_MODE_COURSE
#define LATERAL_MODE_COURSE
Definition: autopilot_firmware.h:39
nav_segment_y_1
float nav_segment_y_1
Definition: nav.c:70
eight_status
eight_status
Definition: nav.c:589
nav_utm_zone0
uint8_t nav_utm_zone0
Definition: common_nav.c:45
nav_segment_x_1
float nav_segment_x_1
Definition: nav.c:70
unit
static unit_t unit
Definition: nav.c:31
point::y
float y
Definition: common_nav.h:41
send_survey
static void send_survey(struct transport_tx *trans, struct link_device *dev)
Definition: nav.c:519
nav_periodic_task
void nav_periodic_task(void)
Navigation main: call to the code generated from the XML flight plan.
Definition: nav.c:444
LINE_STOP_FUNCTION
#define LINE_STOP_FUNCTION
Definition: nav.c:278
send_wp_moved
static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
Definition: nav.c:488
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
nav_altitude
float nav_altitude
Definition: nav.c:306
Max
#define Max(x, y)
Definition: main_fbw.c:53
ac_id
uint8_t ac_id
Definition: sim_ap.c:48
nav_carrot_leg_progress
static float nav_carrot_leg_progress
Definition: nav.c:62
nav_throttle_setpoint
pprz_t nav_throttle_setpoint
Definition: nav.c:308
HORIZONTAL_MODE_ROUTE
#define HORIZONTAL_MODE_ROUTE
Definition: nav.h:91
compute_TOD
static void compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed)
Definition: nav.c:260
V_CTL_MODE_AUTO_CLIMB
#define V_CTL_MODE_AUTO_CLIMB
Definition: guidance_common.h:38
v_ctl_auto_throttle_submode
uint8_t v_ctl_auto_throttle_submode
Definition: energy_ctrl.c:76
nav_oval_count
uint8_t nav_oval_count
Navigation along a figure O.
Definition: nav.c:726
R2T
@ R2T
Definition: nav.c:589
dist2_to_wp
float dist2_to_wp
squared distance to next waypoint
Definition: navigation.c:90
target
struct FloatVect2 target
Definition: obstacle_avoidance.c:78
horizontal_mode
uint8_t horizontal_mode
Definition: nav.c:71
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
pprz_autopilot::launch
bool launch
request launch
Definition: autopilot.h:71
NavVerticalAltitudeMode
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition: nav.h:190
nav_survey_south
float nav_survey_south
Definition: nav.c:88
nav_eight_init
void nav_eight_init(void)
Definition: nav.c:592
nav_segment_y_2
float nav_segment_y_2
Definition: nav.c:70
nav_without_gps
void nav_without_gps(void)
Failsafe navigation without position estimation.
Definition: nav.c:569
fp_throttle
float fp_throttle
Definition: nav.c:311
fp_pitch
float fp_pitch
Definition: nav.c:310
send_segment
static void send_segment(struct transport_tx *trans, struct link_device *dev)
Definition: nav.c:511
C1
@ C1
Definition: nav.c:589
Min
#define Min(x, y)
Definition: esc_dshot.c:85
fly_to_xy
void fly_to_xy(float x, float y)
Computes desired_x, desired_y and desired_course.
Definition: nav.c:355
OC2
@ OC2
Definition: nav.h:58
h_ctl_course_setpoint
float h_ctl_course_setpoint
Definition: stabilization_adaptive.c:80