Paparazzi UAS  v5.15_devel-230-gc96ce27
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 void 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 
130 {
131  // retain altitude
134 
135  //go from center of field to end of field - (before starting the syrvey)
136  if (zs.stage == Z_ENTRY) {
139  zs.stage = Z_TURN1;
141  nav_init_stage();
142  }
143  }
144 
145  //Turn from stage to return
146  else if (zs.stage == Z_TURN1) {
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
153 
154  zs.stage = Z_RET;
155  nav_init_stage();
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) {
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) {
179  //zs.current_laps = zs.current_laps + 1;
180  zs.stage = Z_SEG;
181  nav_init_stage();
182 #ifdef DIGITAL_CAM
184 #endif
185  }
186 
187  //return
188  } else if (zs.stage == Z_SEG) {
191 
192  // calculate the rest of the points for the next fly-over
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;
199 
200  zs.stage = Z_TURN1;
201  nav_init_stage();
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 }
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:86
#define FLOAT_VECT2_NORMALIZE(_v)
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:74
static int32_t altitude
Definition: airspeed_uADC.c:58
Standard Digital Camera Control Interface.
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:98
Core autopilot interface common to all firmwares.
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
#define CARROT
default approaching_time for a wp
Definition: navigation.h:40