Paparazzi UAS  v5.15_devel-113-g1b57ff1
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  survey_uturn = false;
87 }
88 
89 
91 {
92  static float survey_radius;
93 
94  nav_survey_active = true;
95 
96  nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
97  nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
100 
101  /* Update the current segment from corners' coordinates*/
102  if (SurveyGoingNorth()) {
105  } else if (SurveyGoingSouth()) {
108  } else if (SurveyGoingEast()) {
111  } else if (SurveyGoingWest()) {
114  }
115 
116  if (! survey_uturn) { /* S-N, N-S, W-E or E-W straight route */
121  /* Continue ... */
123  } else {
124  if (survey_orientation == NS) {
125  /* North or South limit reached, prepare U-turn and next leg */
126  float x0 = survey_from.x; /* Current longitude */
127  if (x0 + nav_survey_shift < nav_survey_west || x0 + nav_survey_shift > nav_survey_east) {
128  x0 += nav_survey_shift / 2;
130  }
131 
132  x0 = x0 + nav_survey_shift; /* Longitude of next leg */
133  survey_from.x = survey_to.x = x0;
134 
135  /* Swap South and North extremities */
136  float tmp = survey_from.y;
138  survey_to.y = tmp;
139 
141  waypoints[0].x = x0 - nav_survey_shift / 2.;
142  waypoints[0].y = survey_from.y;
143 
144  /* Computes the right direction for the circle */
145  survey_radius = nav_survey_shift / 2.;
146  if (SurveyGoingNorth()) {
147  survey_radius = -survey_radius;
148  }
149  } else { /* (survey_orientation == WE) */
150  /* East or West limit reached, prepare U-turn and next leg */
151  /* There is a y0 declared in math.h (for ARM) !!! */
152  float my_y0 = survey_from.y; /* Current latitude */
153  if (my_y0 + nav_survey_shift < nav_survey_south || my_y0 + nav_survey_shift > nav_survey_north) {
154  my_y0 += nav_survey_shift / 2;
156  }
157 
158  my_y0 = my_y0 + nav_survey_shift; /* Longitude of next leg */
159  survey_from.y = survey_to.y = my_y0;
160 
161  /* Swap West and East extremities */
162  float tmp = survey_from.x;
164  survey_to.x = tmp;
165 
167  waypoints[0].x = survey_from.x;
168  waypoints[0].y = my_y0 - nav_survey_shift / 2.;
169 
170  /* Computes the right direction for the circle */
171  survey_radius = nav_survey_shift / 2.;
172  if (SurveyGoingWest()) {
173  survey_radius = -survey_radius;
174  }
175  }
176 
177  nav_in_segment = false;
178  survey_uturn = true;
180  }
181  } else { /* U-turn */
182  if ((SurveyGoingNorth() && NavCourseCloseTo(0)) ||
183  (SurveyGoingSouth() && NavCourseCloseTo(180)) ||
184  (SurveyGoingEast() && NavCourseCloseTo(90)) ||
185  (SurveyGoingWest() && NavCourseCloseTo(270))) {
186  /* U-turn finished, back on a segment */
187  survey_uturn = false;
188  nav_in_circle = false;
190  } else {
191  NavCircleWaypoint(0, survey_radius);
192  }
193  }
194  NavVerticalAutoThrottleMode(0.); /* No pitch */
195  NavVerticalAltitudeMode(WaypointAlt(wp1), 0.); /* No preclimb */
196 }
#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