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
nav_survey_rectangle.c
Go to the documentation of this file.
2 #include "estimator.h"
3 
4 static struct point survey_from;
5 static struct point survey_to;
6 static bool_t survey_uturn __attribute__ ((unused)) = FALSE;
8 
9 #define SurveyGoingNorth() ((survey_orientation == NS) && (survey_to.y > survey_from.y))
10 #define SurveyGoingSouth() ((survey_orientation == NS) && (survey_to.y < survey_from.y))
11 #define SurveyGoingEast() ((survey_orientation == WE) && (survey_to.x > survey_from.x))
12 #define SurveyGoingWest() ((survey_orientation == WE) && (survey_to.x < survey_from.x))
13 
14 #include "generated/flight_plan.h"
15 
16 #ifndef LINE_START_FUNCTION
17 #define LINE_START_FUNCTION {}
18 #endif
19 #ifndef LINE_STOP_FUNCTION
20 #define LINE_STOP_FUNCTION {}
21 #endif
22 
23 
25  nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
26  nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
29  survey_orientation = so;
30 
31  if (survey_orientation == NS) {
36  } else {
39  }
40  } else { /* survey_orientation == WE */
45  } else {
48  }
49  }
50  nav_survey_shift = grid;
51  survey_uturn = FALSE;
53 }
54 
55 
57  static float survey_radius;
58 
60 
61  nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
62  nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
65 
66  /* Update the current segment from corners' coordinates*/
67  if (SurveyGoingNorth()) {
70  } else if (SurveyGoingSouth()) {
73  } else if (SurveyGoingEast()) {
76  } else if (SurveyGoingWest()) {
79  }
80 
81  if (! survey_uturn) { /* S-N, N-S, W-E or E-W straight route */
86  /* Continue ... */
88  } else {
89  if (survey_orientation == NS) {
90  /* North or South limit reached, prepare U-turn and next leg */
91  float x0 = survey_from.x; /* Current longitude */
92  if (x0+nav_survey_shift < nav_survey_west || x0+nav_survey_shift > nav_survey_east) {
93  x0 += nav_survey_shift / 2;
95  }
96 
97  x0 = x0 + nav_survey_shift; /* Longitude of next leg */
98  survey_from.x = survey_to.x = x0;
99 
100  /* Swap South and North extremities */
101  float tmp = survey_from.y;
103  survey_to.y = tmp;
104 
106  waypoints[0].x = x0 - nav_survey_shift/2.;
107  waypoints[0].y = survey_from.y;
108 
109  /* Computes the right direction for the circle */
110  survey_radius = nav_survey_shift / 2.;
111  if (SurveyGoingNorth()) {
112  survey_radius = -survey_radius;
113  }
114  } else { /* (survey_orientation == WE) */
115  /* East or West limit reached, prepare U-turn and next leg */
116  /* There is a y0 declared in math.h (for ARM) !!! */
117  float my_y0 = survey_from.y; /* Current latitude */
118  if (my_y0+nav_survey_shift < nav_survey_south || my_y0+nav_survey_shift > nav_survey_north) {
119  my_y0 += nav_survey_shift / 2;
121  }
122 
123  my_y0 = my_y0 + nav_survey_shift; /* Longitude of next leg */
124  survey_from.y = survey_to.y = my_y0;
125 
126  /* Swap West and East extremities */
127  float tmp = survey_from.x;
129  survey_to.x = tmp;
130 
132  waypoints[0].x = survey_from.x;
133  waypoints[0].y = my_y0 - nav_survey_shift/2.;
134 
135  /* Computes the right direction for the circle */
136  survey_radius = nav_survey_shift / 2.;
137  if (SurveyGoingWest()) {
138  survey_radius = -survey_radius;
139  }
140  }
141 
143  survey_uturn = TRUE;
145  }
146  } else { /* U-turn */
147  if ((SurveyGoingNorth() && NavCourseCloseTo(0)) ||
148  (SurveyGoingSouth() && NavCourseCloseTo(180)) ||
149  (SurveyGoingEast() && NavCourseCloseTo(90)) ||
150  (SurveyGoingWest() && NavCourseCloseTo(270))) {
151  /* U-turn finished, back on a segment */
152  survey_uturn = FALSE;
155  } else {
156  NavCircleWaypoint(0, survey_radius);
157  }
158  }
159  NavVerticalAutoThrottleMode(0.); /* No pitch */
160  NavVerticalAltitudeMode(WaypointAlt(wp1), 0.); /* No preclimb */
161 }
#define Min(x, y)
float estimator_y
north position in meters
Definition: estimator.c:43
float estimator_hspeed_dir
direction of horizontal ground speed in rad (CW/North)
Definition: estimator.c:65
#define FALSE
Definition: imu_chimu.h:141
#define Max(x, y)
float estimator_x
east position in meters
Definition: estimator.c:42
#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 x
Definition: common_nav.h:37