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