diff --git a/conf/modules/airspeed_ets.xml b/conf/modules/airspeed_ets.xml index 0df5b46da3..2d0b665e81 100644 --- a/conf/modules/airspeed_ets.xml +++ b/conf/modules/airspeed_ets.xml @@ -13,7 +13,7 @@ - + diff --git a/sw/airborne/modules/sensors/airspeed_ets.c b/sw/airborne/modules/sensors/airspeed_ets.c index ec5d29e68c..f4cef5200e 100644 --- a/sw/airborne/modules/sensors/airspeed_ets.c +++ b/sw/airborne/modules/sensors/airspeed_ets.c @@ -100,60 +100,63 @@ void airspeed_ets_read_periodic( void ) { #endif //SITL } -void airspeed_ets_read_event( void ) { - int n; - float airspeed_tmp = 0.0; +void airspeed_ets_event( void ) { - // Get raw airspeed from buffer - airspeed_ets_raw = ((uint16_t)(airspeed_ets_i2c_trans.buf[1]) << 8) | (uint16_t)(airspeed_ets_i2c_trans.buf[0]); - // Check if this is valid airspeed - if (airspeed_ets_raw == 0) - airspeed_ets_valid = FALSE; - else - airspeed_ets_valid = TRUE; + if (airspeed_ets_i2c_trans.status == I2CTransSuccess) { + int n; + float airspeed_tmp = 0.0; - // Continue only if a new airspeed value was received - if (airspeed_ets_valid) { - // Calculate offset average if not done already - if (!airspeed_ets_offset_init) { - --airspeed_ets_cnt; - // Check if averaging completed - if (airspeed_ets_cnt == 0) { - // Calculate average - airspeed_ets_offset = (uint16_t)(airspeed_ets_offset_tmp / AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG); - // Limit offset - if (airspeed_ets_offset < AIRSPEED_ETS_OFFSET_MIN) - airspeed_ets_offset = AIRSPEED_ETS_OFFSET_MIN; - if (airspeed_ets_offset > AIRSPEED_ETS_OFFSET_MAX) - airspeed_ets_offset = AIRSPEED_ETS_OFFSET_MAX; - airspeed_ets_offset_init = TRUE; - } - // Check if averaging needs to continue - else if (airspeed_ets_cnt <= AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG) - airspeed_ets_offset_tmp += airspeed_ets_raw; - } - // Convert raw to m/s - 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; + // Get raw airspeed from buffer + airspeed_ets_raw = ((uint16_t)(airspeed_ets_i2c_trans.buf[1]) << 8) | (uint16_t)(airspeed_ets_i2c_trans.buf[0]); + // Check if this is valid airspeed + if (airspeed_ets_raw == 0) + airspeed_ets_valid = FALSE; else - airspeed_tmp = 0.0; - // Airspeed should always be positive - if (airspeed_tmp < 0.0) - airspeed_tmp = 0.0; - // Moving average - airspeed_ets_buffer[airspeed_ets_buffer_idx++] = airspeed_tmp; - if (airspeed_ets_buffer_idx >= AIRSPEED_ETS_NBSAMPLES_AVRG) - airspeed_ets_buffer_idx = 0; - airspeed_ets = 0.0; - for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n) - airspeed_ets += airspeed_ets_buffer[n]; - airspeed_ets = airspeed_ets / (float)AIRSPEED_ETS_NBSAMPLES_AVRG; - EstimatorSetAirspeed(airspeed_ets); - } else { - airspeed_ets = 0.0; + airspeed_ets_valid = TRUE; + + // Continue only if a new airspeed value was received + if (airspeed_ets_valid) { + // Calculate offset average if not done already + if (!airspeed_ets_offset_init) { + --airspeed_ets_cnt; + // Check if averaging completed + if (airspeed_ets_cnt == 0) { + // Calculate average + airspeed_ets_offset = (uint16_t)(airspeed_ets_offset_tmp / AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG); + // Limit offset + if (airspeed_ets_offset < AIRSPEED_ETS_OFFSET_MIN) + airspeed_ets_offset = AIRSPEED_ETS_OFFSET_MIN; + if (airspeed_ets_offset > AIRSPEED_ETS_OFFSET_MAX) + airspeed_ets_offset = AIRSPEED_ETS_OFFSET_MAX; + airspeed_ets_offset_init = TRUE; + } + // Check if averaging needs to continue + else if (airspeed_ets_cnt <= AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG) + airspeed_ets_offset_tmp += airspeed_ets_raw; + } + // Convert raw to m/s + 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; + else + airspeed_tmp = 0.0; + // Airspeed should always be positive + if (airspeed_tmp < 0.0) + airspeed_tmp = 0.0; + // Moving average + airspeed_ets_buffer[airspeed_ets_buffer_idx++] = airspeed_tmp; + if (airspeed_ets_buffer_idx >= AIRSPEED_ETS_NBSAMPLES_AVRG) + airspeed_ets_buffer_idx = 0; + airspeed_ets = 0.0; + for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n) + airspeed_ets += airspeed_ets_buffer[n]; + airspeed_ets = airspeed_ets / (float)AIRSPEED_ETS_NBSAMPLES_AVRG; + EstimatorSetAirspeed(airspeed_ets); + } else { + airspeed_ets = 0.0; + } + + // Transaction has been read + airspeed_ets_i2c_trans.status = I2CTransDone; + } - - // Transaction has been read - airspeed_ets_i2c_trans.status = I2CTransDone; - } diff --git a/sw/airborne/modules/sensors/airspeed_ets.h b/sw/airborne/modules/sensors/airspeed_ets.h index 6ce9db430c..b14f3f2544 100644 --- a/sw/airborne/modules/sensors/airspeed_ets.h +++ b/sw/airborne/modules/sensors/airspeed_ets.h @@ -50,8 +50,6 @@ extern struct i2c_transaction airspeed_ets_i2c_trans; extern void airspeed_ets_init( void ); extern void airspeed_ets_read_periodic( void ); -extern void airspeed_ets_read_event( void ); - -#define AirspeedEtsEvent() { if (airspeed_ets_i2c_trans.status == I2CTransSuccess) airspeed_ets_read_event(); } +extern void airspeed_ets_event( void ); #endif // AIRSPEED_ETS_H