Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
chemotaxis.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 
10 #define MAX_RADIUS 250
11 #define ALPHA 0.5
12 
14 
15 static float radius;
16 static int8_t sign;
17 
19 {
21  last_plume_value = 0;
22  sign = 1;
23  waypoints[plume].x = waypoints[c].x;
24  waypoints[plume].y = waypoints[c].y;
25  return FALSE;
26 }
27 
28 bool_t nav_chemotaxis(uint8_t c, uint8_t plume)
29 {
30 
32  /* Move the circle in this direction */
33  float x = stateGetPositionEnu_f()->x - waypoints[plume].x;
34  float y = stateGetPositionEnu_f()->y - waypoints[plume].y;
35  waypoints[c].x = waypoints[plume].x + ALPHA * x;
36  waypoints[c].y = waypoints[plume].y + ALPHA * y;
37  // DownlinkSendWp(c);
38  /* Turn in the right direction */
39  float dir_x = cos(M_PI_2 - stateGetHorizontalSpeedDir_f());
40  float dir_y = sin(M_PI_2 - stateGetHorizontalSpeedDir_f());
41  float pvect = dir_x * y - dir_y * x;
42  sign = (pvect > 0 ? -1 : 1);
43  /* Reduce the radius */
46 
47 
48  /* Store this plume */
49  waypoints[plume].x = stateGetPositionEnu_f()->x;
50  waypoints[plume].y = stateGetPositionEnu_f()->y;
51  // DownlinkSendWp(plume);
53  }
54 
56  return TRUE;
57 }
float x
Definition: common_nav.h:40
#define MAX_CHEMO
Definition: chemo_detect.h:7
#define MAX_RADIUS
Definition: chemotaxis.c:10
static float radius
Definition: chemotaxis.c:15
#define ALPHA
Definition: chemotaxis.c:11
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:702
bool_t nav_chemotaxis(uint8_t c, uint8_t plume)
Definition: chemotaxis.c:28
#define FALSE
Definition: std.h:5
#define TRUE
Definition: std.h:4
static uint8_t last_plume_value
Definition: chemotaxis.c:13
float y
Definition: common_nav.h:41
uint16_t chemo_sensor
Definition: chemo_detect.c:7
float x
in meters
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:921
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
bool_t nav_chemotaxis_init(uint8_t c, uint8_t plume)
Definition: chemotaxis.c:18
static int8_t sign
Definition: chemotaxis.c:16
signed char int8_t
Definition: types.h:15
float y
in meters