Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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"
5 #include "subsystems/nav.h"
6 #include "generated/flight_plan.h"
7 #include "ap_downlink.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 ) {
18 }
19 
22  float wind_dir = atan2(wind->x, wind->y);
23  waypoints[c].x = waypoints[WP_HOME].x + radius*cos(wind_dir);
24  waypoints[c].y = waypoints[WP_HOME].y + radius*sin(wind_dir);
25  return FALSE;
26 }
27 
29  status = UTURN;
30  sign = 1;
32  float wind_dir = atan2(wind->x, wind->y);
33  waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS*cos(wind_dir+M_PI);
34  waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS*sin(wind_dir+M_PI);
36  return FALSE;
37 }
38 
40  if (chemo_sensor) {
42  waypoints[plume].x = stateGetPositionEnu_f()->x;
43  waypoints[plume].y = stateGetPositionEnu_f()->y;
44  // DownlinkSendWp(plume);
45  }
46 
48  float wind_dir = atan2(wind->x, wind->y) + M_PI;
49 
51  float upwind_x = cos(wind_dir);
52  float upwind_y = sin(wind_dir);
53 
54  switch (status) {
55  case UTURN:
57  if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
58  float crosswind_x = - upwind_y;
59  float crosswind_y = upwind_x;
62 
63  float width = Max(2*ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x-last_plume.x, stateGetPositionEnu_f()->y-last_plume.y), DEFAULT_CIRCLE_RADIUS);
64 
65  waypoints[c2].x = waypoints[c1].x - width*crosswind_x*sign;
66  waypoints[c2].y = waypoints[c1].y - width*crosswind_y*sign;
67 
68  // DownlinkSendWp(c1);
69  // DownlinkSendWp(c2);
70 
71  status = CROSSWIND;
73  }
74  break;
75 
76  case CROSSWIND:
77  NavSegment(c1, c2);
78  if (NavApproaching(c2, CARROT)) {
81 
82  // DownlinkSendWp(c);
83 
84  sign = -sign;
85  status = UTURN;
87  }
88 
89  if (chemo_sensor) {
92 
93  // DownlinkSendWp(c);
94 
95  sign = -sign;
96  status = UTURN;
98  }
99  break;
100  }
101  chemo_sensor = 0;
102  return TRUE;
103 }
status
Definition: anemotaxis.c:10
bool_t nav_anemotaxis_downwind(uint8_t c, float radius)
Definition: anemotaxis.c:20
static float radius
Definition: chemotaxis.c:15
static int8_t sign
Definition: anemotaxis.c:12
bool_t nav_anemotaxis(uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume)
Definition: anemotaxis.c:39
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
static struct point last_plume
Definition: anemotaxis.c:13
static void last_plume_was_here(void)
Definition: anemotaxis.c:15
#define Max(x, y)
uint16_t chemo_sensor
Definition: chemo_detect.c:7
float x
in meters
static uint16_t c1
Definition: baro_MS5534A.c:196
#define TRUE
Definition: imu_chimu.h:144
unsigned char uint8_t
Definition: types.h:14
bool_t nav_anemotaxis_init(uint8_t c)
Definition: anemotaxis.c:28
API to get/set the generic vehicle states.
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1192
float y
Definition: common_nav.h:41
static uint16_t c2
Definition: baro_MS5534A.c:196
int32_t y
North.
signed char int8_t
Definition: types.h:15
static struct point c
Definition: discsurvey.c:39
float x
Definition: common_nav.h:40
int32_t x
East.