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