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) {
#if !AIRSPEED_ETS_3RD_PARTY_MODE
// 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;
}
//use raw value for sensor set to third-party mode
#else
airspeed_tmp = airspeed_ets_raw;
#endif //AIRSPEED_ETS_3RD_PARTY_MODE
// 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_ETS
stateSetAirspeed_f(airspeed_ets);
#endif
#if AIRSPEED_ETS_SYNC_SEND
DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, DefaultDevice, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets);
#endif
} else {
airspeed_ets = 0.0;
}
#if AIRSPEED_ETS_SDLOG
#ifndef SITL
if (pprzLogFile != -1) {
if (!log_airspeed_ets_started) {
sdLogWriteLog(pprzLogFile, "AIRSPEED_ETS: raw offset airspeed(m/s) GPS_fix TOW(ms) Week Lat(1e7deg) Lon(1e7deg) HMSL(mm) gpseed(cm/s) course(1e7rad) climb(cm/s)\n");
log_airspeed_ets_started = true;
}
sdLogWriteLog(pprzLogFile, "airspeed_ets: %d %d %8.4f %d %d %d %d %d %d %d %d %d\n",
airspeed_ets_raw, airspeed_ets_offset, airspeed_ets,
gps.fix, gps.tow, gps.week,
gps.lla_pos.lat, gps.lla_pos.lon, gps.hmsl,
gps.gspeed, gps.course, -gps.ned_vel.z);
}
#endif
#endif
// Transaction has been read
airspeed_ets_i2c_trans.status = I2CTransDone;
}
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
stateSetAirspeed_f(&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;
}
