Paparazzi UAS v7.0_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
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
58 int sweep_lines, float altitude)
59{
60 zs.current_laps = 0;
62
63 // copy variables from the flight plan
67
68 // if turning right leave circle before angle is reached, if turning left - leave after
69 if (sweep_spacing > 0) {
71 }
72
76
77 // calculate the flight_angle
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)
89
90 // calculate number of laps to fly and turning radius for each end
91 zs.total_laps = (sweep_lines + 1) / 2;
94
97
98 //CALCULATE THE NAVIGATION POINTS
99 //start and end of flight-line in flight-direction
102
103 struct FloatVect2 sweep_span;
105 //start and end of flight-line in return-direction
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;
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;
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;
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
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)
uint16_t foo
Definition main_demo5.c:58
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
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.
#define LINE_STOP_FUNCTION
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.
#define LINE_START_FUNCTION
struct ZamboniSurvey zs
Zamboni pattern survey for fixedwings.
struct FloatVect2 wp_center
struct FloatVect2 turn_center2
struct FloatVect2 wp_dir
float return_angle
in degrees
float flight_angle
in degrees
struct FloatVect2 ret_start
z_survey_stage stage
z_stage starts at ENTRY and than circles trought the other states until to rectangle is completely co...
struct FloatVect2 turn_center1
struct FloatVect2 sweep_width
struct FloatVect2 seg_start
struct FloatVect2 ret_end
@ Z_ENTRY
@ Z_TURN1
@ Z_TURN2
int pre_leave_angle
in degrees.
struct FloatVect2 seg_end
#define CARROT
default approaching_time for a wp
Definition navigation.h:70
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.