Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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_t nav_survey_zamboni_setup(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude)
58 {
59  zs.current_laps = 0;
60  zs.pre_leave_angle = 2;
61 
62  // copy variables from the flight plan
63  VECT2_COPY(zs.wp_center, waypoints[center_wp]);
64  VECT2_COPY(zs.wp_dir, waypoints[dir_wp]);
66 
67  // if turning right leave circle before angle is reached, if turning left - leave after
68  if (sweep_spacing > 0) {
70  }
71 
72  struct FloatVect2 flight_vec;
73  VECT2_DIFF(flight_vec, zs.wp_dir, zs.wp_center);
74  FLOAT_VECT2_NORMALIZE(flight_vec);
75 
76  // calculate the flight_angle
77  zs.flight_angle = DegOfRad(atan2(flight_vec.x, flight_vec.y));
79  if (zs.return_angle > 359) {
80  zs.return_angle -= 360;
81  }
82 
83  // calculate the vector from one flightline perpendicular to the next flightline left,
84  // seen in the flightdirection. (Delta x and delta y betwen two adjecent flightlines)
85  // (used later to move the flight pattern one flightline up for each round)
86  zs.sweep_width.x = -flight_vec.y * sweep_spacing;
87  zs.sweep_width.y = +flight_vec.x * sweep_spacing;
88 
89  // calculate number of laps to fly and turning radius for each end
90  zs.total_laps = (sweep_lines+1)/2;
91  zs.turnradius1 = (zs.total_laps-1) * sweep_spacing * 0.5;
92  zs.turnradius2 = zs.total_laps * sweep_spacing * 0.5;
93 
94  struct FloatVect2 flight_line;
95  VECT2_SMUL(flight_line, flight_vec, sweep_length * 0.5);
96 
97  //CALCULATE THE NAVIGATION POINTS
98  //start and end of flight-line in flight-direction
99  VECT2_DIFF(zs.seg_start, zs.wp_center, flight_line);
100  VECT2_SUM(zs.seg_end, zs.wp_center, flight_line);
101 
102  struct FloatVect2 sweep_span;
103  VECT2_SMUL(sweep_span, zs.sweep_width, zs.total_laps-1);
104  //start and end of flight-line in return-direction
105  VECT2_DIFF(zs.ret_start, zs.seg_end, sweep_span);
106  VECT2_DIFF(zs.ret_end, zs.seg_start, sweep_span);
107 
108  //turn-centers at both ends
109  zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x) / 2.0;
110  zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y) / 2.0;
113 
114  //fast climbing to desired altitude
117 
118  zs.stage = Z_ENTRY;
119 
120  return FALSE;
121 }
122 
131 {
132  // retain altitude
135 
136  //go from center of field to end of field - (before starting the syrvey)
137  if (zs.stage == Z_ENTRY) {
140  zs.stage = Z_TURN1;
142  nav_init_stage();
143  }
144  }
145 
146  //Turn from stage to return
147  else if (zs.stage == Z_TURN1) {
150  // && nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, CARROT
151  //calculate SEG-points for the next flyover
154 
155  zs.stage = Z_RET;
156  nav_init_stage();
157 #ifdef DIGITAL_CAM
159 #endif
160  }
161  }
162 
163  //fly the segment until seg_end is reached
164  else if (zs.stage == Z_RET) {
168 #ifdef DIGITAL_CAM
169  //dc_stop();
171 #endif
172  zs.stage = Z_TURN2;
173  }
174  }
175 
176  //turn from stage to return
177  else if (zs.stage == Z_TURN2) {
180  //zs.current_laps = zs.current_laps + 1;
181  zs.stage = Z_SEG;
182  nav_init_stage();
183 #ifdef DIGITAL_CAM
185 #endif
186  }
187 
188  //return
189  } else if (zs.stage == Z_SEG) {
192 
193  // calculate the rest of the points for the next fly-over
200 
201  zs.stage = Z_TURN1;
202  nav_init_stage();
203 #ifdef DIGITAL_CAM
204  //dc_stop();
206 #endif
207  }
208  }
209  if (zs.current_laps >= zs.total_laps) {
210 #ifdef DIGITAL_CAM
212 #endif
213  return FALSE;
214  }
215  else {
216  return TRUE;
217  }
218 }
float flight_angle
in degrees
struct FloatVect2 turn_center1
struct FloatVect2 ret_end
#define FALSE
Definition: imu_chimu.h:141
Standard Digital Camera Control Interface.
struct FloatVect2 turn_center2
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:74
int pre_leave_angle
in degrees.
#define TRUE
Definition: imu_chimu.h:144
struct FloatVect2 seg_end
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:68
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:56
unsigned char uint8_t
Definition: types.h:14
struct FloatVect2 sweep_width
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:80
struct FloatVect2 ret_start
float return_angle
in degrees
struct FloatVect2 wp_center
struct FloatVect2 wp_dir
#define FLOAT_VECT2_NORMALIZE(_v)
z_survey_stage stage
z_stage starts at ENTRY and than circles trought the other states until to rectangle is completely co...
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:50
struct FloatVect2 seg_start