Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
nav_survey_polygon.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2011-2013 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
28
29#include "nav_survey_polygon.h"
30
32#include "state.h"
33#include "autopilot.h"
34#include "generated/flight_plan.h"
35
36#ifdef DIGITAL_CAM
38#endif
39
41
42static void nav_points(struct FloatVect2 start, struct FloatVect2 end)
43{
44 nav_route_xy(start.x, start.y, end.x, end.y);
45}
46
55static bool intercept_two_lines(struct FloatVect2 *p, struct FloatVect2 x, struct FloatVect2 y, float a1, float a2,
56 float b1, float b2)
57{
58 float divider, fac;
59
60 divider = (((b2 - a2) * (y.x - x.x)) + ((x.y - y.y) * (b1 - a1)));
61 if (divider == 0) { return false; }
62 fac = ((y.x * (x.y - a2)) + (x.x * (a2 - y.y)) + (a1 * (y.y - x.y))) / divider;
63 if (fac > 1.0) { return false; }
64 if (fac < 0.0) { return false; }
65
66 p->x = a1 + fac * (b1 - a1);
67 p->y = a2 + fac * (b2 - a2);
68
69 return true;
70}
71
78static bool get_two_intersects(struct FloatVect2 *x, struct FloatVect2 *y, struct FloatVect2 a, struct FloatVect2 b)
79{
80 int i, count = 0;
81 struct FloatVect2 tmp;
82
83 for (i = 0; i < survey.poly_count - 1; i++)
84 if (intercept_two_lines(&tmp, a, b, waypoints[survey.poly_first + i].x, waypoints[survey.poly_first + i].y,
85 waypoints[survey.poly_first + i + 1].x, waypoints[survey.poly_first + i + 1].y)) {
86 if (count == 0) {
87 *x = tmp;
88 count++;
89 } else {
90 *y = tmp;
91 count++;
92 break;
93 }
94 }
95
96 //wrapover first,last polygon waypoint
97 if (count == 1
98 && intercept_two_lines(&tmp, a, b, waypoints[survey.poly_first + survey.poly_count - 1].x,
99 waypoints[survey.poly_first + survey.poly_count - 1].y, waypoints[survey.poly_first].x,
100 waypoints[survey.poly_first].y)) {
101 *y = tmp;
102 count++;
103 }
104
105 if (count != 2) {
106 return false;
107 }
108
109 //change points
110 if (fabs(survey.dir_vec.x) > fabs(survey.dir_vec.y)) {
111 if ((y->x - x->x) / survey.dir_vec.x < 0.0) {
112 tmp = *x;
113 *x = *y;
114 *y = tmp;
115 }
116 } else if ((y->y - x->y) / survey.dir_vec.y < 0.0) {
117 tmp = *x;
118 *x = *y;
119 *y = tmp;
120 }
121
122 return true;
123}
124
135void nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist, float min_rad, float altitude)
136{
137 int i;
138 struct FloatVect2 small, sweep;
139 float divider, angle_rad = angle / 180.0 * M_PI;
140
141 if (angle < 0.0) { angle += 360.0; }
142 if (angle >= 360.0) { angle -= 360.0; }
143
144 survey.poly_first = first_wp;
145 survey.poly_count = size;
146
147 survey.psa_sweep_width = sweep_width;
148 survey.psa_min_rad = min_rad;
149 survey.psa_shot_dist = shot_dist;
150 survey.psa_altitude = altitude;
151
152 survey.segment_angle = angle;
153 survey.return_angle = angle + 180;
154 if (survey.return_angle > 359) { survey.return_angle -= 360; }
155
156 if (angle <= 45.0 || angle >= 315.0) {
157 //north
158 survey.dir_vec.y = 1.0;
159 survey.dir_vec.x = 1.0 * tanf(angle_rad);
160 sweep.x = 1.0;
161 sweep.y = - survey.dir_vec.x / survey.dir_vec.y;
162 } else if (angle <= 135.0) {
163 //east
164 survey.dir_vec.x = 1.0;
165 survey.dir_vec.y = 1.0 / tanf(angle_rad);
166 sweep.y = - 1.0;
167 sweep.x = survey.dir_vec.y / survey.dir_vec.x;
168 } else if (angle <= 225.0) {
169 //south
170 survey.dir_vec.y = -1.0;
171 survey.dir_vec.x = -1.0 * tanf(angle_rad);
172 sweep.x = -1.0;
173 sweep.y = survey.dir_vec.x / survey.dir_vec.y;
174 } else {
175 //west
176 survey.dir_vec.x = -1.0;
177 survey.dir_vec.y = -1.0 / tanf(angle_rad);
178 sweep.y = 1.0;
179 sweep.x = - survey.dir_vec.y / survey.dir_vec.x;
180 }
181
182 //normalize
184
185 VECT2_SMUL(survey.rad_vec, sweep, survey.psa_min_rad);
186 VECT2_SMUL(survey.sweep_vec, sweep, survey.psa_sweep_width);
187
188 //begin at leftmost position (relative to survey.dir_vec)
189 VECT2_COPY(small, waypoints[survey.poly_first]);
190
191 divider = (survey.sweep_vec.y * survey.dir_vec.x) - (survey.sweep_vec.x * survey.dir_vec.y);
192
193 //calculate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right
194 if (divider < 0.0) {
195 for (i = 1; i < survey.poly_count; i++)
196 if ((survey.dir_vec.x * (waypoints[survey.poly_first + i].y - small.y)) + (survey.dir_vec.y *
197 (small.x - waypoints[survey.poly_first + i].x)) > 0.0) {
198 VECT2_COPY(small, waypoints[survey.poly_first + i]);
199 }
200 } else
201 for (i = 1; i < survey.poly_count; i++)
202 if ((survey.dir_vec.x * (waypoints[survey.poly_first + i].y - small.y)) + (survey.dir_vec.y *
203 (small.x - waypoints[survey.poly_first + i].x)) > 0.0) {
204 VECT2_COPY(small, waypoints[survey.poly_first + i]);
205 }
206
207 //calculate the line the defines the first flyover
208 survey.seg_start.x = small.x + 0.5 * survey.sweep_vec.x;
209 survey.seg_start.y = small.y + 0.5 * survey.sweep_vec.y;
210 VECT2_SUM(survey.seg_end, survey.seg_start, survey.dir_vec);
211
212 if (!get_two_intersects(&survey.seg_start, &survey.seg_end, survey.seg_start, survey.seg_end)) {
213 survey.stage = ERR;
214 return;
215 }
216
217 //center of the entry circle
218 VECT2_DIFF(survey.entry_center, survey.seg_start, survey.rad_vec);
219
220 //fast climbing to desired altitude
222 NavVerticalAltitudeMode(survey.psa_altitude, 0.0);
223
224 survey.stage = ENTRY;
225}
226
233{
235 NavVerticalAltitudeMode(survey.psa_altitude, 0.0);
236
237 //entry circle around entry-center until the desired altitude is reached
238 if (survey.stage == ENTRY) {
239 nav_circle_XY(survey.entry_center.x, survey.entry_center.y, -survey.psa_min_rad);
240 if (NavCourseCloseTo(survey.segment_angle)
241 && nav_approaching_xy(survey.seg_start.x, survey.seg_start.y, last_x, last_y, CARROT)
242 && fabs(stateGetPositionUtm_f()->alt - survey.psa_altitude) <= 20) {
243 survey.stage = SEG;
245#ifdef DIGITAL_CAM
246 dc_survey(survey.psa_shot_dist, survey.seg_start.x - survey.dir_vec.x * survey.psa_shot_dist * 0.5,
247 survey.seg_start.y - survey.dir_vec.y * survey.psa_shot_dist * 0.5);
248#endif
249 }
250 }
251 //fly the segment until seg_end is reached
252 if (survey.stage == SEG) {
253 nav_points(survey.seg_start, survey.seg_end);
254 //calculate all needed points for the next flyover
255 if (nav_approaching_xy(survey.seg_end.x, survey.seg_end.y, survey.seg_start.x, survey.seg_start.y, 0)) {
256#ifdef DIGITAL_CAM
257 dc_stop();
258#endif
259 VECT2_DIFF(survey.seg_center1, survey.seg_end, survey.rad_vec);
260 survey.ret_start.x = survey.seg_end.x - 2 * survey.rad_vec.x;
261 survey.ret_start.y = survey.seg_end.y - 2 * survey.rad_vec.y;
262
263 //if we get no intersection the survey is finished
264 static struct FloatVect2 sum_start_sweep;
265 static struct FloatVect2 sum_end_sweep;
266 VECT2_SUM(sum_start_sweep, survey.seg_start, survey.sweep_vec);
267 VECT2_SUM(sum_end_sweep, survey.seg_end, survey.sweep_vec);
268 if (!get_two_intersects(&survey.seg_start, &survey.seg_end, sum_start_sweep, sum_end_sweep)) {
269 return false;
270 }
271
272 survey.ret_end.x = survey.seg_start.x - survey.sweep_vec.x - 2 * survey.rad_vec.x;
273 survey.ret_end.y = survey.seg_start.y - survey.sweep_vec.y - 2 * survey.rad_vec.y;
274
275 survey.seg_center2.x = survey.seg_start.x - 0.5 * (2.0 * survey.rad_vec.x + survey.sweep_vec.x);
276 survey.seg_center2.y = survey.seg_start.y - 0.5 * (2.0 * survey.rad_vec.y + survey.sweep_vec.y);
277
278 survey.stage = TURN1;
280 }
281 }
282 //turn from stage to return
283 else if (survey.stage == TURN1) {
284 nav_circle_XY(survey.seg_center1.x, survey.seg_center1.y, -survey.psa_min_rad);
285 if (NavCourseCloseTo(survey.return_angle)) {
286 survey.stage = RET;
288 }
289 //return
290 } else if (survey.stage == RET) {
291 nav_points(survey.ret_start, survey.ret_end);
292 if (nav_approaching_xy(survey.ret_end.x, survey.ret_end.y, survey.ret_start.x, survey.ret_start.y, 0)) {
293 survey.stage = TURN2;
295 }
296 //turn from return to stage
297 } else if (survey.stage == TURN2) {
298 nav_circle_XY(survey.seg_center2.x, survey.seg_center2.y, -(2 * survey.psa_min_rad + survey.psa_sweep_width) * 0.5);
299 if (NavCourseCloseTo(survey.segment_angle)) {
300 survey.stage = SEG;
302#ifdef DIGITAL_CAM
303 dc_survey(survey.psa_shot_dist, survey.seg_start.x - survey.dir_vec.x * survey.psa_shot_dist * 0.5,
304 survey.seg_start.y - survey.dir_vec.y * survey.psa_shot_dist * 0.5);
305#endif
306 }
307 }
308
309 return true;
310}
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
static float p[2][2]
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
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
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition nav.c:108
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 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
void nav_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 bool 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 void nav_points(struct FloatVect2 start, struct FloatVect2 end)
struct SurveyPolyAdv survey
bool nav_survey_polygon_run(void)
main navigation routine.
Advanced polygon survey for fixedwings from Uni Stuttgart.
#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