diff --git a/sw/airborne/anemotaxis.c b/sw/airborne/anemotaxis.c index aed9b00146..816cf17b18 100644 --- a/sw/airborne/anemotaxis.c +++ b/sw/airborne/anemotaxis.c @@ -5,8 +5,7 @@ #include "nav.h" #include "flight_plan.h" #include "ap_downlink.h" - -uint8_t chemo_sensor; +#include "chemo_detect.h" enum status { UTURN, CROSSWIND }; static enum status status; diff --git a/sw/airborne/anemotaxis.h b/sw/airborne/anemotaxis.h index c6d7f2e7a2..16e571b252 100644 --- a/sw/airborne/anemotaxis.h +++ b/sw/airborne/anemotaxis.h @@ -3,7 +3,6 @@ #include "std.h" -extern uint8_t chemo_sensor; extern bool_t nav_anemotaxis_downwind(uint8_t c, float radius); extern bool_t nav_anemotaxis_init(uint8_t c); extern bool_t nav_anemotaxis(uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume); diff --git a/sw/airborne/chemo_detect.c b/sw/airborne/chemo_detect.c index eb1b4aa457..32cb7e776b 100644 --- a/sw/airborne/chemo_detect.c +++ b/sw/airborne/chemo_detect.c @@ -4,7 +4,7 @@ #define DETECT_PERIOD 8 /* *4Hz */ #define THRESHOLD 150 -uint8_t chemo_sensor; +uint16_t chemo_sensor; void chemo_init( void ) { chemo_sensor = 0; @@ -16,8 +16,10 @@ void chemo_periodic( void ) { /* Detection on the first sensor */ int dval = enose_val[0] - vals[0][idx]; - if (dval < -THRESHOLD) - chemo_sensor = Min(-dval, 0xff); + if (dval < -THRESHOLD) { + chemo_sensor = -dval; + } else + chemo_sensor = 0; int i; for(i = 0; i < ENOSE_NB_SENSOR; i++) diff --git a/sw/airborne/chemo_detect.h b/sw/airborne/chemo_detect.h index cc6a2daeab..b0a5e1062c 100644 --- a/sw/airborne/chemo_detect.h +++ b/sw/airborne/chemo_detect.h @@ -3,8 +3,8 @@ #include "std.h" -extern uint8_t chemo_sensor; -#define MAX_CHEMO 255 +extern uint16_t chemo_sensor; +#define MAX_CHEMO 400 void chemo_init( void ); void chemo_periodic( void ); diff --git a/sw/airborne/chemotaxis.c b/sw/airborne/chemotaxis.c index 1fb3609f3f..e2865ab1b9 100644 --- a/sw/airborne/chemotaxis.c +++ b/sw/airborne/chemotaxis.c @@ -5,14 +5,12 @@ #include "nav.h" #include "flight_plan.h" #include "ap_downlink.h" - -uint8_t chemo_sensor; +#include "chemo_detect.h" #define MAX_RADIUS 250 #define ALPHA 0.5 static uint8_t last_plume_value; -#define MAX_CHEMO 255 static float radius; static int8_t sign; @@ -23,7 +21,6 @@ bool_t nav_chemotaxis_init( uint8_t c, uint8_t plume ) { sign = 1; waypoints[plume].x = waypoints[c].x; waypoints[plume].y = waypoints[c].y; - chemo_sensor = 0; return FALSE; } diff --git a/sw/airborne/enose.c b/sw/airborne/enose.c index c78984dfef..4f8d833172 100644 --- a/sw/airborne/enose.c +++ b/sw/airborne/enose.c @@ -68,9 +68,15 @@ void enose_periodic( void ) { enose_i2c_done = FALSE; } else if (enose_status == ENOSE_MEASURING_RD) { - enose_val[0] = (i2c_buf[0]<<8) | i2c_buf[1]; - enose_val[1] = (i2c_buf[2]<<8) | i2c_buf[3]; - enose_val[2] = (i2c_buf[4]<<8) | i2c_buf[5]; + uint16_t val = (i2c_buf[0]<<8) | i2c_buf[1]; + if (val < 5000) + enose_val[0] = val; + val = (i2c_buf[2]<<8) | i2c_buf[3]; + if (val < 5000) + enose_val[1] = val; + val = (i2c_buf[4]<<8) | i2c_buf[5]; + if (val < 5000) + enose_val[2] = val; enose_status = ENOSE_IDLE; } }