Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 #define NAV_C
29 
30 #include <math.h>
31 
33 #include "subsystems/gps.h"
36 #include "inter_mcu.h"
38 
39 #define RCLost() bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST)
40 
42 
43 float last_x, last_y;
44 
46 static uint8_t last_wp __attribute__ ((unused));
47 
48 float rc_pitch;
50 
52 float nav_circle_radians; /* Cumulated */
53 float nav_circle_radians_no_rewind; /* Cumulated */
54 float nav_circle_trigo_qdr; /* Angle from center to mobile */
56 
57 
59 static float nav_leg_progress;
61 
63 static float nav_leg_length;
64 
70 float circle_bank = 0;
71 
74 
76 #ifndef NAV_GLIDE_PITCH_TRIM
77 #define NAV_GLIDE_PITCH_TRIM 0.
78 #endif
79 
80 
81 
83 
84 /* Used in nav_survey_rectangle. Defined here for downlink and uplink */
88 
90 
91 void nav_init_stage( void ) {
94  stage_time = 0;
95  nav_circle_radians = 0;
96  nav_circle_radians_no_rewind = 0;
97  nav_in_circle = FALSE;
98  nav_in_segment = FALSE;
99  nav_shift = 0;
100 }
101 
102 #define PowerVoltage() (vsupply/10.)
103 #define RcRoll(travel) (fbw_state->channels[RADIO_ROLL]* (float)travel /(float)MAX_PPRZ)
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  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  float dist2_center = DistanceSquare(pos->x, pos->y, x, y);
125  float dist_carrot = CARROT*NOMINAL_AIRSPEED;
126 
127  radius += -nav_shift;
128 
129  float abs_radius = fabs(radius);
130 
132  circle_bank =
133  (dist2_center > Square(abs_radius + dist_carrot)
134  || dist2_center < Square(abs_radius - dist_carrot)) ?
135  0 :
137 
138  float carrot_angle = dist_carrot / abs_radius;
139  carrot_angle = Min(carrot_angle, M_PI/4);
140  carrot_angle = Max(carrot_angle, M_PI/16);
141  float alpha_carrot = nav_circle_trigo_qdr - sign_radius * carrot_angle;
142  horizontal_mode = HORIZONTAL_MODE_CIRCLE;
143  float radius_carrot = abs_radius;
144  if (nav_mode == NAV_MODE_COURSE) {
145  radius_carrot += (abs_radius / cosf(carrot_angle) - abs_radius);
146  }
147  fly_to_xy(x+cosf(alpha_carrot)*radius_carrot,
148  y+sinf(alpha_carrot)*radius_carrot);
149  nav_in_circle = TRUE;
150  nav_circle_x = x;
151  nav_circle_y = y;
152  nav_circle_radius = radius;
153 }
154 
155 
156 #define NavGlide(_last_wp, _wp) { \
157  float start_alt = waypoints[_last_wp].a; \
158  float diff_alt = waypoints[_wp].a - start_alt; \
159  float alt = start_alt + nav_leg_progress * diff_alt; \
160  float pre_climb = (*stateGetHorizontalSpeedNorm_f()) * diff_alt / nav_leg_length; \
161  NavVerticalAltitudeMode(alt, pre_climb); \
162  }
163 
164 
165 
166 
167 #define MAX_DIST_CARROT 250.
168 #define MIN_HEIGHT_CARROT 50.
169 #define MAX_HEIGHT_CARROT 150.
170 
171 #define Goto3D(radius) { \
172  if (pprz_mode == PPRZ_MODE_AUTO2) { \
173  int16_t yaw = fbw_state->channels[RADIO_YAW]; \
174  if (yaw > MIN_DX || yaw < -MIN_DX) { \
175  carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.); \
176  carrot_x = Min(carrot_x, MAX_DIST_CARROT); \
177  carrot_x = Max(carrot_x, -MAX_DIST_CARROT); \
178  } \
179  int16_t pitch = fbw_state->channels[RADIO_PITCH]; \
180  if (pitch > MIN_DX || pitch < -MIN_DX) { \
181  carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); \
182  carrot_y = Min(carrot_y, MAX_DIST_CARROT); \
183  carrot_y = Max(carrot_y, -MAX_DIST_CARROT); \
184  } \
185  v_ctl_mode = V_CTL_MODE_AUTO_ALT; \
186  int16_t roll = fbw_state->channels[RADIO_ROLL]; \
187  if (roll > MIN_DX || roll < -MIN_DX) { \
188  nav_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); \
189  nav_altitude = Max(nav_altitude, MIN_HEIGHT_CARROT+ground_alt); \
190  nav_altitude = Min(nav_altitude, MAX_HEIGHT_CARROT+ground_alt); \
191  } \
192  } \
193  nav_circle_XY(carrot_x, carrot_y, radius); \
194  }
195 
196 
197 #define NavFollow(_ac_id, _distance, _height) \
198  nav_follow(_ac_id, _distance, _height);
199 
200 
201 static unit_t unit __attribute__ ((unused));
202 
203 static inline void nav_follow(uint8_t _ac_id, float _distance, float _height);
204 
205 #ifdef NAV_GROUND_SPEED_PGAIN
206 
208 static void nav_ground_speed_loop( void ) {
209  if (MINIMUM_AIRSPEED < nav_ground_speed_setpoint
210  && nav_ground_speed_setpoint < MAXIMUM_AIRSPEED) {
211  float err = nav_ground_speed_setpoint - (*stateGetHorizontalSpeedNorm_f());
212  v_ctl_auto_throttle_cruise_throttle += nav_ground_speed_pgain*err;
213  Bound(v_ctl_auto_throttle_cruise_throttle, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
214  } else {
215  /* Reset cruise throttle to nominal value */
216  v_ctl_auto_throttle_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
217  }
218 }
219 #endif
220 
221 static float baseleg_out_qdr;
222 static inline bool_t nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius ) {
223  nav_radius = radius;
224 
225  float x_0 = waypoints[wp_td].x - waypoints[wp_af].x;
226  float y_0 = waypoints[wp_td].y - waypoints[wp_af].y;
227 
228  /* Unit vector from AF to TD */
229  float d = sqrtf(x_0*x_0+y_0*y_0);
230  float x_1 = x_0 / d;
231  float y_1 = y_0 / d;
232 
233  waypoints[wp_baseleg].x = waypoints[wp_af].x + y_1 * nav_radius;
234  waypoints[wp_baseleg].y = waypoints[wp_af].y - x_1 * nav_radius;
235  waypoints[wp_baseleg].a = waypoints[wp_af].a;
236  baseleg_out_qdr = M_PI - atan2f(-y_1, -x_1);
237  if (nav_radius < 0)
238  baseleg_out_qdr += M_PI;
239 
240  return FALSE;
241 }
242 
243 static inline bool_t nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide ) {
244 
245  float x_0 = waypoints[wp_td].x - waypoints[wp_af].x;
246  float y_0 = waypoints[wp_td].y - waypoints[wp_af].y;
247  float h_0 = waypoints[wp_td].a - waypoints[wp_af].a;
248 
249  /* Unit vector from AF to TD */
250  float d = sqrtf(x_0*x_0+y_0*y_0);
251  float x_1 = x_0 / d;
252  float y_1 = y_0 / d;
253 
254  waypoints[wp_af].x = waypoints[wp_td].x + x_1 * h_0 * glide;
255  waypoints[wp_af].y = waypoints[wp_td].y + y_1 * h_0 * glide;
256  waypoints[wp_af].a = waypoints[wp_af].a;
257 
258  return FALSE;
259 }
260 
261 
262 /* For a landing UPWIND.
263  Computes Top Of Descent waypoint from Touch Down and Approach Fix
264  waypoints, using glide airspeed, glide vertical speed and wind */
265 static inline bool_t compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed) {
266  struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
267  float td_af_x = WaypointX(_af) - WaypointX(_td);
268  float td_af_y = WaypointY(_af) - WaypointY(_td);
269  float td_af = sqrtf( td_af_x*td_af_x + td_af_y*td_af_y);
270  float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrtf(wind->x*wind->x + wind->y*wind->y));
271  WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
272  WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
273  WaypointAlt(_tod) = WaypointAlt(_af);
274  return FALSE;
275 }
276 
277 
278 #include "generated/flight_plan.h"
279 
280 
281 #ifndef LINE_START_FUNCTION
282 #define LINE_START_FUNCTION {}
283 #endif
284 #ifndef LINE_STOP_FUNCTION
285 #define LINE_STOP_FUNCTION {}
286 #endif
287 
288 
289 
290 static inline void nav_follow(uint8_t _ac_id, float _distance, float _height) {
291  struct ac_info_ * ac = get_ac_info(_ac_id);
293  NavVerticalAltitudeMode(Max(ac->alt + _height, ground_alt+SECURITY_HEIGHT), 0.);
294  float alpha = M_PI/2 - ac->course;
295  float ca = cosf(alpha), sa = sinf(alpha);
296  float x = ac->east - _distance*ca;
297  float y = ac->north - _distance*sa;
298  fly_to_xy(x, y);
299 #ifdef NAV_FOLLOW_PGAIN
300  float s = (stateGetPositionEnu_f()->x - x)*ca + (stateGetPositionEnu_f()->y - y)*sa;
301  nav_ground_speed_setpoint = ac->gspeed + NAV_FOLLOW_PGAIN*s;
302  nav_ground_speed_loop();
303 #endif
304 }
305 
306 float nav_altitude = GROUND_ALT + MIN_HEIGHT_CARROT;
309 float nav_pitch; /* Rad */
310 float fp_pitch; /* deg */
311 
312 
322 bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time) {
324  float pw_x = x - stateGetPositionEnu_f()->x;
326  float pw_y = y - stateGetPositionEnu_f()->y;
327 
328  if (approaching_time < 0.) {
329  // fly after the destination waypoint
330  float leg_x = x - from_x;
331  float leg_y = y - from_y;
332  float leg = sqrtf(Max(leg_x * leg_x + leg_y * leg_y, 1.));
333  float exceed_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f()); // negative value
334  float scal_prod = (leg_x * pw_x + leg_y * pw_y) / leg;
335  return (scal_prod < exceed_dist);
336  }
337  else {
338  // fly close enough of the waypoint or cross it
339  dist2_to_wp = pw_x*pw_x + pw_y *pw_y;
340  float min_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f());
341  if (dist2_to_wp < min_dist*min_dist) {
342  return TRUE;
343  }
344  float scal_prod = (x - from_x) * pw_x + (y - from_y) * pw_y;
345  return (scal_prod < 0.);
346  }
347 }
348 
349 
353 //static inline void fly_to_xy(float x, float y) {
354 void fly_to_xy(float x, float y) {
355  struct EnuCoor_f* pos = stateGetPositionEnu_f();
356  desired_x = x;
357  desired_y = y;
358  if (nav_mode == NAV_MODE_COURSE) {
359  h_ctl_course_setpoint = atan2f(x - pos->x, y - pos->y);
360  if (h_ctl_course_setpoint < 0.)
361  h_ctl_course_setpoint += 2 * M_PI;
363  } else {
364  float diff = atan2f(x - pos->x, y - pos->y) - (*stateGetHorizontalSpeedDir_f());
365  NormRadAngle(diff);
366  BoundAbs(diff,M_PI/2.);
367  float s = sinf(diff);
368  float speed = *stateGetHorizontalSpeedNorm_f();
369  h_ctl_roll_setpoint = atanf(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81) );
372  }
373 }
374 
378 void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) {
379  float leg_x = wp_x - last_wp_x;
380  float leg_y = wp_y - last_wp_y;
381  float leg2 = Max(leg_x * leg_x + leg_y * leg_y, 1.);
382  nav_leg_progress = ((stateGetPositionEnu_f()->x - last_wp_x) * leg_x + (stateGetPositionEnu_f()->y - last_wp_y) * leg_y) / leg2;
383  nav_leg_length = sqrtf(leg2);
384 
386  float carrot = CARROT * NOMINAL_AIRSPEED;
387 
388  nav_carrot_leg_progress = nav_leg_progress + Max(carrot / nav_leg_length, 0.);
389  nav_in_segment = TRUE;
390  nav_segment_x_1 = last_wp_x;
391  nav_segment_y_1 = last_wp_y;
392  nav_segment_x_2 = wp_x;
393  nav_segment_y_2 = wp_y;
394  horizontal_mode = HORIZONTAL_MODE_ROUTE;
395 
396  fly_to_xy(last_wp_x + nav_carrot_leg_progress*leg_x +nav_shift*leg_y/nav_leg_length, last_wp_y + nav_carrot_leg_progress*leg_y-nav_shift*leg_x/nav_leg_length);
397 }
398 
400 
401 #ifndef FAILSAFE_HOME_RADIUS
402 #define FAILSAFE_HOME_RADIUS DEFAULT_CIRCLE_RADIUS
403 #endif
404 
405 static void nav_set_altitude(void) {
406  static float last_nav_altitude;
407  if (fabs(nav_altitude - last_nav_altitude) > 1.) {
408  flight_altitude = nav_altitude;
409  last_nav_altitude = nav_altitude;
410  }
412 }
413 
415 void nav_home(void) {
418  nav_pitch = 0.;
420  nav_altitude = ground_alt+HOME_MODE_HEIGHT;
424 }
425 
430 void nav_periodic_task(void) {
431  nav_survey_active = FALSE;
432 
434  dist2_to_wp = 0.;
435 
436  auto_nav(); /* From flight_plan.h */
437 
438  h_ctl_course_pre_bank = nav_in_circle ? circle_bank : 0;
439 
440 #ifdef AGR_CLIMB
443 #endif
444 
446 }
447 
451 #if PERIODIC_TELEMETRY
453 
454 static void send_nav_ref(void) {
455  DOWNLINK_SEND_NAVIGATION_REF(DefaultChannel, DefaultDevice,
457 }
458 
459 static void send_nav(void) {
461 }
462 
463 static void send_wp_moved(void) {
464  static uint8_t i;
465  i++; if (i >= nb_waypoint) i = 0;
467 }
468 
469 static void send_circle(void) {
470  if (nav_in_circle) {
471  DOWNLINK_SEND_CIRCLE(DefaultChannel, DefaultDevice,
472  &nav_circle_x, &nav_circle_y, &nav_circle_radius);
473  }
474 }
475 
476 static void send_segment(void) {
477  if (nav_in_segment) {
478  DOWNLINK_SEND_SEGMENT(DefaultChannel, DefaultDevice,
479  &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2);
480  }
481 }
482 
483 static void send_survey(void) {
484  if (nav_survey_active) {
485  DOWNLINK_SEND_SURVEY(DefaultChannel, DefaultDevice,
486  &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south);
487  }
488 }
489 #endif
490 
494 void nav_init(void) {
495  nav_block = 0;
496  nav_stage = 0;
497  ground_alt = GROUND_ALT;
498  nav_glide_pitch_trim = NAV_GLIDE_PITCH_TRIM;
499  nav_radius = DEFAULT_CIRCLE_RADIUS;
500  nav_survey_shift = 2*DEFAULT_CIRCLE_RADIUS;
501  nav_mode = NAV_MODE_COURSE;
502 
503 #ifdef NAV_GROUND_SPEED_PGAIN
504  nav_ground_speed_pgain = ABS(NAV_GROUND_SPEED_PGAIN);
505  nav_ground_speed_setpoint = NOMINAL_AIRSPEED;
506 #endif
507 
508 #if PERIODIC_TELEMETRY
509  register_periodic_telemetry(DefaultPeriodic, "NAVIGATION_REF", send_nav_ref);
510  register_periodic_telemetry(DefaultPeriodic, "NAVIGATION", send_nav);
511  register_periodic_telemetry(DefaultPeriodic, "WP_MOVED", send_wp_moved);
512  register_periodic_telemetry(DefaultPeriodic, "CIRCLE", send_circle);
513  register_periodic_telemetry(DefaultPeriodic, "SEGMENT", send_segment);
514  register_periodic_telemetry(DefaultPeriodic, "SURVEY", send_survey);
515 #endif
516 }
517 
524 void nav_without_gps(void) {
527 
528 #ifdef SECTION_FAILSAFE
529  h_ctl_roll_setpoint = FAILSAFE_DEFAULT_ROLL;
530  nav_pitch = FAILSAFE_DEFAULT_PITCH;
531  nav_throttle_setpoint = TRIM_UPPRZ((FAILSAFE_DEFAULT_THROTTLE)*MAX_PPRZ);
532 #else
534  nav_pitch = 0;
535  nav_throttle_setpoint = TRIM_UPPRZ((V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE)*MAX_PPRZ);
536 #endif
537 }
538 
539 
540 /**************** 8 Navigation **********************************************/
541 
542 
543 enum eight_status { R1T, RT2, C2, R2T, RT1, C1 };
544 
546 void nav_eight_init( void ) {
547  eight_status = C1;
548 }
549 
558 void nav_eight(uint8_t target, uint8_t c1, float radius) {
559  float aradius = fabs(radius);
560  float alt = waypoints[target].a;
561  waypoints[c1].a = alt;
562 
563  float target_c1_x = waypoints[c1].x - waypoints[target].x;
564  float target_c1_y = waypoints[c1].y - waypoints[target].y;
565  float d = sqrtf(target_c1_x*target_c1_x+target_c1_y*target_c1_y);
566  d = Max(d, 1.); /* To prevent a division by zero */
567 
568  /* Unit vector from target to c1 */
569  float u_x = target_c1_x / d;
570  float u_y = target_c1_y / d;
571 
572  /* Move [c1] closer if needed */
573  if (d > 2 * aradius) {
574  d = 2*aradius;
575  waypoints[c1].x = waypoints[target].x + d*u_x;
576  waypoints[c1].y = waypoints[target].y + d*u_y;
577  }
578 
579  /* The other center */
580  struct point c2 = {
581  waypoints[target].x - d*u_x,
582  waypoints[target].y - d*u_y,
583  alt };
584 
585  struct point c1_in = {
586  waypoints[c1].x + radius * -u_y,
587  waypoints[c1].y + radius * u_x,
588  alt };
589  struct point c1_out = {
590  waypoints[c1].x - radius * -u_y,
591  waypoints[c1].y - radius * u_x,
592  alt };
593 
594  struct point c2_in = {
595  c2.x + radius * -u_y,
596  c2.y + radius * u_x,
597  alt };
598  struct point c2_out = {
599  c2.x - radius * -u_y,
600  c2.y - radius * u_x,
601  alt };
602 
603  float qdr_out = M_PI - atan2f(u_y, u_x);
604  if (radius < 0)
605  qdr_out += M_PI;
606 
607  switch (eight_status) {
608  case C1 :
609  NavCircleWaypoint(c1, radius);
610  if (NavQdrCloseTo(DegOfRad(qdr_out)-10)) {
611  eight_status = R1T;
612  InitStage();
613  }
614  return;
615 
616  case R1T:
617  nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y);
618  if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c1_out.x, c1_out.y, 0)) {
619  eight_status = RT2;
620  InitStage();
621  }
622  return;
623 
624  case RT2:
625  nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y);
626  if (nav_approaching_xy(c2_in.x, c2_in.y, c1_out.x, c1_out.y, CARROT)) {
627  eight_status = C2;
628  InitStage();
629  }
630  return;
631 
632  case C2 :
633  nav_circle_XY(c2.x, c2.y, -radius);
634  if (NavQdrCloseTo(DegOfRad(qdr_out)+10)) {
635  eight_status = R2T;
636  InitStage();
637  }
638  return;
639 
640  case R2T:
641  nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y);
642  if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c2_out.x, c2_out.y, 0)) {
643  eight_status = RT1;
644  InitStage();
645  }
646  return;
647 
648  case RT1:
649  nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y);
650  if (nav_approaching_xy(c1_in.x, c1_in.y, c2_out.x, c2_out.y, CARROT)) {
651  eight_status = C1;
652  InitStage();
653  }
654  return;
655 
656  default:/* Should not occur !!! Doing nothing */
657  return;
658  } /* switch */
659 }
660 
661 /************** Oval Navigation **********************************************/
662 
673 
674 void nav_oval_init( void ) {
675  oval_status = OC2;
676  nav_oval_count = 0;
677 }
678 
679 void nav_oval(uint8_t p1, uint8_t p2, float radius) {
680  radius = - radius; /* Historical error ? */
681 
682  float alt = waypoints[p1].a;
683  waypoints[p2].a = alt;
684 
685  float p2_p1_x = waypoints[p1].x - waypoints[p2].x;
686  float p2_p1_y = waypoints[p1].y - waypoints[p2].y;
687  float d = sqrtf(p2_p1_x*p2_p1_x+p2_p1_y*p2_p1_y);
688 
689  /* Unit vector from p1 to p2 */
690  float u_x = p2_p1_x / d;
691  float u_y = p2_p1_y / d;
692 
693  /* The half circle centers and the other leg */
694  struct point p1_center = { waypoints[p1].x + radius * -u_y,
695  waypoints[p1].y + radius * u_x,
696  alt };
697  struct point p1_out = { waypoints[p1].x + 2*radius * -u_y,
698  waypoints[p1].y + 2*radius * u_x,
699  alt };
700 
701  struct point p2_in = { waypoints[p2].x + 2*radius * -u_y,
702  waypoints[p2].y + 2*radius * u_x,
703  alt };
704  struct point p2_center = { waypoints[p2].x + radius * -u_y,
705  waypoints[p2].y + radius * u_x,
706  alt };
707 
708  float qdr_out_2 = M_PI - atan2f(u_y, u_x);
709  float qdr_out_1 = qdr_out_2 + M_PI;
710  if (radius < 0) {
711  qdr_out_2 += M_PI;
712  qdr_out_1 += M_PI;
713  }
714  float qdr_anticipation = (radius > 0 ? -15 : 15);
715 
716  switch (oval_status) {
717  case OC1 :
718  nav_circle_XY(p1_center.x,p1_center.y, -radius);
719  if (NavQdrCloseTo(DegOfRad(qdr_out_1)-qdr_anticipation)) {
720  oval_status = OR12;
721  InitStage();
723  }
724  return;
725 
726  case OR12:
727  nav_route_xy(p1_out.x, p1_out.y, p2_in.x, p2_in.y);
728  if (nav_approaching_xy(p2_in.x, p2_in.y, p1_out.x, p1_out.y, CARROT)) {
729  oval_status = OC2;
730  nav_oval_count++;
731  InitStage();
733  }
734  return;
735 
736  case OC2 :
737  nav_circle_XY(p2_center.x, p2_center.y, -radius);
738  if (NavQdrCloseTo(DegOfRad(qdr_out_2)-qdr_anticipation)) {
739  oval_status = OR21;
740  InitStage();
742  }
743  return;
744 
745  case OR21:
746  nav_route_xy(waypoints[p2].x, waypoints[p2].y, waypoints[p1].x, waypoints[p1].y);
747  if (nav_approaching_xy(waypoints[p1].x, waypoints[p1].y, waypoints[p2].x, waypoints[p2].y, CARROT)) {
748  oval_status = OC1;
749  InitStage();
751  }
752  return;
753 
754  default: /* Should not occur !!! Doing nothing */
755  return;
756  }
757 }
static float * stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:861
#define Min(x, y)
float v_ctl_altitude_setpoint
in meters above MSL
Definition: energy_ctrl.c:91
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
uint8_t lateral_mode
Definition: autopilot.c:49
int16_t pprz_t
Definition: paparazzi.h:6
float north
Definition: traffic_info.h:38
static float * stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:854
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
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
struct ac_info_ * get_ac_info(uint8_t id)
Definition: traffic_info.c:44
float y
in meters
float east
Definition: traffic_info.h:37
#define FALSE
Definition: imu_chimu.h:141
float alt
Definition: traffic_info.h:40
Fixed wing horizontal control.
#define V_CTL_AUTO_THROTTLE_STANDARD
#define TRIM_UPPRZ(pprz)
Definition: paparazzi.h:14
#define Max(x, y)
#define V_CTL_MODE_AUTO_ALT
int32_t nav_utm_north0
Definition: common_nav.c:43
Device independent GPS code (interface)
float x
in meters
static uint16_t c1
Definition: baro_MS5534A.c:198
float course
Definition: traffic_info.h:39
uint8_t v_ctl_auto_throttle_submode
Definition: energy_ctrl.c:79
uint8_t v_ctl_mode
Definition: energy_ctrl.c:77
#define V_CTL_MODE_AUTO_CLIMB
uint16_t stage_time
In s.
float h_ctl_roll_max_setpoint
#define TRUE
Definition: imu_chimu.h:144
uint8_t nav_stage
float h_ctl_course_pre_bank
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:106
unsigned char uint8_t
Definition: types.h:14
float h_ctl_roll_setpoint
float h_ctl_course_pgain
int32_t nav_utm_east0
Definition: common_nav.c:42
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1192
float y
Definition: common_nav.h:41
#define LATERAL_MODE_ROLL
Definition: autopilot.h:76
vector in East North Up coordinates Units: meters
int32_t y
North.
#define MAX_PPRZ
Definition: paparazzi.h:8
#define LATERAL_MODE_COURSE
Definition: autopilot.h:77
float gspeed
Definition: traffic_info.h:41
float x
Definition: common_nav.h:40
int32_t x
East.
Information relative to the other aircrafts.
Fixedwing autopilot modes.