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
anemotaxis.c
Go to the documentation of this file.
2 #include "generated/airframe.h"
3 #include "state.h"
4 #include "std.h"
6 #include "generated/flight_plan.h"
9 
11 static enum status status;
12 static int8_t sign;
13 static struct point last_plume;
14 
15 static void last_plume_was_here(void)
16 {
19 }
20 
22 {
24  float wind_dir = atan2(wind->x, wind->y);
25  waypoints[c].x = waypoints[WP_HOME].x + radius * cos(wind_dir);
26  waypoints[c].y = waypoints[WP_HOME].y + radius * sin(wind_dir);
27  return false;
28 }
29 
31 {
32  status = UTURN;
33  sign = 1;
35  float wind_dir = atan2(wind->x, wind->y);
36  waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS * cos(wind_dir + M_PI);
37  waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS * sin(wind_dir + M_PI);
39  return false;
40 }
41 
43 {
44  if (chemo_sensor) {
46  waypoints[plume].x = stateGetPositionEnu_f()->x;
47  waypoints[plume].y = stateGetPositionEnu_f()->y;
48  // DownlinkSendWpNr(plume);
49  }
50 
52  float wind_dir = atan2(wind->x, wind->y) + M_PI;
53 
55  float upwind_x = cos(wind_dir);
56  float upwind_y = sin(wind_dir);
57 
58  switch (status) {
59  case UTURN:
61  if (NavQdrCloseTo(DegOfRad(M_PI_2 - wind_dir))) {
62  float crosswind_x = - upwind_y;
63  float crosswind_y = upwind_x;
64  waypoints[c1].x = waypoints[c].x + DEFAULT_CIRCLE_RADIUS * upwind_x;
65  waypoints[c1].y = waypoints[c].y + DEFAULT_CIRCLE_RADIUS * upwind_y;
66 
67  float width = Max(2 * ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x - last_plume.x,
69 
70  waypoints[c2].x = waypoints[c1].x - width * crosswind_x * sign;
71  waypoints[c2].y = waypoints[c1].y - width * crosswind_y * sign;
72 
73  // DownlinkSendWpNr(c1);
74  // DownlinkSendWpNr(c2);
75 
76  status = CROSSWIND;
78  }
79  break;
80 
81  case CROSSWIND:
82  NavSegment(c1, c2);
83  if (NavApproaching(c2, CARROT)) {
84  waypoints[c].x = waypoints[c2].x + DEFAULT_CIRCLE_RADIUS * upwind_x;
85  waypoints[c].y = waypoints[c2].y + DEFAULT_CIRCLE_RADIUS * upwind_y;
86 
87  // DownlinkSendWpNr(c);
88 
89  sign = -sign;
90  status = UTURN;
92  }
93 
94  if (chemo_sensor) {
97 
98  // DownlinkSendWpNr(c);
99 
100  sign = -sign;
101  status = UTURN;
102  nav_init_stage();
103  }
104  break;
105  }
106  chemo_sensor = 0;
107  return true;
108 }
status
Definition: anemotaxis.c:10
float x
Definition: common_nav.h:40
bool nav_anemotaxis_init(uint8_t c)
Definition: anemotaxis.c:30
static float radius
Definition: chemotaxis.c:15
static int8_t sign
Definition: anemotaxis.c:12
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static struct point last_plume
Definition: anemotaxis.c:13
float y
Definition: common_nav.h:41
static void last_plume_was_here(void)
Definition: anemotaxis.c:15
uint16_t chemo_sensor
Definition: chemo_detect.c:7
float x
in meters
static uint16_t c1
Definition: baro_MS5534A.c:203
#define Max(x, y)
Definition: main_fbw.c:53
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
static uint16_t c2
Definition: baro_MS5534A.c:203
bool nav_anemotaxis(uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume)
Definition: anemotaxis.c:42
signed char int8_t
Definition: types.h:15
float y
in meters
bool nav_anemotaxis_downwind(uint8_t c, float radius)
Definition: anemotaxis.c:21