Paparazzi UAS  v5.8.2_stable-0-g6260b7c
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 #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)
92 {
95  stage_time = 0;
96  nav_circle_radians = 0;
97  nav_circle_radians_no_rewind = 0;
98  nav_in_circle = FALSE;
99  nav_in_segment = FALSE;
100  nav_shift = 0;
101 }
102 
103 #define PowerVoltage() (vsupply/10.)
104 #define RcRoll(travel) (fbw_state->channels[RADIO_ROLL]* (float)travel /(float)MAX_PPRZ)
105 
106 #define MIN_DX ((int16_t)(MAX_PPRZ * 0.05))
107 
108 
110 void nav_circle_XY(float x, float y, float radius)
111 {
112  struct EnuCoor_f *pos = stateGetPositionEnu_f();
113  float last_trigo_qdr = nav_circle_trigo_qdr;
114  nav_circle_trigo_qdr = atan2f(pos->y - y, pos->x - x);
115  float sign_radius = radius > 0 ? 1 : -1;
116 
117  if (nav_in_circle) {
118  float trigo_diff = nav_circle_trigo_qdr - last_trigo_qdr;
119  NormRadAngle(trigo_diff);
120  nav_circle_radians += trigo_diff;
121  trigo_diff *= - sign_radius;
122  if (trigo_diff > 0) { // do not rewind if the change in angle is in the opposite sense than nav_radius
123  nav_circle_radians_no_rewind += trigo_diff;
124  }
125  }
126 
127  float dist2_center = DistanceSquare(pos->x, pos->y, x, y);
128  float dist_carrot = CARROT * NOMINAL_AIRSPEED;
129 
130  radius += -nav_shift;
131 
132  float abs_radius = fabs(radius);
133 
135  circle_bank =
136  (dist2_center > Square(abs_radius + dist_carrot)
137  || dist2_center < Square(abs_radius - dist_carrot)) ?
138  0 :
140 
141  float carrot_angle = dist_carrot / abs_radius;
142  carrot_angle = Min(carrot_angle, M_PI / 4);
143  carrot_angle = Max(carrot_angle, M_PI / 16);
144  float alpha_carrot = nav_circle_trigo_qdr - sign_radius * carrot_angle;
145  horizontal_mode = HORIZONTAL_MODE_CIRCLE;
146  float radius_carrot = abs_radius;
147  if (nav_mode == NAV_MODE_COURSE) {
148  radius_carrot += (abs_radius / cosf(carrot_angle) - abs_radius);
149  }
150  fly_to_xy(x + cosf(alpha_carrot)*radius_carrot,
151  y + sinf(alpha_carrot)*radius_carrot);
152  nav_in_circle = TRUE;
153  nav_circle_x = x;
154  nav_circle_y = y;
155  nav_circle_radius = radius;
156 }
157 
158 
159 #define NavGlide(_last_wp, _wp) { \
160  float start_alt = waypoints[_last_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 
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 (pprz_mode == PPRZ_MODE_AUTO2) { \
176  int16_t yaw = fbw_state->channels[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 = fbw_state->channels[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 = fbw_state->channels[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 #define NavFollow(_ac_id, _distance, _height) \
201  nav_follow(_ac_id, _distance, _height);
202 
203 
204 static unit_t unit __attribute__((unused));
205 
206 static inline void nav_follow(uint8_t _ac_id, float _distance, float _height);
207 
208 #ifdef NAV_GROUND_SPEED_PGAIN
209 
211 static void nav_ground_speed_loop(void)
212 {
213  if (MINIMUM_AIRSPEED < nav_ground_speed_setpoint
214  && nav_ground_speed_setpoint < MAXIMUM_AIRSPEED) {
215  float err = nav_ground_speed_setpoint - stateGetHorizontalSpeedNorm_f();
216  v_ctl_auto_throttle_cruise_throttle += nav_ground_speed_pgain * err;
219  } else {
220  /* Reset cruise throttle to nominal value */
221  v_ctl_auto_throttle_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
222  }
223 }
224 #endif
225 
226 static float baseleg_out_qdr;
227 static inline bool_t nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius)
228 {
229  nav_radius = radius;
230 
231  float x_0 = waypoints[wp_td].x - waypoints[wp_af].x;
232  float y_0 = waypoints[wp_td].y - waypoints[wp_af].y;
233 
234  /* Unit vector from AF to TD */
235  float d = sqrtf(x_0 * x_0 + y_0 * y_0);
236  float x_1 = x_0 / d;
237  float y_1 = y_0 / d;
238 
239  waypoints[wp_baseleg].x = waypoints[wp_af].x + y_1 * nav_radius;
240  waypoints[wp_baseleg].y = waypoints[wp_af].y - x_1 * nav_radius;
241  waypoints[wp_baseleg].a = waypoints[wp_af].a;
242  baseleg_out_qdr = M_PI - atan2f(-y_1, -x_1);
243  if (nav_radius < 0) {
244  baseleg_out_qdr += M_PI;
245  }
246 
247  return FALSE;
248 }
249 
250 static inline bool_t nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide)
251 {
252 
253  float x_0 = waypoints[wp_td].x - waypoints[wp_af].x;
254  float y_0 = waypoints[wp_td].y - waypoints[wp_af].y;
255  float h_0 = waypoints[wp_td].a - waypoints[wp_af].a;
256 
257  /* Unit vector from AF to TD */
258  float d = sqrtf(x_0 * x_0 + y_0 * y_0);
259  float x_1 = x_0 / d;
260  float y_1 = y_0 / d;
261 
262  waypoints[wp_af].x = waypoints[wp_td].x + x_1 * h_0 * glide;
263  waypoints[wp_af].y = waypoints[wp_td].y + y_1 * h_0 * glide;
264  waypoints[wp_af].a = waypoints[wp_af].a;
265 
266  return FALSE;
267 }
268 
269 
270 /* For a landing UPWIND.
271  Computes Top Of Descent waypoint from Touch Down and Approach Fix
272  waypoints, using glide airspeed, glide vertical speed and wind */
273 static inline bool_t compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed)
274 {
275  struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
276  float td_af_x = WaypointX(_af) - WaypointX(_td);
277  float td_af_y = WaypointY(_af) - WaypointY(_td);
278  float td_af = sqrtf(td_af_x * td_af_x + td_af_y * td_af_y);
279  float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrtf(
280  wind->x * wind->x + wind->y * wind->y));
281  WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
282  WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
283  WaypointAlt(_tod) = WaypointAlt(_af);
284  return FALSE;
285 }
286 
287 
288 #include "generated/flight_plan.h"
289 
290 
291 #ifndef LINE_START_FUNCTION
292 #define LINE_START_FUNCTION {}
293 #endif
294 #ifndef LINE_STOP_FUNCTION
295 #define LINE_STOP_FUNCTION {}
296 #endif
297 
298 
299 
300 static inline void nav_follow(uint8_t _ac_id, float _distance, float _height)
301 {
302  struct ac_info_ * ac = get_ac_info(_ac_id);
304  NavVerticalAltitudeMode(Max(ac->alt + _height, ground_alt + SECURITY_HEIGHT), 0.);
305  float alpha = M_PI / 2 - ac->course;
306  float ca = cosf(alpha), sa = sinf(alpha);
307  float x = ac->east - _distance * ca;
308  float y = ac->north - _distance * sa;
309  fly_to_xy(x, y);
310 #ifdef NAV_FOLLOW_PGAIN
311  float s = (stateGetPositionEnu_f()->x - x) * ca + (stateGetPositionEnu_f()->y - y) * sa;
312  nav_ground_speed_setpoint = ac->gspeed + NAV_FOLLOW_PGAIN * s;
313  nav_ground_speed_loop();
314 #endif
315 }
316 
317 float nav_altitude = GROUND_ALT + MIN_HEIGHT_CARROT;
320 float nav_pitch; /* Rad */
321 float fp_pitch; /* deg */
322 float fp_throttle; /* [0-1] */
323 float fp_climb; /* m/s */
324 
325 
335 bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time)
336 {
338  float pw_x = x - stateGetPositionEnu_f()->x;
340  float pw_y = y - stateGetPositionEnu_f()->y;
341 
342  if (approaching_time < 0.) {
343  // fly after the destination waypoint
344  float leg_x = x - from_x;
345  float leg_y = y - from_y;
346  float leg = sqrtf(Max(leg_x * leg_x + leg_y * leg_y, 1.));
347  float exceed_dist = approaching_time * stateGetHorizontalSpeedNorm_f(); // negative value
348  float scal_prod = (leg_x * pw_x + leg_y * pw_y) / leg;
349  return (scal_prod < exceed_dist);
350  } else {
351  // fly close enough of the waypoint or cross it
352  dist2_to_wp = pw_x * pw_x + pw_y * pw_y;
353  float min_dist = approaching_time * stateGetHorizontalSpeedNorm_f();
354  if (dist2_to_wp < min_dist * min_dist) {
355  return TRUE;
356  }
357  float scal_prod = (x - from_x) * pw_x + (y - from_y) * pw_y;
358  return (scal_prod < 0.);
359  }
360 }
361 
362 
366 //static inline void fly_to_xy(float x, float y) {
367 void fly_to_xy(float x, float y)
368 {
369  struct EnuCoor_f *pos = stateGetPositionEnu_f();
370  desired_x = x;
371  desired_y = y;
372  if (nav_mode == NAV_MODE_COURSE) {
373  h_ctl_course_setpoint = atan2f(x - pos->x, y - pos->y);
374  if (h_ctl_course_setpoint < 0.) {
375  h_ctl_course_setpoint += 2 * M_PI;
376  }
378  } else {
379  float diff = atan2f(x - pos->x, y - pos->y) - stateGetHorizontalSpeedDir_f();
380  NormRadAngle(diff);
381  BoundAbs(diff, M_PI / 2.);
382  float s = sinf(diff);
383  float speed = stateGetHorizontalSpeedNorm_f();
384  h_ctl_roll_setpoint = atanf(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81));
387  }
388 }
389 
393 void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y)
394 {
395  float leg_x = wp_x - last_wp_x;
396  float leg_y = wp_y - last_wp_y;
397  float leg2 = Max(leg_x * leg_x + leg_y * leg_y, 1.);
398  nav_leg_progress = ((stateGetPositionEnu_f()->x - last_wp_x) * leg_x + (stateGetPositionEnu_f()->y - last_wp_y) *
399  leg_y) / leg2;
400  nav_leg_length = sqrtf(leg2);
401 
403  float carrot = CARROT * NOMINAL_AIRSPEED;
404 
405  nav_carrot_leg_progress = nav_leg_progress + Max(carrot / nav_leg_length, 0.);
406  nav_in_segment = TRUE;
407  nav_segment_x_1 = last_wp_x;
408  nav_segment_y_1 = last_wp_y;
409  nav_segment_x_2 = wp_x;
410  nav_segment_y_2 = wp_y;
411  horizontal_mode = HORIZONTAL_MODE_ROUTE;
412 
413  fly_to_xy(last_wp_x + nav_carrot_leg_progress * leg_x + nav_shift * leg_y / nav_leg_length,
414  last_wp_y + nav_carrot_leg_progress * leg_y - nav_shift * leg_x / nav_leg_length);
415 }
416 
418 
419 #ifndef FAILSAFE_HOME_RADIUS
420 #define FAILSAFE_HOME_RADIUS DEFAULT_CIRCLE_RADIUS
421 #endif
422 
423 static void nav_set_altitude(void)
424 {
425  static float last_nav_altitude;
426  if (fabs(nav_altitude - last_nav_altitude) > 1.) {
427  flight_altitude = nav_altitude;
428  last_nav_altitude = nav_altitude;
429  }
431 }
432 
434 void nav_home(void)
435 {
438  nav_pitch = 0.;
440  nav_altitude = ground_alt + HOME_MODE_HEIGHT;
444 }
445 
451 {
452  nav_survey_active = FALSE;
453 
455  dist2_to_wp = 0.;
456 
457  auto_nav(); /* From flight_plan.h */
458 
459  h_ctl_course_pre_bank = nav_in_circle ? circle_bank : 0;
460 
461 #ifdef AGR_CLIMB
464  }
465 #endif
466 
468 }
469 
473 #if PERIODIC_TELEMETRY
475 
476 static void send_nav_ref(struct transport_tx *trans, struct link_device *dev)
477 {
478  pprz_msg_send_NAVIGATION_REF(trans, dev, AC_ID,
480 }
481 
482 static void send_nav(struct transport_tx *trans, struct link_device *dev)
483 {
484  SEND_NAVIGATION(trans, dev);
485 }
486 
487 static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
488 {
489  static uint8_t i;
490  i++;
491  if (i >= nb_waypoint) { i = 0; }
492  DownlinkSendWp(trans, dev, i);
493 }
494 
495 bool_t DownlinkSendWpNr(uint8_t _wp)
496 {
497  DownlinkSendWp(&(DefaultChannel).trans_tx, &(DefaultDevice).device, _wp);
498  return FALSE;
499 }
500 
501 
502 static void send_circle(struct transport_tx *trans, struct link_device *dev)
503 {
504  if (nav_in_circle) {
505  pprz_msg_send_CIRCLE(trans, dev, AC_ID,
506  &nav_circle_x, &nav_circle_y, &nav_circle_radius);
507  }
508 }
509 
510 static void send_segment(struct transport_tx *trans, struct link_device *dev)
511 {
512  if (nav_in_segment) {
513  pprz_msg_send_SEGMENT(trans, dev, AC_ID,
514  &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2);
515  }
516 }
517 
518 static void send_survey(struct transport_tx *trans, struct link_device *dev)
519 {
520  if (nav_survey_active) {
521  pprz_msg_send_SURVEY(trans, dev, AC_ID,
522  &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south);
523  }
524 }
525 #endif
526 
530 void nav_init(void)
531 {
532  nav_block = 0;
533  nav_stage = 0;
534  ground_alt = GROUND_ALT;
535  nav_glide_pitch_trim = NAV_GLIDE_PITCH_TRIM;
536  nav_radius = DEFAULT_CIRCLE_RADIUS;
537  nav_survey_shift = 2 * DEFAULT_CIRCLE_RADIUS;
538  nav_mode = NAV_MODE_COURSE;
539 
540  fp_pitch = 0.f;
541  fp_throttle = 0.f;
542  fp_climb = 0.f;
543 
544 #ifdef NAV_GROUND_SPEED_PGAIN
545  nav_ground_speed_pgain = ABS(NAV_GROUND_SPEED_PGAIN);
546  nav_ground_speed_setpoint = NOMINAL_AIRSPEED;
547 #endif
548 
549 #if PERIODIC_TELEMETRY
550  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_NAVIGATION_REF, send_nav_ref);
551  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_NAVIGATION, send_nav);
552  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WP_MOVED, send_wp_moved);
553  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CIRCLE, send_circle);
554  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SEGMENT, send_segment);
556 #endif
557 }
558 
565 void nav_without_gps(void)
566 {
569 
570 #ifdef SECTION_FAILSAFE
571  h_ctl_roll_setpoint = FAILSAFE_DEFAULT_ROLL;
572  nav_pitch = FAILSAFE_DEFAULT_PITCH;
573  nav_throttle_setpoint = TRIM_UPPRZ((FAILSAFE_DEFAULT_THROTTLE) * MAX_PPRZ);
574 #else
576  nav_pitch = 0;
577  nav_throttle_setpoint = TRIM_UPPRZ((V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE) * MAX_PPRZ);
578 #endif
579 }
580 
581 
582 /**************** 8 Navigation **********************************************/
583 
584 
585 enum eight_status { R1T, RT2, C2, R2T, RT1, C1 };
586 
588 void nav_eight_init(void)
589 {
590  eight_status = C1;
591 }
592 
602 {
603  float aradius = fabs(radius);
604  float alt = waypoints[target].a;
605  waypoints[c1].a = alt;
606 
607  float target_c1_x = waypoints[c1].x - waypoints[target].x;
608  float target_c1_y = waypoints[c1].y - waypoints[target].y;
609  float d = sqrtf(target_c1_x * target_c1_x + target_c1_y * target_c1_y);
610  d = Max(d, 1.); /* To prevent a division by zero */
611 
612  /* Unit vector from target to c1 */
613  float u_x = target_c1_x / d;
614  float u_y = target_c1_y / d;
615 
616  /* Move [c1] closer if needed */
617  if (d > 2 * aradius) {
618  d = 2 * aradius;
619  waypoints[c1].x = waypoints[target].x + d * u_x;
620  waypoints[c1].y = waypoints[target].y + d * u_y;
621  }
622 
623  /* The other center */
624  struct point c2 = {
625  waypoints[target].x - d * u_x,
626  waypoints[target].y - d * u_y,
627  alt
628  };
629 
630  struct point c1_in = {
631  waypoints[c1].x + radius * -u_y,
632  waypoints[c1].y + radius * u_x,
633  alt
634  };
635  struct point c1_out = {
636  waypoints[c1].x - radius * -u_y,
637  waypoints[c1].y - radius * u_x,
638  alt
639  };
640 
641  struct point c2_in = {
642  c2.x + radius * -u_y,
643  c2.y + radius * u_x,
644  alt
645  };
646  struct point c2_out = {
647  c2.x - radius * -u_y,
648  c2.y - radius * u_x,
649  alt
650  };
651 
652  float qdr_out = M_PI - atan2f(u_y, u_x);
653  if (radius < 0) {
654  qdr_out += M_PI;
655  }
656 
657  switch (eight_status) {
658  case C1 :
659  NavCircleWaypoint(c1, radius);
660  if (NavQdrCloseTo(DegOfRad(qdr_out) - 10)) {
661  eight_status = R1T;
662  InitStage();
663  }
664  return;
665 
666  case R1T:
667  nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y);
668  if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c1_out.x, c1_out.y, 0)) {
669  eight_status = RT2;
670  InitStage();
671  }
672  return;
673 
674  case RT2:
675  nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y);
676  if (nav_approaching_xy(c2_in.x, c2_in.y, c1_out.x, c1_out.y, CARROT)) {
677  eight_status = C2;
678  InitStage();
679  }
680  return;
681 
682  case C2 :
683  nav_circle_XY(c2.x, c2.y, -radius);
684  if (NavQdrCloseTo(DegOfRad(qdr_out) + 10)) {
685  eight_status = R2T;
686  InitStage();
687  }
688  return;
689 
690  case R2T:
691  nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y);
692  if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c2_out.x, c2_out.y, 0)) {
693  eight_status = RT1;
694  InitStage();
695  }
696  return;
697 
698  case RT1:
699  nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y);
700  if (nav_approaching_xy(c1_in.x, c1_in.y, c2_out.x, c2_out.y, CARROT)) {
701  eight_status = C1;
702  InitStage();
703  }
704  return;
705 
706  default:/* Should not occur !!! Doing nothing */
707  return;
708  } /* switch */
709 }
710 
711 /************** Oval Navigation **********************************************/
712 
723 
724 void nav_oval_init(void)
725 {
726  oval_status = OC2;
727  nav_oval_count = 0;
728 }
729 
730 void nav_oval(uint8_t p1, uint8_t p2, float radius)
731 {
732  radius = - radius; /* Historical error ? */
733 
734  float alt = waypoints[p1].a;
735  waypoints[p2].a = alt;
736 
737  float p2_p1_x = waypoints[p1].x - waypoints[p2].x;
738  float p2_p1_y = waypoints[p1].y - waypoints[p2].y;
739  float d = sqrtf(p2_p1_x * p2_p1_x + p2_p1_y * p2_p1_y);
740 
741  /* Unit vector from p1 to p2 */
742  float u_x = p2_p1_x / d;
743  float u_y = p2_p1_y / d;
744 
745  /* The half circle centers and the other leg */
746  struct point p1_center = { waypoints[p1].x + radius * -u_y,
747  waypoints[p1].y + radius * u_x,
748  alt
749  };
750  struct point p1_out = { waypoints[p1].x + 2 * radius * -u_y,
751  waypoints[p1].y + 2 * radius * u_x,
752  alt
753  };
754 
755  struct point p2_in = { waypoints[p2].x + 2 * radius * -u_y,
756  waypoints[p2].y + 2 * radius * u_x,
757  alt
758  };
759  struct point p2_center = { waypoints[p2].x + radius * -u_y,
760  waypoints[p2].y + radius * u_x,
761  alt
762  };
763 
764  float qdr_out_2 = M_PI - atan2f(u_y, u_x);
765  float qdr_out_1 = qdr_out_2 + M_PI;
766  if (radius < 0) {
767  qdr_out_2 += M_PI;
768  qdr_out_1 += M_PI;
769  }
770  float qdr_anticipation = (radius > 0 ? -15 : 15);
771 
772  switch (oval_status) {
773  case OC1 :
774  nav_circle_XY(p1_center.x, p1_center.y, -radius);
775  if (NavQdrCloseTo(DegOfRad(qdr_out_1) - qdr_anticipation)) {
776  oval_status = OR12;
777  InitStage();
779  }
780  return;
781 
782  case OR12:
783  nav_route_xy(p1_out.x, p1_out.y, p2_in.x, p2_in.y);
784  if (nav_approaching_xy(p2_in.x, p2_in.y, p1_out.x, p1_out.y, CARROT)) {
785  oval_status = OC2;
786  nav_oval_count++;
787  InitStage();
789  }
790  return;
791 
792  case OC2 :
793  nav_circle_XY(p2_center.x, p2_center.y, -radius);
794  if (NavQdrCloseTo(DegOfRad(qdr_out_2) - qdr_anticipation)) {
795  oval_status = OR21;
796  InitStage();
798  }
799  return;
800 
801  case OR21:
802  nav_route_xy(waypoints[p2].x, waypoints[p2].y, waypoints[p1].x, waypoints[p1].y);
803  if (nav_approaching_xy(waypoints[p1].x, waypoints[p1].y, waypoints[p2].x, waypoints[p2].y, CARROT)) {
804  oval_status = OC1;
805  InitStage();
807  }
808  return;
809 
810  default: /* Should not occur !!! Doing nothing */
811  return;
812  }
813 }
Communication between fbw and ap processes.
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:912
#define WaypointAlt(_wp)
Definition: common_nav.h:47
float x
Definition: common_nav.h:40
#define Min(x, y)
float v_ctl_altitude_setpoint
in meters above MSL
Definition: energy_ctrl.c:88
Generic transmission transport header.
Definition: transport.h:89
float gspeed
Definition: traffic_info.h:41
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
float course
Definition: traffic_info.h:39
uint8_t lateral_mode
Definition: autopilot.c:50
int16_t pprz_t
Definition: paparazzi.h:6
static float radius
Definition: chemotaxis.c:15
uint8_t nav_utm_zone0
Definition: common_nav.c:44
#define InitStage()
uint8_t nav_block
float north
Definition: traffic_info.h:38
#define V_CTL_MODE_AUTO_THROTTLE
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:702
struct ac_info_ * get_ac_info(uint8_t id)
Definition: traffic_info.c:45
float v_ctl_auto_throttle_max_cruise_throttle
Definition: guidance_v.c:58
#define FALSE
Definition: std.h:5
Fixed wing horizontal control.
float alt
Definition: traffic_info.h:40
#define TRUE
Definition: std.h:4
#define V_CTL_AUTO_THROTTLE_STANDARD
float y
Definition: common_nav.h:41
#define TRIM_UPPRZ(pprz)
Definition: paparazzi.h:14
#define Max(x, y)
#define V_CTL_MODE_AUTO_ALT
float east
Definition: traffic_info.h:37
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:57
uint8_t v_ctl_mode
Definition: energy_ctrl.c:74
#define V_CTL_MODE_AUTO_CLIMB
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:69
uint8_t nav_stage
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:921
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
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
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1301
#define LATERAL_MODE_ROLL
Definition: autopilot.h:76
float a
Definition: common_nav.h:42
#define MAX_PPRZ
Definition: paparazzi.h:8
struct FloatVect2 target
#define LATERAL_MODE_COURSE
Definition: autopilot.h:77
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
Information relative to the other aircrafts.
Fixedwing autopilot modes.