Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
34 
35 static struct point survey_from;
36 static struct point survey_to;
37 static bool survey_uturn __attribute__((unused)) = false;
39 
40 #define SurveyGoingNorth() ((survey_orientation == NS) && (survey_to.y > survey_from.y))
41 #define SurveyGoingSouth() ((survey_orientation == NS) && (survey_to.y < survey_from.y))
42 #define SurveyGoingEast() ((survey_orientation == WE) && (survey_to.x > survey_from.x))
43 #define SurveyGoingWest() ((survey_orientation == WE) && (survey_to.x < survey_from.x))
44 
45 #include "generated/flight_plan.h"
46 
47 #ifndef LINE_START_FUNCTION
48 #define LINE_START_FUNCTION {}
49 #endif
50 #ifndef LINE_STOP_FUNCTION
51 #define LINE_STOP_FUNCTION {}
52 #endif
53 
54 
56 {
57  nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
58  nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
60  nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2));
61  survey_orientation = so;
62 
63  if (survey_orientation == NS) {
65  nav_survey_east - grid / 2.);
67  && stateGetHorizontalSpeedDir_f() > M_PI / 2. && stateGetHorizontalSpeedDir_f() < 3 * M_PI / 2)) {
70  } else {
73  }
74  } else { /* survey_orientation == WE */
76  nav_survey_north - grid / 2.);
78  && stateGetHorizontalSpeedDir_f() > M_PI)) {
81  } else {
84  }
85  }
86  nav_survey_shift = grid;
87  nav_survey_sweep = grid;
88  survey_uturn = false;
90 }
91 
93 {
94  #ifdef NAV_SURVEY_RECTANGLE_DYNAMIC
96  #endif
97 
98  static float survey_radius;
99 
100  nav_survey_active = true;
101 
102  nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
103  nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
104  nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2));
105  nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2));
106 
107  /* Update the current segment from corners' coordinates*/
108  if (SurveyGoingNorth()) {
111  } else if (SurveyGoingSouth()) {
114  } else if (SurveyGoingEast()) {
117  } else if (SurveyGoingWest()) {
120  }
121 
122  if (! survey_uturn) { /* S-N, N-S, W-E or E-W straight route */
127  /* Continue ... */
129  } else {
130  if (survey_orientation == NS) {
131  /* North or South limit reached, prepare U-turn and next leg */
132  float x0 = survey_from.x; /* Current longitude */
133  if (x0 + nav_survey_shift < nav_survey_west || x0 + nav_survey_shift > nav_survey_east) {
134  x0 += nav_survey_shift / 2;
136  }
137 
138  x0 = x0 + nav_survey_shift; /* Longitude of next leg */
139  survey_from.x = survey_to.x = x0;
140 
141  /* Swap South and North extremities */
142  float tmp = survey_from.y;
144  survey_to.y = tmp;
145 
147  waypoints[0].x = x0 - nav_survey_shift / 2.;
148  waypoints[0].y = survey_from.y;
149 
150  /* Computes the right direction for the circle */
151  survey_radius = nav_survey_shift / 2.;
152  if (SurveyGoingNorth()) {
153  survey_radius = -survey_radius;
154  }
155  } else { /* (survey_orientation == WE) */
156  /* East or West limit reached, prepare U-turn and next leg */
157  /* There is a y0 declared in math.h (for ARM) !!! */
158  float my_y0 = survey_from.y; /* Current latitude */
159  if (my_y0 + nav_survey_shift < nav_survey_south || my_y0 + nav_survey_shift > nav_survey_north) {
160  my_y0 += nav_survey_shift / 2;
162  }
163 
164  my_y0 = my_y0 + nav_survey_shift; /* Longitude of next leg */
165  survey_from.y = survey_to.y = my_y0;
166 
167  /* Swap West and East extremities */
168  float tmp = survey_from.x;
170  survey_to.x = tmp;
171 
173  waypoints[0].x = survey_from.x;
174  waypoints[0].y = my_y0 - nav_survey_shift / 2.;
175 
176  /* Computes the right direction for the circle */
177  survey_radius = nav_survey_shift / 2.;
178  if (SurveyGoingWest()) {
179  survey_radius = -survey_radius;
180  }
181  }
182 
183  nav_in_segment = false;
184  survey_uturn = true;
186  }
187  } else { /* U-turn */
188  if ((SurveyGoingNorth() && NavCourseCloseTo(0)) ||
189  (SurveyGoingSouth() && NavCourseCloseTo(180)) ||
190  (SurveyGoingEast() && NavCourseCloseTo(90)) ||
191  (SurveyGoingWest() && NavCourseCloseTo(270))) {
192  /* U-turn finished, back on a segment */
193  survey_uturn = false;
194  nav_in_circle = false;
196  } else {
197  NavCircleWaypoint(0, survey_radius);
198  }
199  }
200  NavVerticalAutoThrottleMode(0.); /* No pitch */
201  NavVerticalAltitudeMode(WaypointAlt(wp1), 0.); /* No preclimb */
202 }
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
#define WaypointX(_wp)
Definition: common_nav.h:45
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition: common_nav.h:48
float y
Definition: common_nav.h:41
float x
Definition: common_nav.h:40
#define WaypointY(_wp)
Definition: common_nav.h:46
#define Min(x, y)
Definition: esc_dshot.c:109
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
bool nav_in_segment
Definition: nav.c:67
bool nav_survey_active
Definition: nav.c:88
float nav_survey_east
Definition: nav.c:87
float nav_survey_north
Definition: nav.c:87
float nav_survey_south
Definition: nav.c:87
bool nav_in_circle
Definition: nav.c:66
void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y)
Computes the carrot position along the desired segment.
Definition: nav.c:382
float nav_survey_shift
Definition: nav.c:86
float nav_survey_west
Definition: nav.c:87
#define NavCircleWaypoint(wp, radius)
Definition: nav.h:151
#define NavCourseCloseTo(x)
Definition: nav.h:163
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition: nav.h:191
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:177
float nav_survey_sweep
static bool survey_uturn
#define LINE_STOP_FUNCTION
static survey_orientation_t survey_orientation
void nav_survey_rectangle(uint8_t wp1, uint8_t wp2)
static struct point survey_to
#define SurveyGoingNorth()
#define LINE_START_FUNCTION
#define SurveyGoingEast()
#define SurveyGoingWest()
void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so)
static struct point survey_from
#define SurveyGoingSouth()
survey_orientation_t
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98