Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "inter_mcu.h"
38 #include "subsystems/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 {
95  last_x = stateGetPositionEnu_f()->x;
96  last_y = stateGetPositionEnu_f()->y;
97  stage_time = 0;
98  nav_circle_radians = 0;
99  nav_circle_radians_no_rewind = 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;
144  horizontal_mode = HORIZONTAL_MODE_CIRCLE;
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_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  float start_alt = waypoints[start_wp].a;
161  float diff_alt = waypoints[wp].a - start_alt;
162  float alt = start_alt + nav_leg_progress * diff_alt;
163  float pre_climb = stateGetHorizontalSpeedNorm_f() * diff_alt / nav_leg_length;
164  NavVerticalAltitudeMode(alt, pre_climb);
165 }
166 
167 
168 #define MAX_DIST_CARROT 250.
169 #define MIN_HEIGHT_CARROT 50.
170 #define MAX_HEIGHT_CARROT 150.
171 
172 #define Goto3D(radius) { \
173  if (autopilot_get_mode() == AP_MODE_AUTO2) { \
174  int16_t yaw = imcu_get_radio(RADIO_YAW); \
175  if (yaw > MIN_DX || yaw < -MIN_DX) { \
176  carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.); \
177  carrot_x = Min(carrot_x, MAX_DIST_CARROT); \
178  carrot_x = Max(carrot_x, -MAX_DIST_CARROT); \
179  } \
180  int16_t pitch = imcu_get_radio(RADIO_PITCH); \
181  if (pitch > MIN_DX || pitch < -MIN_DX) { \
182  carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); \
183  carrot_y = Min(carrot_y, MAX_DIST_CARROT); \
184  carrot_y = Max(carrot_y, -MAX_DIST_CARROT); \
185  } \
186  v_ctl_mode = V_CTL_MODE_AUTO_ALT; \
187  int16_t roll = imcu_get_radio(RADIO_ROLL); \
188  if (roll > MIN_DX || roll < -MIN_DX) { \
189  nav_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); \
190  nav_altitude = Max(nav_altitude, MIN_HEIGHT_CARROT+ground_alt); \
191  nav_altitude = Min(nav_altitude, MAX_HEIGHT_CARROT+ground_alt); \
192  } \
193  } \
194  nav_circle_XY(carrot_x, carrot_y, radius); \
195  }
196 
197 
198 
199 #ifdef NAV_GROUND_SPEED_PGAIN
200 
202 static void nav_ground_speed_loop(void)
203 {
204  if (MINIMUM_AIRSPEED < nav_ground_speed_setpoint
205  && nav_ground_speed_setpoint < MAXIMUM_AIRSPEED) {
206  float err = nav_ground_speed_setpoint - stateGetHorizontalSpeedNorm_f();
207  v_ctl_auto_throttle_cruise_throttle += nav_ground_speed_pgain * err;
210  } else {
211  /* Reset cruise throttle to nominal value */
212  v_ctl_auto_throttle_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
213  }
214 }
215 #endif
216 
218 void nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius)
219 {
220  nav_radius = radius;
221 
222  float x_0 = waypoints[wp_td].x - waypoints[wp_af].x;
223  float y_0 = waypoints[wp_td].y - waypoints[wp_af].y;
224 
225  /* Unit vector from AF to TD */
226  float d = sqrtf(x_0 * x_0 + y_0 * y_0);
227  float x_1 = x_0 / d;
228  float y_1 = y_0 / d;
229 
230  waypoints[wp_baseleg].x = waypoints[wp_af].x + y_1 * nav_radius;
231  waypoints[wp_baseleg].y = waypoints[wp_af].y - x_1 * nav_radius;
232  waypoints[wp_baseleg].a = waypoints[wp_af].a;
233  baseleg_out_qdr = M_PI - atan2f(-y_1, -x_1);
234  if (nav_radius < 0) {
235  baseleg_out_qdr += M_PI;
236  }
237 }
238 
239 void nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide)
240 {
241 
242  float x_0 = waypoints[wp_td].x - waypoints[wp_af].x;
243  float y_0 = waypoints[wp_td].y - waypoints[wp_af].y;
244  float h_0 = waypoints[wp_td].a - waypoints[wp_af].a;
245 
246  /* Unit vector from AF to TD */
247  float d = sqrtf(x_0 * x_0 + y_0 * y_0);
248  float x_1 = x_0 / d;
249  float y_1 = y_0 / d;
250 
251  waypoints[wp_af].x = waypoints[wp_td].x + x_1 * h_0 * glide;
252  waypoints[wp_af].y = waypoints[wp_td].y + y_1 * h_0 * glide;
253 }
254 
255 
256 /* For a landing UPWIND.
257  Computes Top Of Descent waypoint from Touch Down and Approach Fix
258  waypoints, using glide airspeed, glide vertical speed and wind */
259 static inline void compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed)
260 {
261  struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
262  float td_af_x = WaypointX(_af) - WaypointX(_td);
263  float td_af_y = WaypointY(_af) - WaypointY(_td);
264  float td_af = sqrtf(td_af_x * td_af_x + td_af_y * td_af_y);
265  float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrtf(
266  wind->x * wind->x + wind->y * wind->y));
267  WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
268  WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
269  WaypointAlt(_tod) = WaypointAlt(_af);
270 }
271 
272 
273 #ifndef LINE_START_FUNCTION
274 #define LINE_START_FUNCTION {}
275 #endif
276 #ifndef LINE_STOP_FUNCTION
277 #define LINE_STOP_FUNCTION {}
278 #endif
279 
280 #ifdef TRAFFIC_INFO
282 
283 void nav_follow(uint8_t ac_id, float distance, float height)
284 {
285  struct EnuCoor_f *ac = acInfoGetPositionEnu_f(ac_id);
287  NavVerticalAltitudeMode(Max(ac->z + height, ground_alt + SECURITY_HEIGHT), 0.);
288  float alpha = M_PI / 2 - acInfoGetCourse(ac_id);
289  float ca = cosf(alpha), sa = sinf(alpha);
290  float x = ac->x - distance * ca;
291  float y = ac->y - distance * sa;
292  fly_to_xy(x, y);
293 #ifdef NAV_FOLLOW_PGAIN
294  float s = (stateGetPositionEnu_f()->x - x) * ca + (stateGetPositionEnu_f()->y - y) * sa;
295  nav_ground_speed_setpoint = acInfoGetGspeed(ac_id) + NAV_FOLLOW_PGAIN * s;
296  nav_ground_speed_loop();
297 #endif
298 }
299 #else
300 void nav_follow(uint8_t __attribute__((unused)) _ac_id, float __attribute__((unused)) distance,
301  float __attribute__((unused)) height) {}
302 #endif // TRAFFIC_INFO
303 
304 
305 float nav_altitude = GROUND_ALT + MIN_HEIGHT_CARROT;
308 float nav_pitch; /* Rad */
309 float fp_pitch; /* deg */
310 float fp_throttle; /* [0-1] */
311 float fp_climb; /* m/s */
312 
313 
323 bool nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time)
324 {
326  float pw_x = x - stateGetPositionEnu_f()->x;
328  float pw_y = y - stateGetPositionEnu_f()->y;
329 
330  if (approaching_time < 0.) {
331  // fly after the destination waypoint
332  float leg_x = x - from_x;
333  float leg_y = y - from_y;
334  float leg = sqrtf(Max(leg_x * leg_x + leg_y * leg_y, 1.));
335  float exceed_dist = approaching_time * stateGetHorizontalSpeedNorm_f(); // negative value
336  float scal_prod = (leg_x * pw_x + leg_y * pw_y) / leg;
337  return (scal_prod < exceed_dist);
338  } else {
339  // fly close enough of the waypoint or cross it
340  dist2_to_wp = pw_x * pw_x + pw_y * pw_y;
341  float min_dist = approaching_time * stateGetHorizontalSpeedNorm_f();
342  if (dist2_to_wp < min_dist * min_dist) {
343  return true;
344  }
345  float scal_prod = (x - from_x) * pw_x + (y - from_y) * pw_y;
346  return (scal_prod < 0.);
347  }
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 
393  nav_carrot_leg_progress = nav_leg_progress + Max(carrot / nav_leg_length, 0.);
394  nav_in_segment = true;
395  nav_segment_x_1 = last_wp_x;
396  nav_segment_y_1 = last_wp_y;
397  nav_segment_x_2 = wp_x;
398  nav_segment_y_2 = wp_y;
399  horizontal_mode = HORIZONTAL_MODE_ROUTE;
400 
401  fly_to_xy(last_wp_x + nav_carrot_leg_progress * leg_x + nav_shift * leg_y / nav_leg_length,
402  last_wp_y + nav_carrot_leg_progress * leg_y - nav_shift * leg_x / nav_leg_length);
403 }
404 
406 
407 #ifndef FAILSAFE_HOME_RADIUS
408 #define FAILSAFE_HOME_RADIUS DEFAULT_CIRCLE_RADIUS
409 #endif
410 
411 static void nav_set_altitude(void)
412 {
413  static float last_nav_altitude;
414  if (fabs(nav_altitude - last_nav_altitude) > 1.) {
415  flight_altitude = nav_altitude;
416  last_nav_altitude = nav_altitude;
417  }
419 }
420 
422 void nav_home(void)
423 {
426  nav_pitch = 0.;
428  nav_altitude = ground_alt + HOME_MODE_HEIGHT;
432 }
433 
439 {
440  nav_survey_active = false;
441 
443  dist2_to_wp = 0.;
444 
445  auto_nav(); /* From flight_plan.h */
446 
447  h_ctl_course_pre_bank = nav_in_circle ? circle_bank : 0;
448 
449 #ifdef AGR_CLIMB
452  }
453 #endif
454 
456 }
457 
461 #if PERIODIC_TELEMETRY
463 
464 static void send_nav_ref(struct transport_tx *trans, struct link_device *dev)
465 {
466  pprz_msg_send_NAVIGATION_REF(trans, dev, AC_ID,
468 }
469 
470 static void send_nav(struct transport_tx *trans, struct link_device *dev)
471 {
472  SEND_NAVIGATION(trans, dev);
473 }
474 
475 static void DownlinkSendWp(struct transport_tx *trans, struct link_device *dev, uint8_t _wp)
476 {
477  float x = nav_utm_east0 + waypoints[_wp].x;
478  float y = nav_utm_north0 + waypoints[_wp].y;
479  pprz_msg_send_WP_MOVED(trans, dev, AC_ID, &_wp, &x, &y, &(waypoints[_wp].a), &nav_utm_zone0);
480 }
481 
482 static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
483 {
484  static uint8_t i;
485  i++;
486  if (i >= nb_waypoint) { i = 0; }
487  DownlinkSendWp(trans, dev, i);
488 }
489 
491 {
492  if (_wp >= nb_waypoint) return;
493  DownlinkSendWp(&(DefaultChannel).trans_tx, &(DefaultDevice).device, _wp);
494 }
495 
496 
497 static void send_circle(struct transport_tx *trans, struct link_device *dev)
498 {
499  if (nav_in_circle) {
500  pprz_msg_send_CIRCLE(trans, dev, AC_ID,
501  &nav_circle_x, &nav_circle_y, &nav_circle_radius);
502  }
503 }
504 
505 static void send_segment(struct transport_tx *trans, struct link_device *dev)
506 {
507  if (nav_in_segment) {
508  pprz_msg_send_SEGMENT(trans, dev, AC_ID,
509  &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2);
510  }
511 }
512 
513 static void send_survey(struct transport_tx *trans, struct link_device *dev)
514 {
515  if (nav_survey_active) {
516  pprz_msg_send_SURVEY(trans, dev, AC_ID,
517  &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south);
518  }
519 }
520 #endif
521 
525 void nav_init(void)
526 {
527  nav_block = 0;
528  nav_stage = 0;
529  ground_alt = GROUND_ALT;
530  nav_glide_pitch_trim = NAV_GLIDE_PITCH_TRIM;
531  nav_radius = DEFAULT_CIRCLE_RADIUS;
532  nav_survey_shift = 2 * DEFAULT_CIRCLE_RADIUS;
533  nav_mode = NAV_MODE_COURSE;
534 
535  fp_pitch = 0.f;
536  fp_throttle = 0.f;
537  fp_climb = 0.f;
538 
539 #ifdef NAV_GROUND_SPEED_PGAIN
540  nav_ground_speed_pgain = ABS(NAV_GROUND_SPEED_PGAIN);
541  nav_ground_speed_setpoint = NOMINAL_AIRSPEED;
542 #endif
543 
544 #if PERIODIC_TELEMETRY
545  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_NAVIGATION_REF, send_nav_ref);
546  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_NAVIGATION, send_nav);
551 #endif
552 
553  // generated init function
554  auto_nav_init();
555 }
556 
563 void nav_without_gps(void)
564 {
567 
568 #ifdef SECTION_FAILSAFE
569  h_ctl_roll_setpoint = FAILSAFE_DEFAULT_ROLL;
570  nav_pitch = FAILSAFE_DEFAULT_PITCH;
571  nav_throttle_setpoint = TRIM_UPPRZ((FAILSAFE_DEFAULT_THROTTLE) * MAX_PPRZ);
572 #else
574  nav_pitch = 0;
575  nav_throttle_setpoint = TRIM_UPPRZ((V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE) * MAX_PPRZ);
576 #endif
577 }
578 
579 
580 /**************** 8 Navigation **********************************************/
581 
582 
583 enum eight_status { R1T, RT2, C2, R2T, RT1, C1 };
584 
586 void nav_eight_init(void)
587 {
588  eight_status = C1;
589 }
590 
600 {
601  float aradius = fabs(radius);
602  float alt = waypoints[target].a;
603  waypoints[c1].a = alt;
604 
605  float target_c1_x = waypoints[c1].x - waypoints[target].x;
606  float target_c1_y = waypoints[c1].y - waypoints[target].y;
607  float d = sqrtf(target_c1_x * target_c1_x + target_c1_y * target_c1_y);
608  d = Max(d, 1.); /* To prevent a division by zero */
609 
610  /* Unit vector from target to c1 */
611  float u_x = target_c1_x / d;
612  float u_y = target_c1_y / d;
613 
614  /* Move [c1] closer if needed */
615  if (d > 2 * aradius) {
616  d = 2 * aradius;
617  waypoints[c1].x = waypoints[target].x + d * u_x;
618  waypoints[c1].y = waypoints[target].y + d * u_y;
619  }
620 
621  /* The other center */
622  struct point c2 = {
623  waypoints[target].x - d * u_x,
624  waypoints[target].y - d * u_y,
625  alt
626  };
627 
628  struct point c1_in = {
629  waypoints[c1].x + radius * -u_y,
630  waypoints[c1].y + radius * u_x,
631  alt
632  };
633  struct point c1_out = {
634  waypoints[c1].x - radius * -u_y,
635  waypoints[c1].y - radius * u_x,
636  alt
637  };
638 
639  struct point c2_in = {
640  c2.x + radius * -u_y,
641  c2.y + radius * u_x,
642  alt
643  };
644  struct point c2_out = {
645  c2.x - radius * -u_y,
646  c2.y - radius * u_x,
647  alt
648  };
649 
650  float qdr_out = M_PI - atan2f(u_y, u_x);
651  if (radius < 0) {
652  qdr_out += M_PI;
653  }
654 
655  switch (eight_status) {
656  case C1 :
657  NavCircleWaypoint(c1, radius);
658  if (NavQdrCloseTo(DegOfRad(qdr_out) - 10)) {
659  eight_status = R1T;
660  InitStage();
661  }
662  return;
663 
664  case R1T:
665  nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y);
666  if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c1_out.x, c1_out.y, 0)) {
667  eight_status = RT2;
668  InitStage();
669  }
670  return;
671 
672  case RT2:
673  nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y);
674  if (nav_approaching_xy(c2_in.x, c2_in.y, c1_out.x, c1_out.y, CARROT)) {
675  eight_status = C2;
676  InitStage();
677  }
678  return;
679 
680  case C2 :
681  nav_circle_XY(c2.x, c2.y, -radius);
682  if (NavQdrCloseTo(DegOfRad(qdr_out) + 10)) {
683  eight_status = R2T;
684  InitStage();
685  }
686  return;
687 
688  case R2T:
689  nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y);
690  if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c2_out.x, c2_out.y, 0)) {
691  eight_status = RT1;
692  InitStage();
693  }
694  return;
695 
696  case RT1:
697  nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y);
698  if (nav_approaching_xy(c1_in.x, c1_in.y, c2_out.x, c2_out.y, CARROT)) {
699  eight_status = C1;
700  InitStage();
701  }
702  return;
703 
704  default:/* Should not occur !!! Doing nothing */
705  return;
706  } /* switch */
707 }
708 
709 /************** Oval Navigation **********************************************/
710 
721 
722 void nav_oval_init(void)
723 {
724  oval_status = OC2;
725  nav_oval_count = 0;
726 }
727 
728 void nav_oval(uint8_t p1, uint8_t p2, float radius)
729 {
730  radius = - radius; /* Historical error ? */
731 
732  float alt = waypoints[p1].a;
733  waypoints[p2].a = alt;
734 
735  float p2_p1_x = waypoints[p1].x - waypoints[p2].x;
736  float p2_p1_y = waypoints[p1].y - waypoints[p2].y;
737  float d = sqrtf(p2_p1_x * p2_p1_x + p2_p1_y * p2_p1_y);
738 
739  /* Unit vector from p1 to p2 */
740  float u_x = p2_p1_x / d;
741  float u_y = p2_p1_y / d;
742 
743  /* The half circle centers and the other leg */
744  struct point p1_center = { waypoints[p1].x + radius * -u_y,
745  waypoints[p1].y + radius * u_x,
746  alt
747  };
748  struct point p1_out = { waypoints[p1].x + 2 * radius * -u_y,
749  waypoints[p1].y + 2 * radius * u_x,
750  alt
751  };
752 
753  struct point p2_in = { waypoints[p2].x + 2 * radius * -u_y,
754  waypoints[p2].y + 2 * radius * u_x,
755  alt
756  };
757  struct point p2_center = { waypoints[p2].x + radius * -u_y,
758  waypoints[p2].y + radius * u_x,
759  alt
760  };
761 
762  float qdr_out_2 = M_PI - atan2f(u_y, u_x);
763  float qdr_out_1 = qdr_out_2 + M_PI;
764  if (radius < 0) {
765  qdr_out_2 += M_PI;
766  qdr_out_1 += M_PI;
767  }
768  float qdr_anticipation = (radius > 0 ? -15 : 15);
769 
770  switch (oval_status) {
771  case OC1 :
772  nav_circle_XY(p1_center.x, p1_center.y, -radius);
773  if (NavQdrCloseTo(DegOfRad(qdr_out_1) - qdr_anticipation)) {
774  oval_status = OR12;
775  InitStage();
777  }
778  return;
779 
780  case OR12:
781  nav_route_xy(p1_out.x, p1_out.y, p2_in.x, p2_in.y);
782  if (nav_approaching_xy(p2_in.x, p2_in.y, p1_out.x, p1_out.y, CARROT)) {
783  oval_status = OC2;
784  nav_oval_count++;
785  InitStage();
787  }
788  return;
789 
790  case OC2 :
791  nav_circle_XY(p2_center.x, p2_center.y, -radius);
792  if (NavQdrCloseTo(DegOfRad(qdr_out_2) - qdr_anticipation)) {
793  oval_status = OR21;
794  InitStage();
796  }
797  return;
798 
799  case OR21:
800  nav_route_xy(waypoints[p2].x, waypoints[p2].y, waypoints[p1].x, waypoints[p1].y);
801  if (nav_approaching_xy(waypoints[p1].x, waypoints[p1].y, waypoints[p2].x, waypoints[p2].y, CARROT)) {
802  oval_status = OC1;
803  InitStage();
805  }
806  return;
807 
808  default: /* Should not occur !!! Doing nothing */
809  return;
810  }
811 }
uint8_t ac_id
Definition: sim_ap.c:45
Communication between fbw and ap processes.
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition: common_nav.h:48
float x
Definition: common_nav.h:40
uint8_t lateral_mode
float v_ctl_altitude_setpoint
in meters above MSL
Definition: energy_ctrl.c:88
static float acInfoGetGspeed(uint8_t ac_id)
Get vehicle ground speed (float).
Definition: traffic_info.h:424
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:40
Periodic telemetry system header (includes downlink utility and generated code).
float h_ctl_course_setpoint
vector in East North Up coordinates Units: meters
static float acInfoGetCourse(uint8_t ac_id)
Get vehicle course (float).
Definition: traffic_info.h:416
int16_t pprz_t
Definition: paparazzi.h:6
float alpha
Definition: textons.c:107
static float radius
Definition: chemotaxis.c:15
uint8_t nav_utm_zone0
Definition: common_nav.c:44
#define InitStage()
uint8_t nav_block
#define V_CTL_MODE_AUTO_THROTTLE
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
float v_ctl_auto_throttle_max_cruise_throttle
Definition: guidance_v.c:59
Fixed wing horizontal control.
#define LATERAL_MODE_COURSE
#define V_CTL_AUTO_THROTTLE_STANDARD
float y
Definition: common_nav.h:41
static struct EnuCoor_f * acInfoGetPositionEnu_f(uint8_t ac_id)
Get position in local ENU coordinates (float).
Definition: traffic_info.h:383
#define TRIM_UPPRZ(pprz)
Definition: paparazzi.h:14
#define V_CTL_MODE_AUTO_ALT
int32_t nav_utm_north0
Definition: common_nav.c:43
#define WaypointX(_wp)
Definition: common_nav.h:45
float x
in meters
Device independent GPS code (interface)
static uint16_t c1
Definition: baro_MS5534A.c:203
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
uint8_t v_ctl_auto_throttle_submode
Definition: energy_ctrl.c:76
float v_ctl_auto_throttle_min_cruise_throttle
Definition: guidance_v.c:58
uint8_t v_ctl_mode
Definition: energy_ctrl.c:74
#define Min(x, y)
Definition: main_fbw.c:52
#define V_CTL_MODE_AUTO_CLIMB
#define Max(x, y)
Definition: main_fbw.c:53
uint16_t stage_time
In s.
#define WaypointY(_wp)
Definition: common_nav.h:46
const uint8_t nb_waypoint
Definition: common_nav.c:37
float h_ctl_roll_max_setpoint
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
uint8_t nav_stage
Core autopilot interface common to all firmwares.
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
float h_ctl_course_pre_bank
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:103
unsigned char uint8_t
Definition: types.h:14
#define LATERAL_MODE_ROLL
float h_ctl_roll_setpoint
float h_ctl_course_pgain
int32_t nav_utm_east0
Definition: common_nav.c:42
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
float z
in meters
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1377
float a
Definition: common_nav.h:42
#define MAX_PPRZ
Definition: paparazzi.h:8
struct FloatVect2 target
float y
in meters
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46