Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
discsurvey.c
Go to the documentation of this file.
2 
3 #include "generated/airframe.h"
4 #include "estimator.h"
5 #include "std.h"
6 #include "subsystems/nav.h"
7 #include "generated/flight_plan.h"
8 #include "ap_downlink.h"
9 
11 static enum status status;
12 static int8_t sign;
13 static struct point c;
14 static struct point c1;
15 static struct point c2;
16 
17 bool_t disc_survey_init( float grid ) {
18  nav_survey_shift = grid;
19  status = DOWNWIND;
20  sign = 1;
21  c1.x = estimator_x;
22  c1.y = estimator_y;
23  return FALSE;
24 }
25 
26 bool_t disc_survey( uint8_t center, float radius) {
27  float wind_dir = atan2(wind_north, wind_east) + M_PI;
28 
30  float upwind_x = cos(wind_dir);
31  float upwind_y = sin(wind_dir);
32 
33  float grid = nav_survey_shift / 2;
34 
35  switch (status) {
36  case UTURN:
37  nav_circle_XY(c.x, c.y, grid*sign);
38  if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
39  c1.x = estimator_x;
40  c1.y = estimator_y;
41 
42  float d = ScalarProduct(upwind_x, upwind_y, estimator_x-WaypointX(center), estimator_y-WaypointY(center));
43  if (d > radius) {
44  status = DOWNWIND;
45  } else {
46  float w = sqrt(radius*radius - d*d) - 1.5*grid;
47 
48  float crosswind_x = - upwind_y;
49  float crosswind_y = upwind_x;
50 
51  c2.x = WaypointX(center)+d*upwind_x-w*sign*crosswind_x;
52  c2.y = WaypointY(center)+d*upwind_y-w*sign*crosswind_y;
53 
54  status = SEGMENT;
55  }
57  }
58  break;
59 
60  case DOWNWIND:
61  c2.x = WaypointX(center) - upwind_x * radius;
62  c2.y = WaypointY(center) - upwind_y * radius;
63  status = SEGMENT;
64  /* No break; */
65 
66  case SEGMENT:
67  nav_route_xy(c1.x, c1.y, c2.x, c2.y);
68  if (nav_approaching_xy(c2.x, c2.y, c1.x, c1.y, CARROT)) {
69  c.x = c2.x + grid*upwind_x;
70  c.y = c2.y + grid*upwind_y;
71 
72  sign = -sign;
73  status = UTURN;
75  }
76  break;
77  default:
78  break;
79  }
80 
81  NavVerticalAutoThrottleMode(0.); /* No pitch */
82  NavVerticalAltitudeMode(WaypointAlt(center), 0.); /* No preclimb */
83 
84  return TRUE;
85 }
status
Definition: anemotaxis.c:10
status
Definition: discsurvey.c:10
static float radius
Definition: chemotaxis.c:15
float estimator_y
north position in meters
Definition: estimator.c:43
#define FALSE
Definition: imu_chimu.h:141
static struct point c1
Definition: discsurvey.c:14
bool_t disc_survey(uint8_t center, float radius)
Definition: discsurvey.c:26
float wind_east
Definition: estimator.c:68
float estimator_x
east position in meters
Definition: estimator.c:42
static struct point c2
Definition: discsurvey.c:15
#define TRUE
Definition: imu_chimu.h:144
unsigned char uint8_t
Definition: types.h:14
float y
Definition: common_nav.h:38
State estimation, fusioning sensors.
float wind_north
Definition: estimator.c:68
signed char int8_t
Definition: types.h:15
static struct point c
Definition: discsurvey.c:13
float x
Definition: common_nav.h:37
bool_t disc_survey_init(float grid)
Definition: discsurvey.c:17
static int8_t sign
Definition: discsurvey.c:12