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
chemotaxis.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 
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 = stateGetPositionEnu_f()->x - waypoints[plume].x;
32  float y = stateGetPositionEnu_f()->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 - (*stateGetHorizontalSpeedDir_f()));
38  float dir_y = sin(M_PI_2 - (*stateGetHorizontalSpeedDir_f()));
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 = stateGetPositionEnu_f()->x;
47  waypoints[plume].y = stateGetPositionEnu_f()->y;
48  // DownlinkSendWp(plume);
50  }
51 
53  return TRUE;
54 }
static float * stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:861
#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:672
bool_t nav_chemotaxis(uint8_t c, uint8_t plume)
Definition: chemotaxis.c:27
float y
in meters
#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 x
in meters
#define TRUE
Definition: imu_chimu.h:144
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
bool_t nav_chemotaxis_init(uint8_t c, uint8_t plume)
Definition: chemotaxis.c:18
static int8_t sign
Definition: chemotaxis.c:16
int32_t y
North.
signed char int8_t
Definition: types.h:15
static struct point c
Definition: discsurvey.c:39
int32_t x
East.