Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nav_survey_disc.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"
33 #include "generated/flight_plan.h"
35 
37 
38 struct DiscSurvey {
41  struct FloatVect2 c;
42  struct FloatVect2 c1;
43  struct FloatVect2 c2;
44 };
45 
46 static struct DiscSurvey disc_survey;
47 
48 
49 bool_t nav_survey_disc_setup(float grid)
50 {
51  nav_survey_shift = grid;
53  disc_survey.sign = 1;
56  return FALSE;
57 }
58 
59 bool_t nav_survey_disc_run(uint8_t center_wp, float radius)
60 {
62  float wind_dir = atan2(wind->x, wind->y) + M_PI;
63 
65  struct FloatVect2 upwind;
66  upwind.x = cos(wind_dir);
67  upwind.y = sin(wind_dir);
68 
69  float grid = nav_survey_shift / 2;
70 
71  switch (disc_survey.status) {
72  case UTURN:
74  if (NavQdrCloseTo(DegOfRad(M_PI_2 - wind_dir))) {
77 
78  struct FloatVect2 dist;
79  VECT2_DIFF(dist, disc_survey.c1, waypoints[center_wp]);
80  float d = VECT2_DOT_PRODUCT(upwind, dist);
81  if (d > radius) {
83  } else {
84  float w = sqrtf(radius * radius - d * d) - 1.5 * grid;
85 
86  struct FloatVect2 crosswind;
87  crosswind.x = -upwind.y;
88  crosswind.y = upwind.x;
89 
90  disc_survey.c2.x = waypoints[center_wp].x + d * upwind.x - w * disc_survey.sign * crosswind.x;
91  disc_survey.c2.y = waypoints[center_wp].y + d * upwind.y - w * disc_survey.sign * crosswind.y;
92 
94  }
96  }
97  break;
98 
99  case DOWNWIND:
100  disc_survey.c2.x = waypoints[center_wp].x - upwind.x * radius;
101  disc_survey.c2.y = waypoints[center_wp].y - upwind.y * radius;
103  /* No break; */
104 
105  case SEGMENT:
108  disc_survey.c.x = disc_survey.c2.x + grid * upwind.x;
109  disc_survey.c.y = disc_survey.c2.y + grid * upwind.y;
110 
113  nav_init_stage();
114  }
115  break;
116  default:
117  break;
118  }
119 
120  NavVerticalAutoThrottleMode(0.); /* No pitch */
121  NavVerticalAltitudeMode(WaypointAlt(center_wp), 0.); /* No preclimb */
122 
123  return TRUE;
124 }
#define WaypointAlt(_wp)
Definition: common_nav.h:47
float x
Definition: common_nav.h:40
static float radius
Definition: chemotaxis.c:15
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:91
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:702
#define FALSE
Definition: std.h:5
#define TRUE
Definition: std.h:4
float y
Definition: common_nav.h:41
Paparazzi floating point algebra.
float x
in meters
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1301
#define VECT2_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:115
signed char int8_t
Definition: types.h:15
float y
in meters