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; }