Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
discsurvey.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2003-2005 Pascal Brisset, Antoine Drouin
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 
28 
29 #include "generated/airframe.h"
30 #include "state.h"
31 #include "std.h"
32 #include "subsystems/nav.h"
33 #include "generated/flight_plan.h"
34 #include "ap_downlink.h"
35 
37 static enum status status;
38 static int8_t sign;
39 static struct point c;
40 static struct point c1;
41 static struct point c2;
42 
43 bool_t disc_survey_init( float grid ) {
44  nav_survey_shift = grid;
45  status = DOWNWIND;
46  sign = 1;
49  return FALSE;
50 }
51 
52 bool_t disc_survey( uint8_t center, float radius) {
54  float wind_dir = atan2(wind->x, wind->y) + M_PI;
55 
57  float upwind_x = cos(wind_dir);
58  float upwind_y = sin(wind_dir);
59 
60  float grid = nav_survey_shift / 2;
61 
62  switch (status) {
63  case UTURN:
64  nav_circle_XY(c.x, c.y, grid*sign);
65  if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
68 
69  float d = ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x-WaypointX(center), stateGetPositionEnu_f()->y-WaypointY(center));
70  if (d > radius) {
71  status = DOWNWIND;
72  } else {
73  float w = sqrt(radius*radius - d*d) - 1.5*grid;
74 
75  float crosswind_x = - upwind_y;
76  float crosswind_y = upwind_x;
77 
78  c2.x = WaypointX(center)+d*upwind_x-w*sign*crosswind_x;
79  c2.y = WaypointY(center)+d*upwind_y-w*sign*crosswind_y;
80 
81  status = SEGMENT;
82  }
84  }
85  break;
86 
87  case DOWNWIND:
88  c2.x = WaypointX(center) - upwind_x * radius;
89  c2.y = WaypointY(center) - upwind_y * radius;
90  status = SEGMENT;
91  /* No break; */
92 
93  case SEGMENT:
94  nav_route_xy(c1.x, c1.y, c2.x, c2.y);
95  if (nav_approaching_xy(c2.x, c2.y, c1.x, c1.y, CARROT)) {
96  c.x = c2.x + grid*upwind_x;
97  c.y = c2.y + grid*upwind_y;
98 
99  sign = -sign;
100  status = UTURN;
101  nav_init_stage();
102  }
103  break;
104  default:
105  break;
106  }
107 
108  NavVerticalAutoThrottleMode(0.); /* No pitch */
109  NavVerticalAltitudeMode(WaypointAlt(center), 0.); /* No preclimb */
110 
111  return TRUE;
112 }
status
Definition: anemotaxis.c:10
status
Definition: discsurvey.c:36
static float radius
Definition: chemotaxis.c:15
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
float y
in meters
#define FALSE
Definition: imu_chimu.h:141
static struct point c1
Definition: discsurvey.c:40
bool_t disc_survey(uint8_t center, float radius)
Definition: discsurvey.c:52
float x
in meters
static struct point c2
Definition: discsurvey.c:41
#define TRUE
Definition: imu_chimu.h:144
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1192
float y
Definition: common_nav.h:41
signed char int8_t
Definition: types.h:15
static struct point c
Definition: discsurvey.c:39
float x
Definition: common_nav.h:40
bool_t disc_survey_init(float grid)
Definition: discsurvey.c:43
static int8_t sign
Definition: discsurvey.c:38