Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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  /* fallthrough */
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 }
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition: common_nav.h:48
float y
Definition: common_nav.h:41
float x
Definition: common_nav.h:40
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
#define VECT2_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:116
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1377
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
float nav_survey_shift
Definition: nav.c:86
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition: nav.c:108
Fixedwing Navigation library.
#define NavQdrCloseTo(x)
True if x (in degrees) is close to the current QDR (less than 10 degrees)
Definition: nav.h:161
#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
struct FloatVect2 c
static struct DiscSurvey disc_survey
enum DiscSurveyStatus status
struct FloatVect2 c1
void nav_survey_disc_setup(float grid)
bool nav_survey_disc_run(uint8_t center_wp, float radius)
DiscSurveyStatus
@ SEGMENT
@ DOWNWIND
@ UTURN
struct FloatVect2 c2
Paparazzi floating point algebra.
float y
in meters
float x
in meters
#define CARROT
default approaching_time for a wp
Definition: navigation.h:70
API to get/set the generic vehicle states.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103