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_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_t 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  nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
55  nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
58  survey_orientation = so;
59 
60  if (survey_orientation == NS) {
65  } else {
68  }
69  } else { /* survey_orientation == WE */
74  } else {
77  }
78  }
79  nav_survey_shift = grid;
82 }
83 
84 
86  static float survey_radius;
87 
89 
90  nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
91  nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
94 
95  /* Update the current segment from corners' coordinates*/
96  if (SurveyGoingNorth()) {
99  } else if (SurveyGoingSouth()) {
102  } else if (SurveyGoingEast()) {
105  } else if (SurveyGoingWest()) {
108  }
109 
110  if (! survey_uturn) { /* S-N, N-S, W-E or E-W straight route */
115  /* Continue ... */
117  } else {
118  if (survey_orientation == NS) {
119  /* North or South limit reached, prepare U-turn and next leg */
120  float x0 = survey_from.x; /* Current longitude */
121  if (x0+nav_survey_shift < nav_survey_west || x0+nav_survey_shift > nav_survey_east) {
122  x0 += nav_survey_shift / 2;
124  }
125 
126  x0 = x0 + nav_survey_shift; /* Longitude of next leg */
127  survey_from.x = survey_to.x = x0;
128 
129  /* Swap South and North extremities */
130  float tmp = survey_from.y;
132  survey_to.y = tmp;
133 
135  waypoints[0].x = x0 - nav_survey_shift/2.;
136  waypoints[0].y = survey_from.y;
137 
138  /* Computes the right direction for the circle */
139  survey_radius = nav_survey_shift / 2.;
140  if (SurveyGoingNorth()) {
141  survey_radius = -survey_radius;
142  }
143  } else { /* (survey_orientation == WE) */
144  /* East or West limit reached, prepare U-turn and next leg */
145  /* There is a y0 declared in math.h (for ARM) !!! */
146  float my_y0 = survey_from.y; /* Current latitude */
147  if (my_y0+nav_survey_shift < nav_survey_south || my_y0+nav_survey_shift > nav_survey_north) {
148  my_y0 += nav_survey_shift / 2;
150  }
151 
152  my_y0 = my_y0 + nav_survey_shift; /* Longitude of next leg */
153  survey_from.y = survey_to.y = my_y0;
154 
155  /* Swap West and East extremities */
156  float tmp = survey_from.x;
158  survey_to.x = tmp;
159 
161  waypoints[0].x = survey_from.x;
162  waypoints[0].y = my_y0 - nav_survey_shift/2.;
163 
164  /* Computes the right direction for the circle */
165  survey_radius = nav_survey_shift / 2.;
166  if (SurveyGoingWest()) {
167  survey_radius = -survey_radius;
168  }
169  }
170 
172  survey_uturn = TRUE;
174  }
175  } else { /* U-turn */
176  if ((SurveyGoingNorth() && NavCourseCloseTo(0)) ||
177  (SurveyGoingSouth() && NavCourseCloseTo(180)) ||
178  (SurveyGoingEast() && NavCourseCloseTo(90)) ||
179  (SurveyGoingWest() && NavCourseCloseTo(270))) {
180  /* U-turn finished, back on a segment */
184  } else {
185  NavCircleWaypoint(0, survey_radius);
186  }
187  }
188  NavVerticalAutoThrottleMode(0.); /* No pitch */
189  NavVerticalAltitudeMode(WaypointAlt(wp1), 0.); /* No preclimb */
190 }
static float * stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:861
#define Min(x, y)
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
#define FALSE
Definition: imu_chimu.h:141
#define Max(x, y)
#define TRUE
Definition: imu_chimu.h:144
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
float y
Definition: common_nav.h:41
int32_t y
North.
float x
Definition: common_nav.h:40
int32_t x
East.