void AOA_adc_update( void ) { #ifndef SITL adc_AOA_val = buf_AOA.sum / buf_AOA.av_nb_sample; // PT1 filter and convert to rad AOA = AOA_filter * AOA_old + (1 - AOA_filter) * (adc_AOA_val*(2*M_PI)/1024-M_PI+AOA_offset); AOA_old = AOA; #endif RunOnceEvery(30, DOWNLINK_SEND_AOA_adc(DefaultChannel, DefaultDevice, &adc_AOA_val, &AOA)); #ifdef USE_AOA EstimatorSetAOA(AOA); #endif }
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; }