Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
nav_survey_zamboni.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2013 Jorn Anke, Felix Ruess
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
27
29
31#include "autopilot.h"
32#include "generated/flight_plan.h"
33
34#ifdef DIGITAL_CAM
36#endif
37
38#ifndef LINE_START_FUNCTION
39#define LINE_START_FUNCTION {}
40#endif
41#ifndef LINE_STOP_FUNCTION
42#define LINE_STOP_FUNCTION {}
43#endif
44
46
57void nav_survey_zamboni_setup(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing,
58 int sweep_lines, float altitude)
59{
60 zs.current_laps = 0;
61 zs.pre_leave_angle = 2;
62
63 // copy variables from the flight plan
64 VECT2_COPY(zs.wp_center, waypoints[center_wp]);
65 VECT2_COPY(zs.wp_dir, waypoints[dir_wp]);
66 zs.altitude = altitude;
67
68 // if turning right leave circle before angle is reached, if turning left - leave after
69 if (sweep_spacing > 0) {
70 zs.pre_leave_angle = 0;
71 }
72
73 struct FloatVect2 flight_vec;
74 VECT2_DIFF(flight_vec, zs.wp_dir, zs.wp_center);
75 FLOAT_VECT2_NORMALIZE(flight_vec);
76
77 // calculate the flight_angle
78 zs.flight_angle = DegOfRad(atan2(flight_vec.x, flight_vec.y));
79 zs.return_angle = zs.flight_angle + 180;
80 if (zs.return_angle > 359) {
81 zs.return_angle -= 360;
82 }
83
84 // calculate the vector from one flightline perpendicular to the next flightline left,
85 // seen in the flightdirection. (Delta x and delta y betwen two adjecent flightlines)
86 // (used later to move the flight pattern one flightline up for each round)
87 zs.sweep_width.x = -flight_vec.y * sweep_spacing;
88 zs.sweep_width.y = +flight_vec.x * sweep_spacing;
89
90 // calculate number of laps to fly and turning radius for each end
91 zs.total_laps = (sweep_lines + 1) / 2;
92 zs.turnradius1 = (zs.total_laps - 1) * sweep_spacing * 0.5;
93 zs.turnradius2 = zs.total_laps * sweep_spacing * 0.5;
94
95 struct FloatVect2 flight_line;
96 VECT2_SMUL(flight_line, flight_vec, sweep_length * 0.5);
97
98 //CALCULATE THE NAVIGATION POINTS
99 //start and end of flight-line in flight-direction
100 VECT2_DIFF(zs.seg_start, zs.wp_center, flight_line);
101 VECT2_SUM(zs.seg_end, zs.wp_center, flight_line);
102
103 struct FloatVect2 sweep_span;
104 VECT2_SMUL(sweep_span, zs.sweep_width, zs.total_laps - 1);
105 //start and end of flight-line in return-direction
106 VECT2_DIFF(zs.ret_start, zs.seg_end, sweep_span);
107 VECT2_DIFF(zs.ret_end, zs.seg_start, sweep_span);
108
109 //turn-centers at both ends
110 zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x) / 2.0;
111 zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y) / 2.0;
112 zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2.0;
113 zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2.0;
114
115 //fast climbing to desired altitude
117 NavVerticalAltitudeMode(zs.altitude, 0.0);
118
119 zs.stage = Z_ENTRY;
120}
121
130{
131 // retain altitude
133 NavVerticalAltitudeMode(zs.altitude, 0.0);
134
135 //go from center of field to end of field - (before starting the syrvey)
136 if (zs.stage == Z_ENTRY) {
137 nav_route_xy(zs.wp_center.x, zs.wp_center.y, zs.seg_end.x, zs.seg_end.y);
138 if (nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.wp_center.x, zs.wp_center.y, CARROT)) {
139 zs.stage = Z_TURN1;
142 }
143 }
144
145 //Turn from stage to return
146 else if (zs.stage == Z_TURN1) {
147 nav_circle_XY(zs.turn_center1.x, zs.turn_center1.y, zs.turnradius1);
148 if (NavCourseCloseTo(zs.return_angle + zs.pre_leave_angle)) {
149 // && nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, CARROT
150 //calculate SEG-points for the next flyover
151 VECT2_ADD(zs.seg_start, zs.sweep_width);
152 VECT2_ADD(zs.seg_end, zs.sweep_width);
153
154 zs.stage = Z_RET;
156#ifdef DIGITAL_CAM
158#endif
159 }
160 }
161
162 //fly the segment until seg_end is reached
163 else if (zs.stage == Z_RET) {
164 nav_route_xy(zs.ret_start.x, zs.ret_start.y, zs.ret_end.x, zs.ret_end.y);
165 if (nav_approaching_xy(zs.ret_end.x, zs.ret_end.y, zs.ret_start.x, zs.ret_start.y, 0)) {
166 zs.current_laps = zs.current_laps + 1;
167#ifdef DIGITAL_CAM
168 //dc_stop();
170#endif
171 zs.stage = Z_TURN2;
172 }
173 }
174
175 //turn from stage to return
176 else if (zs.stage == Z_TURN2) {
177 nav_circle_XY(zs.turn_center2.x, zs.turn_center2.y, zs.turnradius2);
178 if (NavCourseCloseTo(zs.flight_angle + zs.pre_leave_angle)) {
179 //zs.current_laps = zs.current_laps + 1;
180 zs.stage = Z_SEG;
182#ifdef DIGITAL_CAM
184#endif
185 }
186
187 //return
188 } else if (zs.stage == Z_SEG) {
189 nav_route_xy(zs.seg_start.x, zs.seg_start.y, zs.seg_end.x, zs.seg_end.y);
190 if (nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, 0)) {
191
192 // calculate the rest of the points for the next fly-over
193 VECT2_ADD(zs.ret_start, zs.sweep_width);
194 VECT2_ADD(zs.ret_end, zs.sweep_width);
195 zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x) / 2;
196 zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y) / 2;
197 zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2;
198 zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2;
199
200 zs.stage = Z_TURN1;
202#ifdef DIGITAL_CAM
203 //dc_stop();
205#endif
206 }
207 }
208 if (zs.current_laps >= zs.total_laps) {
209#ifdef DIGITAL_CAM
211#endif
212 return false;
213 } else {
214 return true;
215 }
216}
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
Standard Digital Camera Control Interface.
#define FLOAT_VECT2_NORMALIZE(_v)
#define VECT2_ADD(_a, _b)
#define VECT2_SMUL(_vo, _vi, _s)
#define VECT2_DIFF(_c, _a, _b)
#define VECT2_COPY(_a, _b)
#define VECT2_SUM(_c, _a, _b)
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
#define LINE_STOP_FUNCTION
Definition nav.c:279
#define LINE_START_FUNCTION
Definition nav.c:276
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
bool nav_survey_zamboni_run(void)
main navigation routine.
void nav_survey_zamboni_setup(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude)
initializes the variables needed for the survey to start.
struct ZamboniSurvey zs
Zamboni pattern survey for fixedwings.
@ Z_ENTRY
@ Z_TURN1
@ Z_TURN2
#define CARROT
default approaching_time for a wp
Definition navigation.h:70
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.