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