Paparazzi UAS  v5.15_devel-164-g81d4ceb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nav_survey_rectangle.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2007-2009 ENAC, 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 
31 #include "state.h"
32 
33 static struct point survey_from;
34 static struct point survey_to;
35 static bool survey_uturn __attribute__((unused)) = false;
37 
38 #define SurveyGoingNorth() ((survey_orientation == NS) && (survey_to.y > survey_from.y))
39 #define SurveyGoingSouth() ((survey_orientation == NS) && (survey_to.y < survey_from.y))
40 #define SurveyGoingEast() ((survey_orientation == WE) && (survey_to.x > survey_from.x))
41 #define SurveyGoingWest() ((survey_orientation == WE) && (survey_to.x < survey_from.x))
42 
43 #include "generated/flight_plan.h"
44 
45 #ifndef LINE_START_FUNCTION
46 #define LINE_START_FUNCTION {}
47 #endif
48 #ifndef LINE_STOP_FUNCTION
49 #define LINE_STOP_FUNCTION {}
50 #endif
51 
52 
54 {
55  nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
56  nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
59  survey_orientation = so;
60 
61  if (survey_orientation == NS) {
63  nav_survey_east - grid / 2.);
65  && stateGetHorizontalSpeedDir_f() > M_PI / 2. && stateGetHorizontalSpeedDir_f() < 3 * M_PI / 2)) {
68  } else {
71  }
72  } else { /* survey_orientation == WE */
74  nav_survey_north - grid / 2.);
76  && stateGetHorizontalSpeedDir_f() > M_PI)) {
79  } else {
82  }
83  }
84  nav_survey_shift = grid;
85  sweep_var = grid;
86  survey_uturn = false;
88 }
89 
91 {
92  #ifdef NAV_SURVEY_RECTANGLE_DYNAMIC
94  #endif
95 
96  static float survey_radius;
97 
98  nav_survey_active = true;
99 
100  nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
101  nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
102  nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2));
103  nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2));
104 
105  /* Update the current segment from corners' coordinates*/
106  if (SurveyGoingNorth()) {
109  } else if (SurveyGoingSouth()) {
112  } else if (SurveyGoingEast()) {
115  } else if (SurveyGoingWest()) {
118  }
119 
120  if (! survey_uturn) { /* S-N, N-S, W-E or E-W straight route */
125  /* Continue ... */
127  } else {
128  if (survey_orientation == NS) {
129  /* North or South limit reached, prepare U-turn and next leg */
130  float x0 = survey_from.x; /* Current longitude */
131  if (x0 + nav_survey_shift < nav_survey_west || x0 + nav_survey_shift > nav_survey_east) {
132  x0 += nav_survey_shift / 2;
134  }
135 
136  x0 = x0 + nav_survey_shift; /* Longitude of next leg */
137  survey_from.x = survey_to.x = x0;
138 
139  /* Swap South and North extremities */
140  float tmp = survey_from.y;
142  survey_to.y = tmp;
143 
145  waypoints[0].x = x0 - nav_survey_shift / 2.;
146  waypoints[0].y = survey_from.y;
147 
148  /* Computes the right direction for the circle */
149  survey_radius = nav_survey_shift / 2.;
150  if (SurveyGoingNorth()) {
151  survey_radius = -survey_radius;
152  }
153  } else { /* (survey_orientation == WE) */
154  /* East or West limit reached, prepare U-turn and next leg */
155  /* There is a y0 declared in math.h (for ARM) !!! */
156  float my_y0 = survey_from.y; /* Current latitude */
157  if (my_y0 + nav_survey_shift < nav_survey_south || my_y0 + nav_survey_shift > nav_survey_north) {
158  my_y0 += nav_survey_shift / 2;
160  }
161 
162  my_y0 = my_y0 + nav_survey_shift; /* Longitude of next leg */
163  survey_from.y = survey_to.y = my_y0;
164 
165  /* Swap West and East extremities */
166  float tmp = survey_from.x;
168  survey_to.x = tmp;
169 
171  waypoints[0].x = survey_from.x;
172  waypoints[0].y = my_y0 - nav_survey_shift / 2.;
173 
174  /* Computes the right direction for the circle */
175  survey_radius = nav_survey_shift / 2.;
176  if (SurveyGoingWest()) {
177  survey_radius = -survey_radius;
178  }
179  }
180 
181  nav_in_segment = false;
182  survey_uturn = true;
184  }
185  } else { /* U-turn */
186  if ((SurveyGoingNorth() && NavCourseCloseTo(0)) ||
187  (SurveyGoingSouth() && NavCourseCloseTo(180)) ||
188  (SurveyGoingEast() && NavCourseCloseTo(90)) ||
189  (SurveyGoingWest() && NavCourseCloseTo(270))) {
190  /* U-turn finished, back on a segment */
191  survey_uturn = false;
192  nav_in_circle = false;
194  } else {
195  NavCircleWaypoint(0, survey_radius);
196  }
197  }
198  NavVerticalAutoThrottleMode(0.); /* No pitch */
199  NavVerticalAltitudeMode(WaypointAlt(wp1), 0.); /* No preclimb */
200 }
#define Min(x, y)
Definition: esc_dshot.c:85
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition: common_nav.h:48
float x
Definition: common_nav.h:40
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
float y
Definition: common_nav.h:41
#define WaypointX(_wp)
Definition: common_nav.h:45
#define Max(x, y)
Definition: main_fbw.c:53
#define WaypointY(_wp)
Definition: common_nav.h:46
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38