Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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  nav_survey_shift = grid;
52  disc_survey.sign = 1;
55  return FALSE;
56 }
57 
58 bool_t nav_survey_disc_run( uint8_t center_wp, float radius) {
60  float wind_dir = atan2(wind->x, wind->y) + M_PI;
61 
63  struct FloatVect2 upwind;
64  upwind.x = cos(wind_dir);
65  upwind.y = sin(wind_dir);
66 
67  float grid = nav_survey_shift / 2;
68 
69  switch (disc_survey.status) {
70  case UTURN:
72  if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
75 
76  struct FloatVect2 dist;
77  VECT2_DIFF(dist, disc_survey.c1, waypoints[center_wp]);
78  float d = FLOAT_VECT2_DOT_PRODUCT(upwind, dist);
79  if (d > radius) {
81  } else {
82  float w = sqrtf(radius*radius - d*d) - 1.5*grid;
83 
84  struct FloatVect2 crosswind;
85  crosswind.x = -upwind.y;
86  crosswind.y = upwind.x;
87 
88  disc_survey.c2.x = waypoints[center_wp].x + d*upwind.x - w*disc_survey.sign*crosswind.x;
89  disc_survey.c2.y = waypoints[center_wp].y + d*upwind.y - w*disc_survey.sign*crosswind.y;
90 
92  }
94  }
95  break;
96 
97  case DOWNWIND:
98  disc_survey.c2.x = waypoints[center_wp].x - upwind.x * radius;
99  disc_survey.c2.y = waypoints[center_wp].y - upwind.y * radius;
101  /* No break; */
102 
103  case SEGMENT:
106  disc_survey.c.x = disc_survey.c2.x + grid*upwind.x;
107  disc_survey.c.y = disc_survey.c2.y + grid*upwind.y;
108 
111  nav_init_stage();
112  }
113  break;
114  default:
115  break;
116  }
117 
118  NavVerticalAutoThrottleMode(0.); /* No pitch */
119  NavVerticalAltitudeMode(WaypointAlt(center_wp), 0.); /* No preclimb */
120 
121  return TRUE;
122 }
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
Paparazzi floating point algebra.
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:74
struct FloatVect2 c
float x
in meters
#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
enum DiscSurveyStatus status
int32_t y
North.
signed char int8_t
Definition: types.h:15
struct FloatVect2 c2
#define FLOAT_VECT2_DOT_PRODUCT(_v1, _v2)
int32_t x
East.
struct FloatVect2 c1