Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
anemotaxis.c
Go to the documentation of this file.
2 #include "generated/airframe.h"
3 #include "estimator.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 
21  float wind_dir = atan2(wind_north, wind_east);
22  waypoints[c].x = waypoints[WP_HOME].x + radius*cos(wind_dir);
23  waypoints[c].y = waypoints[WP_HOME].y + radius*sin(wind_dir);
24  return FALSE;
25 }
26 
28  status = UTURN;
29  sign = 1;
30  float wind_dir = atan2(wind_north, wind_east);
31  waypoints[c].x = estimator_x + DEFAULT_CIRCLE_RADIUS*cos(wind_dir+M_PI);
32  waypoints[c].y = estimator_y + DEFAULT_CIRCLE_RADIUS*sin(wind_dir+M_PI);
34  return FALSE;
35 }
36 
38  if (chemo_sensor) {
40  waypoints[plume].x = estimator_x;
41  waypoints[plume].y = estimator_y;
42  // DownlinkSendWp(plume);
43  }
44 
45  float wind_dir = atan2(wind_north, wind_east) + M_PI;
46 
48  float upwind_x = cos(wind_dir);
49  float upwind_y = sin(wind_dir);
50 
51  switch (status) {
52  case UTURN:
54  if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
55  float crosswind_x = - upwind_y;
56  float crosswind_y = upwind_x;
59 
60  float width = Max(2*ScalarProduct(upwind_x, upwind_y, estimator_x-last_plume.x, estimator_y-last_plume.y), DEFAULT_CIRCLE_RADIUS);
61 
62  waypoints[c2].x = waypoints[c1].x - width*crosswind_x*sign;
63  waypoints[c2].y = waypoints[c1].y - width*crosswind_y*sign;
64 
65  // DownlinkSendWp(c1);
66  // DownlinkSendWp(c2);
67 
68  status = CROSSWIND;
70  }
71  break;
72 
73  case CROSSWIND:
74  NavSegment(c1, c2);
75  if (NavApproaching(c2, CARROT)) {
78 
79  // DownlinkSendWp(c);
80 
81  sign = -sign;
82  status = UTURN;
84  }
85 
86  if (chemo_sensor) {
89 
90  // DownlinkSendWp(c);
91 
92  sign = -sign;
93  status = UTURN;
95  }
96  break;
97  }
98  chemo_sensor = 0;
99  return TRUE;
100 }
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
float estimator_y
north position in meters
Definition: estimator.c:43
bool_t nav_anemotaxis(uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume)
Definition: anemotaxis.c:37
#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 wind_east
Definition: estimator.c:68
static uint16_t c1
Definition: baro_MS5534A.c:197
float estimator_x
east position in meters
Definition: estimator.c:42
#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:27
float y
Definition: common_nav.h:38
static uint16_t c2
Definition: baro_MS5534A.c:197
State estimation, fusioning sensors.
float wind_north
Definition: estimator.c:68
signed char int8_t
Definition: types.h:15
static struct point c
Definition: discsurvey.c:13
float x
Definition: common_nav.h:37