Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
chemotaxis.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 
10 #define MAX_RADIUS 250
11 #define ALPHA 0.5
12 
14 
15 static float radius;
16 static int8_t sign;
17 
20  last_plume_value = 0;
21  sign = 1;
22  waypoints[plume].x = waypoints[c].x;
23  waypoints[plume].y = waypoints[c].y;
24  return FALSE;
25 }
26 
27 bool_t nav_chemotaxis( uint8_t c, uint8_t plume ) {
28 
30  /* Move the circle in this direction */
31  float x = estimator_x - waypoints[plume].x;
32  float y = estimator_y - waypoints[plume].y;
33  waypoints[c].x = waypoints[plume].x + ALPHA * x;
34  waypoints[c].y = waypoints[plume].y + ALPHA * y;
35  // DownlinkSendWp(c);
36  /* Turn in the right direction */
37  float dir_x = cos(M_PI_2 - estimator_hspeed_dir);
38  float dir_y = sin(M_PI_2 - estimator_hspeed_dir);
39  float pvect = dir_x*y - dir_y*x;
40  sign = (pvect > 0 ? -1 : 1);
41  /* Reduce the radius */
43 
44 
45  /* Store this plume */
46  waypoints[plume].x = estimator_x;
47  waypoints[plume].y = estimator_y;
48  // DownlinkSendWp(plume);
50  }
51 
53  return TRUE;
54 }
#define MAX_CHEMO
Definition: chemo_detect.h:7
#define MAX_RADIUS
Definition: chemotaxis.c:10
static float radius
Definition: chemotaxis.c:15
float estimator_y
north position in meters
Definition: estimator.c:43
#define ALPHA
Definition: chemotaxis.c:11
bool_t nav_chemotaxis(uint8_t c, uint8_t plume)
Definition: chemotaxis.c:27
float estimator_hspeed_dir
direction of horizontal ground speed in rad (CW/North)
Definition: estimator.c:65
#define FALSE
Definition: imu_chimu.h:141
static uint8_t last_plume_value
Definition: chemotaxis.c:13
uint16_t chemo_sensor
Definition: chemo_detect.c:7
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
float y
Definition: common_nav.h:38
bool_t nav_chemotaxis_init(uint8_t c, uint8_t plume)
Definition: chemotaxis.c:18
State estimation, fusioning sensors.
static int8_t sign
Definition: chemotaxis.c:16
signed char int8_t
Definition: types.h:15
static struct point c
Definition: discsurvey.c:13
float x
Definition: common_nav.h:37