Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
29 
31 #include "autopilot.h"
32 #include "generated/flight_plan.h"
33 
34 #ifdef DIGITAL_CAM
35 #include "modules/digital_cam/dc.h"
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 
57 bool 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]);
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));
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;
114 
115  //fast climbing to desired altitude
118 
119  zs.stage = Z_ENTRY;
120 
121  return false;
122 }
123 
132 {
133  // retain altitude
136 
137  //go from center of field to end of field - (before starting the syrvey)
138  if (zs.stage == Z_ENTRY) {
141  zs.stage = Z_TURN1;
143  nav_init_stage();
144  }
145  }
146 
147  //Turn from stage to return
148  else if (zs.stage == Z_TURN1) {
151  // && nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, CARROT
152  //calculate SEG-points for the next flyover
155 
156  zs.stage = Z_RET;
157  nav_init_stage();
158 #ifdef DIGITAL_CAM
160 #endif
161  }
162  }
163 
164  //fly the segment until seg_end is reached
165  else if (zs.stage == Z_RET) {
169 #ifdef DIGITAL_CAM
170  //dc_stop();
172 #endif
173  zs.stage = Z_TURN2;
174  }
175  }
176 
177  //turn from stage to return
178  else if (zs.stage == Z_TURN2) {
181  //zs.current_laps = zs.current_laps + 1;
182  zs.stage = Z_SEG;
183  nav_init_stage();
184 #ifdef DIGITAL_CAM
186 #endif
187  }
188 
189  //return
190  } else if (zs.stage == Z_SEG) {
193 
194  // calculate the rest of the points for the next fly-over
197  zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x) / 2;
198  zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y) / 2;
201 
202  zs.stage = Z_TURN1;
203  nav_init_stage();
204 #ifdef DIGITAL_CAM
205  //dc_stop();
207 #endif
208  }
209  }
210  if (zs.current_laps >= zs.total_laps) {
211 #ifdef DIGITAL_CAM
213 #endif
214  return false;
215  } else {
216  return true;
217  }
218 }
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:85
#define FLOAT_VECT2_NORMALIZE(_v)
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:67
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:91
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:73
static int32_t altitude
Definition: airspeed_uADC.c:58
Standard Digital Camera Control Interface.
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:97
unsigned char uint8_t
Definition: types.h:14
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38