Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
nav_survey_polygon_gvf.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2017 The Paparazzi Team
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
29
31
33#include "state.h"
34#include "autopilot.h"
35#include "generated/flight_plan.h"
37
38#ifdef DIGITAL_CAM
40#endif
41
43
45
46static void gvf_circle_direction(float rad)
47{
48 if (rad > 0) {
50 } else {
52 }
53}
54
63static bool gvf_intercept_two_lines(struct FloatVect2 *p, struct FloatVect2 x, struct FloatVect2 y, float a1, float a2,
64 float b1, float b2)
65{
66 float divider, fac;
67
68 divider = (((b2 - a2) * (y.x - x.x)) + ((x.y - y.y) * (b1 - a1)));
69 if (divider == 0) { return false; }
70 fac = ((y.x * (x.y - a2)) + (x.x * (a2 - y.y)) + (a1 * (y.y - x.y))) / divider;
71 if (fac > 1.0) { return false; }
72 if (fac < 0.0) { return false; }
73
74 p->x = a1 + fac * (b1 - a1);
75 p->y = a2 + fac * (b2 - a2);
76
77 return true;
78}
79
86static bool gvf_get_two_intersects(struct FloatVect2 *x, struct FloatVect2 *y, struct FloatVect2 a, struct FloatVect2 b)
87{
88 int i, count = 0;
89 struct FloatVect2 tmp;
90
91 for (i = 0; i < gvf_survey.poly_count - 1; i++)
92 if (gvf_intercept_two_lines(&tmp, a, b, waypoints[gvf_survey.poly_first + i].x, waypoints[gvf_survey.poly_first + i].y,
93 waypoints[gvf_survey.poly_first + i + 1].x, waypoints[gvf_survey.poly_first + i + 1].y)) {
94 if (count == 0) {
95 *x = tmp;
96 count++;
97 } else {
98 *y = tmp;
99 count++;
100 break;
101 }
102 }
103
104 //wrapover first,last polygon waypoint
105 if (count == 1
106 && gvf_intercept_two_lines(&tmp, a, b, waypoints[gvf_survey.poly_first + gvf_survey.poly_count - 1].x,
107 waypoints[gvf_survey.poly_first + gvf_survey.poly_count - 1].y, waypoints[gvf_survey.poly_first].x,
108 waypoints[gvf_survey.poly_first].y)) {
109 *y = tmp;
110 count++;
111 }
112
113 if (count != 2) {
114 return false;
115 }
116
117 //change points
118 if (fabs(gvf_survey.dir_vec.x) > fabs(gvf_survey.dir_vec.y)) {
119 if ((y->x - x->x) / gvf_survey.dir_vec.x < 0.0) {
120 tmp = *x;
121 *x = *y;
122 *y = tmp;
123 }
124 } else if ((y->y - x->y) / gvf_survey.dir_vec.y < 0.0) {
125 tmp = *x;
126 *x = *y;
127 *y = tmp;
128 }
129
130 return true;
131}
132
134
145void nav_gvf_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist,
146 float min_rad, float altitude)
147{
148 int i;
149 struct FloatVect2 small, sweep;
150 float divider, angle_rad = angle / 180.0 * M_PI;
151
152 if (angle < 0.0) { angle += 360.0; }
153 if (angle >= 360.0) { angle -= 360.0; }
154
155 gvf_survey.poly_first = first_wp;
156 gvf_survey.poly_count = size;
157
158 gvf_survey.psa_sweep_width = sweep_width;
159 gvf_survey.psa_min_rad = min_rad;
160 gvf_survey.psa_shot_dist = shot_dist;
161 gvf_survey.psa_altitude = altitude;
162
163 gvf_survey.segment_angle = angle;
164 gvf_survey.return_angle = angle + 180;
165 if (gvf_survey.return_angle > 359) { gvf_survey.return_angle -= 360; }
166
167 if (angle <= 45.0 || angle >= 315.0) {
168 //north
169 gvf_survey.dir_vec.y = 1.0;
170 gvf_survey.dir_vec.x = 1.0 * tanf(angle_rad);
171 sweep.x = 1.0;
172 sweep.y = - gvf_survey.dir_vec.x / gvf_survey.dir_vec.y;
173 } else if (angle <= 135.0) {
174 //east
175 gvf_survey.dir_vec.x = 1.0;
176 gvf_survey.dir_vec.y = 1.0 / tanf(angle_rad);
177 sweep.y = -1.0;
178 sweep.x = gvf_survey.dir_vec.y / gvf_survey.dir_vec.x;
179 } else if (angle <= 225.0) {
180 //south
181 gvf_survey.dir_vec.y = -1.0;
182 gvf_survey.dir_vec.x = -1.0 * tanf(angle_rad);
183 sweep.x = -1.0;
184 sweep.y = gvf_survey.dir_vec.x / gvf_survey.dir_vec.y;
185 } else {
186 //west
187 gvf_survey.dir_vec.x = -1.0;
188 gvf_survey.dir_vec.y = -1.0 / tanf(angle_rad);
189 sweep.y = 1.0;
190 sweep.x = - gvf_survey.dir_vec.y / gvf_survey.dir_vec.x;
191 }
192
193 //normalize
195
196 VECT2_SMUL(gvf_survey.rad_vec, sweep, gvf_survey.psa_min_rad);
197 VECT2_SMUL(gvf_survey.sweep_vec, sweep, gvf_survey.psa_sweep_width);
198
199 //begin at leftmost position (relative to gvf_survey.dir_vec)
200 VECT2_COPY(small, waypoints[gvf_survey.poly_first]);
201
202 divider = (gvf_survey.sweep_vec.y * gvf_survey.dir_vec.x) - (gvf_survey.sweep_vec.x * gvf_survey.dir_vec.y);
203
204 //calculate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right
205 if (divider < 0.0) {
206 for (i = 1; i < gvf_survey.poly_count; i++) {
207 if ((gvf_survey.dir_vec.x * (waypoints[gvf_survey.poly_first + i].y - small.y)) + (gvf_survey.dir_vec.y *
208 (small.x - waypoints[gvf_survey.poly_first + i].x)) > 0.0) {
209 VECT2_COPY(small, waypoints[gvf_survey.poly_first + i]);
210 }
211 }
212 } else {
213 for (i = 1; i < gvf_survey.poly_count; i++) {
214 if ((gvf_survey.dir_vec.x * (waypoints[gvf_survey.poly_first + i].y - small.y)) + (gvf_survey.dir_vec.y *
215 (small.x - waypoints[gvf_survey.poly_first + i].x)) > 0.0) {
216 VECT2_COPY(small, waypoints[gvf_survey.poly_first + i]);
217 }
218 }
219 }
220
221 //calculate the line the defines the first flyover
222 gvf_survey.seg_start.x = small.x + 0.5 * gvf_survey.sweep_vec.x;
223 gvf_survey.seg_start.y = small.y + 0.5 * gvf_survey.sweep_vec.y;
224 VECT2_SUM(gvf_survey.seg_end, gvf_survey.seg_start, gvf_survey.dir_vec);
225
226 if (!gvf_get_two_intersects(&gvf_survey.seg_start, &gvf_survey.seg_end, gvf_survey.seg_start, gvf_survey.seg_end)) {
227 gvf_survey.stage = gERR;
228 return;
229 }
230
231 //center of the entry circle
232 VECT2_DIFF(gvf_survey.entry_center, gvf_survey.seg_start, gvf_survey.rad_vec);
233
234 //fast climbing to desired altitude
236 NavVerticalAltitudeMode(gvf_survey.psa_altitude, 0.0);
237
238 gvf_survey.stage = gENTRY;
239}
240
247{
249 NavVerticalAltitudeMode(gvf_survey.psa_altitude, 0.0);
250
251 //entry circle around entry-center until the desired altitude is reached
252 if (gvf_survey.stage == gENTRY) {
253 gvf_circle_direction(gvf_survey.psa_min_rad);
254 nav_gvf_ellipse_XY(gvf_survey.entry_center.x, gvf_survey.entry_center.y, gvf_survey.psa_min_rad, gvf_survey.psa_min_rad, 0);
255 if (NavCourseCloseTo(gvf_survey.segment_angle)
256 && nav_approaching_xy(gvf_survey.seg_start.x, gvf_survey.seg_start.y, last_x, last_y, CARROT)
257 && fabs(stateGetPositionUtm_f()->alt - gvf_survey.psa_altitude) <= 20) {
258 gvf_survey.stage = gSEG;
260#ifdef DIGITAL_CAM
261 dc_survey(gvf_survey.psa_shot_dist, gvf_survey.seg_start.x - gvf_survey.dir_vec.x * gvf_survey.psa_shot_dist * 0.5,
262 gvf_survey.seg_start.y - gvf_survey.dir_vec.y * gvf_survey.psa_shot_dist * 0.5);
263#endif
264 }
265 }
266 //fly the segment until seg_end is reached
267 if (gvf_survey.stage == gSEG) {
268 nav_gvf_segment_points(gvf_survey.seg_start, gvf_survey.seg_end);
269 //calculate all needed points for the next flyover
270 if (nav_approaching_xy(gvf_survey.seg_end.x, gvf_survey.seg_end.y, gvf_survey.seg_start.x, gvf_survey.seg_start.y, 0)) {
271#ifdef DIGITAL_CAM
272 dc_stop();
273#endif
274 VECT2_DIFF(gvf_survey.seg_center1, gvf_survey.seg_end, gvf_survey.rad_vec);
275 gvf_survey.ret_start.x = gvf_survey.seg_end.x - 2 * gvf_survey.rad_vec.x;
276 gvf_survey.ret_start.y = gvf_survey.seg_end.y - 2 * gvf_survey.rad_vec.y;
277
278 //if we get no intersection the survey is finished
279 static struct FloatVect2 sum_start_sweep;
280 static struct FloatVect2 sum_end_sweep;
281 VECT2_SUM(sum_start_sweep, gvf_survey.seg_start, gvf_survey.sweep_vec);
282 VECT2_SUM(sum_end_sweep, gvf_survey.seg_end, gvf_survey.sweep_vec);
283 if (!gvf_get_two_intersects(&gvf_survey.seg_start, &gvf_survey.seg_end, sum_start_sweep, sum_end_sweep)) {
284 return false;
285 }
286
287 gvf_survey.ret_end.x = gvf_survey.seg_start.x - gvf_survey.sweep_vec.x - 2 * gvf_survey.rad_vec.x;
288 gvf_survey.ret_end.y = gvf_survey.seg_start.y - gvf_survey.sweep_vec.y - 2 * gvf_survey.rad_vec.y;
289
290 gvf_survey.seg_center2.x = gvf_survey.seg_start.x - 0.5 * (2.0 * gvf_survey.rad_vec.x + gvf_survey.sweep_vec.x);
291 gvf_survey.seg_center2.y = gvf_survey.seg_start.y - 0.5 * (2.0 * gvf_survey.rad_vec.y + gvf_survey.sweep_vec.y);
292
293 gvf_survey.stage = gTURN1;
295 }
296 }
297 //turn from stage to return
298 else if (gvf_survey.stage == gTURN1) {
299 gvf_circle_direction(gvf_survey.psa_min_rad);
300 nav_gvf_ellipse_XY(gvf_survey.seg_center1.x, gvf_survey.seg_center1.y, gvf_survey.psa_min_rad, gvf_survey.psa_min_rad, 0);
301 if (NavCourseCloseTo(gvf_survey.return_angle)) {
302 gvf_survey.stage = gRET;
304 }
305 //return
306 } else if (gvf_survey.stage == gRET) {
307 nav_gvf_segment_points(gvf_survey.ret_start, gvf_survey.ret_end);
308 if (nav_approaching_xy(gvf_survey.ret_end.x, gvf_survey.ret_end.y, gvf_survey.ret_start.x, gvf_survey.ret_start.y, 0)) {
309 gvf_survey.stage = gTURN2;
311 }
312 //turn from return to stage
313 } else if (gvf_survey.stage == gTURN2) {
314 float rad_sur = (2 * gvf_survey.psa_min_rad + gvf_survey.psa_sweep_width) * 0.5;
315 gvf_circle_direction(rad_sur);
316 nav_gvf_ellipse_XY(gvf_survey.seg_center2.x, gvf_survey.seg_center2.y, rad_sur, rad_sur, 0);
317 if (NavCourseCloseTo(gvf_survey.segment_angle)) {
318 gvf_survey.stage = gSEG;
320#ifdef DIGITAL_CAM
321 dc_survey(gvf_survey.psa_shot_dist, gvf_survey.seg_start.x - gvf_survey.dir_vec.x * gvf_survey.psa_shot_dist * 0.5,
322 gvf_survey.seg_start.y - gvf_survey.dir_vec.y * gvf_survey.psa_shot_dist * 0.5);
323#endif
324 }
325 }
326
327 return true;
328}
static int32_t altitude
Core autopilot interface common to all firmwares.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition common_nav.c:44
uint8_t dc_survey(float interval, float x, float y)
Sets the dc control in distance mode.
Definition dc.c:240
uint8_t dc_stop(void)
Stop dc control.
Definition dc.c:262
Standard Digital Camera Control Interface.
#define FLOAT_VECT2_NORMALIZE(_v)
#define VECT2_SMUL(_vo, _vi, _s)
#define VECT2_DIFF(_c, _a, _b)
#define VECT2_COPY(_a, _b)
#define VECT2_SUM(_c, _a, _b)
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition state.h:832
bool nav_gvf_segment_points(struct FloatVect2 start, struct FloatVect2 end)
Definition nav_line.c:253
void gvf_set_direction(int8_t s)
Definition gvf.c:232
Guidance algorithm based on vector fields.
static float p[2][2]
bool nav_gvf_ellipse_XY(float x, float y, float a, float b, float alpha)
Definition nav_ellipse.c:64
bool nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time)
Decide if the UAV is approaching the current waypoint.
Definition nav.c:325
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition nav.c:92
float last_y
Definition nav.c:47
float last_x
Definition nav.c:47
Fixedwing Navigation library.
#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
static bool gvf_intercept_two_lines(struct FloatVect2 *p, struct FloatVect2 x, struct FloatVect2 y, float a1, float a2, float b1, float b2)
Intercept two lines and give back the point of intersection.
static bool gvf_get_two_intersects(struct FloatVect2 *x, struct FloatVect2 *y, struct FloatVect2 a, struct FloatVect2 b)
Intersects a line with the polygon and gives back the two intersection points.
bool nav_gvf_survey_polygon_run(void)
Main navigation routine.
struct gvf_SurveyPolyAdv gvf_survey
void nav_gvf_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist, float min_rad, float altitude)
Initializes the variables needed for the survey to start.
static void gvf_circle_direction(float rad)
#define CARROT
default approaching_time for a wp
Definition navigation.h:70
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
float b
Definition wedgebug.c:202