void airspeed_ets_read_periodic( void ) {
#ifndef SITL
if (airspeed_ets_i2c_trans.status == I2CTransDone)
I2CReceive(AIRSPEED_ETS_I2C_DEV, airspeed_ets_i2c_trans, AIRSPEED_ETS_ADDR, 2);
#else // SITL
extern float sim_air_speed;
EstimatorSetAirspeed(sim_air_speed);
#endif //SITL
}
void airspeed_adc_update( void ) {
#ifndef SITL
adc_airspeed_val = buf_airspeed.sum / buf_airspeed.av_nb_sample;
#ifdef AIRSPEED_QUADRATIC_SCALE
float airspeed = (adc_airspeed_val - AIRSPEED_BIAS);
if (airspeed <= 0.0f)
airspeed = 0.0f;
airspeed = sqrtf(airspeed) * AIRSPEED_QUADRATIC_SCALE;
#else
float airspeed = AIRSPEED_SCALE * (adc_airspeed_val - AIRSPEED_BIAS);
#endif
EstimatorSetAirspeed(airspeed);
#else // SITL
extern float sim_air_speed;
EstimatorSetAirspeed(sim_air_speed);
adc_airspeed_val = 0;
#endif //SITL
}
void estimator_init( void ) {
EstimatorSetPosXY(0., 0.);
EstimatorSetAlt(0.);
EstimatorSetAtt (0., 0., 0);
EstimatorSetSpeedPol ( 0., 0., 0.);
EstimatorSetRate(0., 0.);
#ifdef USE_AIRSPEED
EstimatorSetAirspeed( 0. );
#endif
#ifdef USE_AOA
EstimatorSetAOA( 0. );
#endif
estimator_flight_time = 0;
estimator_airspeed = NOMINAL_AIRSPEED;
}
void airspeed_ets_read_event( void ) {
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;
// 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
#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;
#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;
#endif
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;
#if USE_AIRSPEED
EstimatorSetAirspeed(airspeed_ets);
#endif
#ifdef SENSOR_SYNC_SEND
DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, DefaultDevice, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets);
#endif
} else {
airspeed_ets = 0.0;
}
// Transaction has been read
airspeed_ets_i2c_trans.status = I2CTransDone;
}
