diff --git a/conf/modules/airspeed_adc.xml b/conf/modules/airspeed_adc.xml index 0121534ade..b064967f15 100644 --- a/conf/modules/airspeed_adc.xml +++ b/conf/modules/airspeed_adc.xml @@ -16,7 +16,7 @@ - + diff --git a/conf/modules/airspeed_ets.xml b/conf/modules/airspeed_ets.xml index 9bde519b64..bc56326e73 100644 --- a/conf/modules/airspeed_ets.xml +++ b/conf/modules/airspeed_ets.xml @@ -16,7 +16,7 @@ - + diff --git a/conf/modules/baro_ets.xml b/conf/modules/baro_ets.xml index e054d0e2ba..7d2c9351ea 100644 --- a/conf/modules/baro_ets.xml +++ b/conf/modules/baro_ets.xml @@ -12,7 +12,7 @@ - + diff --git a/sw/airborne/modules/sensors/airspeed_ets.c b/sw/airborne/modules/sensors/airspeed_ets.c index 7b8ae76096..e00164cb96 100644 --- a/sw/airborne/modules/sensors/airspeed_ets.c +++ b/sw/airborne/modules/sensors/airspeed_ets.c @@ -109,68 +109,64 @@ void airspeed_ets_read_periodic( void ) { #endif //SITL } -void airspeed_ets_event( void ) { +void airspeed_ets_read_event( void ) { + int n; + float airspeed_tmp = 0.0; - if (airspeed_ets_i2c_trans.status == I2CTransSuccess) { - int n; - float airspeed_tmp = 0.0; + // 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; - // 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; - - // 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; + // 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; } - // 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; + // 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; #ifdef USE_AIRSPEED - EstimatorSetAirspeed(airspeed_ets); + EstimatorSetAirspeed(airspeed_ets); #endif #ifdef SENSOR_SYNC_SEND - DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets); + DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets); #endif - } else { - airspeed_ets = 0.0; - } - - // Transaction has been read - airspeed_ets_i2c_trans.status = I2CTransDone; - + } else { + airspeed_ets = 0.0; } + + // 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 b14f3f2544..6ce9db430c 100644 --- a/sw/airborne/modules/sensors/airspeed_ets.h +++ b/sw/airborne/modules/sensors/airspeed_ets.h @@ -50,6 +50,8 @@ 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_event( void ); +extern void airspeed_ets_read_event( void ); + +#define AirspeedEtsEvent() { if (airspeed_ets_i2c_trans.status == I2CTransSuccess) airspeed_ets_read_event(); } #endif // AIRSPEED_ETS_H diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c index 9775c3d405..d1164a0d19 100644 --- a/sw/airborne/modules/sensors/baro_ets.c +++ b/sw/airborne/modules/sensors/baro_ets.c @@ -102,48 +102,46 @@ void baro_ets_read_periodic( void ) { #endif } -void baro_ets_event( void ) { - if (baro_ets_i2c_trans.status == I2CTransSuccess) { - // Get raw altimeter from buffer - baro_ets_adc = ((uint16_t)(baro_ets_i2c_trans.buf[1]) << 8) | (uint16_t)(baro_ets_i2c_trans.buf[0]); - // Check if this is valid altimeter - if (baro_ets_adc == 0) - baro_ets_valid = FALSE; - else - baro_ets_valid = TRUE; +void baro_ets_read_event( void ) { + // Get raw altimeter from buffer + baro_ets_adc = ((uint16_t)(baro_ets_i2c_trans.buf[1]) << 8) | (uint16_t)(baro_ets_i2c_trans.buf[0]); + // Check if this is valid altimeter + if (baro_ets_adc == 0) + baro_ets_valid = FALSE; + else + baro_ets_valid = TRUE; - // Continue only if a new altimeter value was received - if (baro_ets_valid) { - // Calculate offset average if not done already - if (!baro_ets_offset_init) { - --baro_ets_cnt; - // Check if averaging completed - if (baro_ets_cnt == 0) { - // Calculate average - baro_ets_offset = (uint16_t)(baro_ets_offset_tmp / BARO_ETS_OFFSET_NBSAMPLES_AVRG); - // Limit offset - if (baro_ets_offset < BARO_ETS_OFFSET_MIN) - baro_ets_offset = BARO_ETS_OFFSET_MIN; - if (baro_ets_offset > BARO_ETS_OFFSET_MAX) - baro_ets_offset = BARO_ETS_OFFSET_MAX; - baro_ets_offset_init = TRUE; - } - // Check if averaging needs to continue - else if (baro_ets_cnt <= BARO_ETS_OFFSET_NBSAMPLES_AVRG) - baro_ets_offset_tmp += baro_ets_adc; + // Continue only if a new altimeter value was received + if (baro_ets_valid) { + // Calculate offset average if not done already + if (!baro_ets_offset_init) { + --baro_ets_cnt; + // Check if averaging completed + if (baro_ets_cnt == 0) { + // Calculate average + baro_ets_offset = (uint16_t)(baro_ets_offset_tmp / BARO_ETS_OFFSET_NBSAMPLES_AVRG); + // Limit offset + if (baro_ets_offset < BARO_ETS_OFFSET_MIN) + baro_ets_offset = BARO_ETS_OFFSET_MIN; + if (baro_ets_offset > BARO_ETS_OFFSET_MAX) + baro_ets_offset = BARO_ETS_OFFSET_MAX; + baro_ets_offset_init = TRUE; } - // Convert raw to m/s - if (baro_ets_offset_init) - baro_ets_altitude = BARO_ETS_SCALE * (float)(baro_ets_offset-baro_ets_adc); - else - baro_ets_altitude = 0.0; - // New value available - EstimatorSetAlt(baro_ets_altitude); - } else { - baro_ets_altitude = 0.0; + // Check if averaging needs to continue + else if (baro_ets_cnt <= BARO_ETS_OFFSET_NBSAMPLES_AVRG) + baro_ets_offset_tmp += baro_ets_adc; } - - // Transaction has been read - baro_ets_i2c_trans.status = I2CTransDone; + // Convert raw to m/s + if (baro_ets_offset_init) + baro_ets_altitude = BARO_ETS_SCALE * (float)(baro_ets_offset-baro_ets_adc); + else + baro_ets_altitude = 0.0; + // New value available + EstimatorSetAlt(baro_ets_altitude); + } else { + baro_ets_altitude = 0.0; } + + // Transaction has been read + baro_ets_i2c_trans.status = I2CTransDone; } diff --git a/sw/airborne/modules/sensors/baro_ets.h b/sw/airborne/modules/sensors/baro_ets.h index a288bcfbdf..337c4c041c 100644 --- a/sw/airborne/modules/sensors/baro_ets.h +++ b/sw/airborne/modules/sensors/baro_ets.h @@ -56,6 +56,8 @@ extern struct i2c_transaction baro_ets_i2c_trans; extern void baro_ets_init( void ); extern void baro_ets_read_periodic( void ); -extern void baro_ets_event( void ); +extern void baro_ets_read_event( void ); + +#define BaroEtsEvent() { if (baro_ets_i2c_trans.status == I2CTransSuccess) baro_ets_read_event(); } #endif // BARO_ETS_H