Paparazzi UAS  v6.2_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 }
int8_t
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
DiscSurvey::c1
struct FloatVect2 c1
Definition: nav_survey_disc.c:42
nav_survey_disc.h
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
nav_circle_XY
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition: nav.c:111
DiscSurveyStatus
DiscSurveyStatus
Definition: nav_survey_disc.c:36
NavQdrCloseTo
#define NavQdrCloseTo(x)
True if x (in degrees) is close to the current QDR (less than 10 degrees)
Definition: nav.h:164
pprz_algebra_float.h
Paparazzi floating point algebra.
waypoints
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
DiscSurvey::c2
struct FloatVect2 c2
Definition: nav_survey_disc.c:43
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
DiscSurvey::sign
int8_t sign
Definition: nav_survey_disc.c:40
NavVerticalAutoThrottleMode
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition: nav.h:180
VECT2_DIFF
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
CARROT
#define CARROT
default approaching_time for a wp
Definition: navigation.h:40
FloatVect2
Definition: pprz_algebra_float.h:49
nav_survey_disc_setup
void nav_survey_disc_setup(float grid)
Definition: nav_survey_disc.c:49
std.h
nav_route_xy
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:383
disc_survey
static struct DiscSurvey disc_survey
Definition: nav_survey_disc.c:46
SEGMENT
@ SEGMENT
Definition: nav_survey_disc.c:36
nav_init_stage
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition: nav.c:95
point::x
float x
Definition: common_nav.h:40
UTURN
@ UTURN
Definition: nav_survey_disc.c:36
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
stateGetHorizontalWindspeed_f
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1377
DiscSurvey::c
struct FloatVect2 c
Definition: nav_survey_disc.c:41
nav.h
WaypointAlt
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition: common_nav.h:48
nav_survey_shift
float nav_survey_shift
Definition: nav.c:89
nav_approaching_xy
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:326
DiscSurvey
Definition: nav_survey_disc.c:38
point::y
float y
Definition: common_nav.h:41
VECT2_DOT_PRODUCT
#define VECT2_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:116
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
state.h
nav_survey_disc_run
bool nav_survey_disc_run(uint8_t center_wp, float radius)
Definition: nav_survey_disc.c:58
DiscSurvey::status
enum DiscSurveyStatus status
Definition: nav_survey_disc.c:39
DOWNWIND
@ DOWNWIND
Definition: nav_survey_disc.c:36
NavVerticalAltitudeMode
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition: nav.h:194