diff --git a/conf/modules/airspeed_ets.xml b/conf/modules/airspeed_ets.xml index 4cadfe3a91..fbb839f38e 100644 --- a/conf/modules/airspeed_ets.xml +++ b/conf/modules/airspeed_ets.xml @@ -4,27 +4,37 @@ Airspeed ETS (I2C). - Driver for the EagleTree Systems Airspeed Sensor. - Has only been tested with V3 of the sensor hardware. + Driver for the EagleTree Systems Airspeed Sensor v3 connected via an I2C port. + This device measures airspeed from 4m/s to 156m/s with about 0,5 m/s resolution Notes: - Connect directly to TWOG/Tiny/Lisa I2C port. Multiple sensors can be chained together. - Sensor may be in the proprietary mode (default) or in 3rd-party mode. In 3rd-party mode you must define AIRSPEED_ETS_3RD_PARTY_MODE. - A eLogger is needed to set sensor to 3rd-party mode, which is supposed to be more reliable than the default mode. + + Connects directly to a flightcontroller board I2C port. + + Has I2C signal level conversion build in. + + Multiple sensors can be chained together. + + Sensor may be in the default or in 3rd-party mode. + + Per default the sensor is in Eagltreemode, but if you have set your sensor to 3rd-party mode + only then must you add the define AIRSPEED_ETS_3RD_PARTY_MODE to your airframe + + This driver has only been tested with v3 of the sensor hardware, older versions might or might not work. Sensor module wire assignments: - - Red wire: 5V + - Red wire: 3V-16V - White wire: Ground - Yellow wire: SDA - Brown wire: SCL + - - - - - - + + + + + + + + + i2c @@ -44,6 +54,7 @@ + @@ -53,4 +64,3 @@ - diff --git a/sw/airborne/modules/sensors/airspeed_ets.c b/sw/airborne/modules/sensors/airspeed_ets.c index 9e7442ca5b..d663f30308 100644 --- a/sw/airborne/modules/sensors/airspeed_ets.c +++ b/sw/airborne/modules/sensors/airspeed_ets.c @@ -23,14 +23,14 @@ /** * @file modules/sensors/airspeed_ets.c - * - * Driver for the EagleTree Systems Airspeed Sensor. - * Has only been tested with V3 of the sensor hardware. + * @brief Driver for the EagleTree Systems Airspeed Sensor v3 connected via an I2C port. + * The device Measures airspeed from 4m/s to 156m/s with about 0,5 m/s resolution * * Notes: - * Connect directly to TWOG/Tiny I2C port. Multiple sensors can be chained together. - * Sensor should be in the proprietary mode (default) and not in 3rd party mode. - * Define AIRSPEED_ETS_3RD_PARTY_MODE to run it in 3rd party mode. + * Connects directly to a flightcontroller board I2C port. Has signal level conversion build in. Multiple sensors can be chained together. + * Sensor may be in the default or in 3rd-party mode. If set to 3rd-party mode you must define AIRSPEED_ETS_3RD_PARTY_MODE. + * + * To set sensor to 3rd-party mode one can use the supplied Jumper, read Eagletree original manual on how to do this. * * Sensor module wire assignments: * Red wire: 5V @@ -65,6 +65,7 @@ PRINT_CONFIG_MSG("AIRSPEED_ETS not used") #endif #define AIRSPEED_ETS_ADDR 0xEA + #ifndef AIRSPEED_ETS_SCALE #define AIRSPEED_ETS_SCALE 1.8 #endif @@ -75,7 +76,23 @@ PRINT_CONFIG_MSG("AIRSPEED_ETS not used") #define AIRSPEED_ETS_OFFSET_MIN 1450 #define AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT 40 #define AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG 60 + +#ifndef AIRSPEED_ETS_USE_FILTER +#define AIRSPEED_ETS_USE_FILTER FALSE +#endif + +#ifdef AIRSPEED_ETS_USE_FILTER +#define AIRSPEED_ETS_NBSAMPLES_AVRG 20 +#else #define AIRSPEED_ETS_NBSAMPLES_AVRG 10 +#endif + +/** Time constant for second order Butterworth low pass filter + * Default of 0.15 should give cut-off freq of 1/(2*pi*tau) ~= 1Hz + */ +#ifndef AIRSPEED_ETS_LOWPASS_TAU +#define AIRSPEED_ETS_LOWPASS_TAU 0.15 +#endif #ifndef AIRSPEED_ETS_I2C_DEV #define AIRSPEED_ETS_I2C_DEV i2c0 @@ -88,6 +105,10 @@ PRINT_CONFIG_VAR(AIRSPEED_ETS_I2C_DEV) #endif PRINT_CONFIG_VAR(AIRSPEED_ETS_START_DELAY) +#ifdef AIRSPEED_ETS_USE_FILTER +#include "filters/low_pass_filter.h" +#endif + #ifndef SITL #if AIRSPEED_ETS_SDLOG #include "modules/loggers/sdlog_chibios.h" @@ -108,6 +129,10 @@ float airspeed_ets_buffer[AIRSPEED_ETS_NBSAMPLES_AVRG]; struct i2c_transaction airspeed_ets_i2c_trans; +#ifdef AIRSPEED_ETS_USE_FILTER +static Butterworth2LowPass airspeed_filter; +#endif + // Local variables volatile bool airspeed_ets_i2c_done; bool airspeed_ets_offset_init; @@ -148,6 +173,11 @@ void airspeed_ets_init(void) airspeed_ets_buffer[n] = 0.0; } +#ifdef AIRSPEED_ETS_USE_FILTER + init_butterworth_2_low_pass(&airspeed_filter, AIRSPEED_ETS_LOWPASS_TAU, + AIRSPEED_ETS_READ_PERIODIC_PERIOD, airspeed_ets); +#endif + airspeed_ets_i2c_trans.status = I2CTransDone; airspeed_ets_delay_done = false; @@ -198,6 +228,7 @@ void airspeed_ets_read_event(void) // Continue only if a new airspeed value was received if (airspeed_ets_valid) { #if !AIRSPEED_ETS_3RD_PARTY_MODE + // Calculate offset average if not done already if (!airspeed_ets_offset_init) { --airspeed_ets_cnt; @@ -219,14 +250,15 @@ void airspeed_ets_read_event(void) airspeed_ets_offset_tmp += airspeed_ets_raw; } } + // Convert raw to m/s #ifdef AIRSPEED_ETS_REVERSE if (airspeed_ets_offset_init && airspeed_ets_raw < airspeed_ets_offset) { - airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_offset - airspeed_ets_raw)) - AIRSPEED_ETS_OFFSET; + airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_offset - airspeed_ets_raw)) - (float)AIRSPEED_ETS_OFFSET; } #else if (airspeed_ets_offset_init && airspeed_ets_raw > airspeed_ets_offset) { - airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_raw - airspeed_ets_offset)) - AIRSPEED_ETS_OFFSET; + airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_raw - airspeed_ets_offset)) - (float)AIRSPEED_ETS_OFFSET; } #endif else { @@ -252,6 +284,20 @@ void airspeed_ets_read_event(void) } airspeed_ets = airspeed_ets / (float)AIRSPEED_ETS_NBSAMPLES_AVRG; +#ifdef AIRSPEED_ETS_USE_FILTER + float a_out; + a_out = airspeed_tmp; + airspeed_ets = update_butterworth_2_low_pass(&airspeed_filter, a_out); +#endif + + //Sometimes offset is not correct enough calculated dynamically and yields to a small negative airspeed once in a while at certain samples + //Also with steep airspeed decline and a butterworth_2_low_pass filter on it could overshoot to a negative value this is also capped + //so force cap it to zero + if ((airspeed_ets < 0.0) || (airspeed_tmp < 0.0)) { + airspeed_tmp = 0.0; //for filter also + airspeed_ets = 0.0; + } + // Publish airspeed sensor AbiSendMsgAIRSPEED(AIRSPEED_ETS_ID, airspeed_ets);