Paparazzi UAS  v5.12_stable-4-g9b43e9b
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 void nav_survey_disc_setup(float grid)
50 {
51  nav_survey_shift = grid;
53  disc_survey.sign = 1;
56 }
57 
58 bool nav_survey_disc_run(uint8_t center_wp, float radius)
59 {
61  float wind_dir = atan2(wind->x, wind->y) + M_PI;
62 
64  struct FloatVect2 upwind;
65  upwind.x = cos(wind_dir);
66  upwind.y = sin(wind_dir);
67 
68  float grid = nav_survey_shift / 2;
69 
70  switch (disc_survey.status) {
71  case UTURN:
73  if (NavQdrCloseTo(DegOfRad(M_PI_2 - wind_dir))) {
76 
77  struct FloatVect2 dist;
78  VECT2_DIFF(dist, disc_survey.c1, waypoints[center_wp]);
79  float d = VECT2_DOT_PRODUCT(upwind, dist);
80  if (d > radius) {
82  } else {
83  float w = sqrtf(radius * radius - d * d) - 1.5 * grid;
84 
85  struct FloatVect2 crosswind;
86  crosswind.x = -upwind.y;
87  crosswind.y = upwind.x;
88 
89  disc_survey.c2.x = waypoints[center_wp].x + d * upwind.x - w * disc_survey.sign * crosswind.x;
90  disc_survey.c2.y = waypoints[center_wp].y + d * upwind.y - w * disc_survey.sign * crosswind.y;
91 
93  }
95  }
96  break;
97 
98  case DOWNWIND:
99  disc_survey.c2.x = waypoints[center_wp].x - upwind.x * radius;
100  disc_survey.c2.y = waypoints[center_wp].y - upwind.y * radius;
102  /* No break; */
103 
104  case SEGMENT:
107  disc_survey.c.x = disc_survey.c2.x + grid * upwind.x;
108  disc_survey.c.y = disc_survey.c2.y + grid * upwind.y;
109 
112  nav_init_stage();
113  }
114  break;
115  default:
116  break;
117  }
118 
119  NavVerticalAutoThrottleMode(0.); /* No pitch */
120  NavVerticalAltitudeMode(WaypointAlt(center_wp), 0.); /* No preclimb */
121 
122  return true;
123 }
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition: common_nav.h:48
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:719
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:1377
#define VECT2_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:115
signed char int8_t
Definition: types.h:15
float y
in meters