Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 }
ZamboniSurvey
Definition: nav_survey_zamboni.h:36
VECT2_SMUL
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:98
FLOAT_VECT2_NORMALIZE
#define FLOAT_VECT2_NORMALIZE(_v)
Definition: pprz_algebra_float.h:154
ZamboniSurvey::return_angle
float return_angle
in degrees
Definition: nav_survey_zamboni.h:48
Z_TURN1
@ Z_TURN1
Definition: nav_survey_zamboni.h:34
LINE_START_FUNCTION
#define LINE_START_FUNCTION
Definition: nav_survey_zamboni.c:39
ZamboniSurvey::turnradius1
float turnradius1
Definition: nav_survey_zamboni.h:51
ZamboniSurvey::total_laps
int total_laps
Definition: nav_survey_zamboni.h:50
nav_survey_zamboni_setup
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.
Definition: nav_survey_zamboni.c:57
ZamboniSurvey::seg_start
struct FloatVect2 seg_start
Definition: nav_survey_zamboni.h:55
nav_circle_XY
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition: nav.c:109
ZamboniSurvey::sweep_width
struct FloatVect2 sweep_width
Definition: nav_survey_zamboni.h:40
VECT2_ADD
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:74
waypoints
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
nav_survey_zamboni_run
bool nav_survey_zamboni_run(void)
main navigation routine.
Definition: nav_survey_zamboni.c:129
ZamboniSurvey::turn_center1
struct FloatVect2 turn_center1
Definition: nav_survey_zamboni.h:53
ZamboniSurvey::altitude
float altitude
Definition: nav_survey_zamboni.h:41
dc.h
ZamboniSurvey::turnradius2
float turnradius2
Definition: nav_survey_zamboni.h:52
NavVerticalAutoThrottleMode
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:171
VECT2_DIFF
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
VECT2_COPY
#define VECT2_COPY(_a, _b)
Definition: pprz_algebra.h:68
CARROT
#define CARROT
default approaching_time for a wp
Definition: navigation.h:40
FloatVect2
Definition: pprz_algebra_float.h:49
NavCourseCloseTo
#define NavCourseCloseTo(x)
Definition: nav.h:157
nav_route_xy
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:381
ZamboniSurvey::ret_start
struct FloatVect2 ret_start
Definition: nav_survey_zamboni.h:57
nav_init_stage
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition: nav.c:93
zs
struct ZamboniSurvey zs
Definition: nav_survey_zamboni.c:45
ZamboniSurvey::pre_leave_angle
int pre_leave_angle
in degrees.
Definition: nav_survey_zamboni.h:46
ZamboniSurvey::turn_center2
struct FloatVect2 turn_center2
Definition: nav_survey_zamboni.h:54
uint8_t
unsigned char uint8_t
Definition: types.h:14
ZamboniSurvey::seg_end
struct FloatVect2 seg_end
Definition: nav_survey_zamboni.h:56
autopilot.h
Z_SEG
@ Z_SEG
Definition: nav_survey_zamboni.h:34
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
Z_TURN2
@ Z_TURN2
Definition: nav_survey_zamboni.h:34
ZamboniSurvey::stage
z_survey_stage stage
z_stage starts at ENTRY and than circles trought the other states until to rectangle is completely co...
Definition: nav_survey_zamboni.h:69
ZamboniSurvey::flight_angle
float flight_angle
in degrees
Definition: nav_survey_zamboni.h:47
nav.h
LINE_STOP_FUNCTION
#define LINE_STOP_FUNCTION
Definition: nav_survey_zamboni.c:42
nav_survey_zamboni.h
nav_approaching_xy
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:323
ZamboniSurvey::current_laps
int current_laps
Definition: nav_survey_zamboni.h:49
Z_RET
@ Z_RET
Definition: nav_survey_zamboni.h:34
ZamboniSurvey::ret_end
struct FloatVect2 ret_end
Definition: nav_survey_zamboni.h:58
VECT2_SUM
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:86
altitude
static int32_t altitude
Definition: airspeed_uADC.c:59
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
ZamboniSurvey::wp_center
struct FloatVect2 wp_center
Definition: nav_survey_zamboni.h:38
ZamboniSurvey::wp_dir
struct FloatVect2 wp_dir
Definition: nav_survey_zamboni.h:39
Z_ENTRY
@ Z_ENTRY
Definition: nav_survey_zamboni.h:34
NavVerticalAltitudeMode
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition: nav.h:185