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_poles.c
Go to the documentation of this file.
3 
5 float nav_poles_time = 0.;
7 
8 #define SAFETY_MARGIN 0.7
9 
13  uint8_t wp1c, uint8_t wp2c,
14  float radius) {
15  float x = WaypointX(wp2) - WaypointX(wp1);
16  float y = WaypointY(wp2) - WaypointY(wp1);
17  float d = sqrt(x*x+y*y);
18 
19  /* Unit vector from wp1 to wp2 */
20  x /= d;
21  y /= d;
22 
23  WaypointX(wp2c) = WaypointX(wp2) - (x * SAFETY_MARGIN + y) * radius;
24  WaypointY(wp2c) = WaypointY(wp2) - (y * SAFETY_MARGIN - x) * radius;
25 
26  WaypointX(wp1c) = WaypointX(wp1) + (x * SAFETY_MARGIN - y) * radius;
27  WaypointY(wp1c) = WaypointY(wp1) + (y * SAFETY_MARGIN + x) * radius;
28 
29  return false;
30 }
static float radius
Definition: chemotaxis.c:15
unsigned char uint8_t
Definition: types.h:14
signed char int8_t
Definition: types.h:15