// read one airspeed sensor void AP_Airspeed::read(uint8_t i) { float airspeed_pressure; if (!enabled(i) || !sensor[i]) { return; } bool prev_healthy = state[i].healthy; float raw_pressure = get_pressure(i); if (state[i].cal.start_ms != 0) { update_calibration(i, raw_pressure); } airspeed_pressure = raw_pressure - param[i].offset; // remember raw pressure for logging state[i].corrected_pressure = airspeed_pressure; // filter before clamping positive if (!prev_healthy) { // if the previous state was not healthy then we should not // use an IIR filter, otherwise a bad reading will last for // some time after the sensor becomees healthy again state[i].filtered_pressure = airspeed_pressure; } else { state[i].filtered_pressure = 0.7f * state[i].filtered_pressure + 0.3f * airspeed_pressure; } /* we support different pitot tube setups so user can choose if they want to be able to detect pressure on the static port */ switch ((enum pitot_tube_order)param[i].tube_order.get()) { case PITOT_TUBE_ORDER_NEGATIVE: state[i].last_pressure = -airspeed_pressure; state[i].raw_airspeed = sqrtf(MAX(-airspeed_pressure, 0) * param[i].ratio); state[i].airspeed = sqrtf(MAX(-state[i].filtered_pressure, 0) * param[i].ratio); break; case PITOT_TUBE_ORDER_POSITIVE: state[i].last_pressure = airspeed_pressure; state[i].raw_airspeed = sqrtf(MAX(airspeed_pressure, 0) * param[i].ratio); state[i].airspeed = sqrtf(MAX(state[i].filtered_pressure, 0) * param[i].ratio); break; case PITOT_TUBE_ORDER_AUTO: default: state[i].last_pressure = fabsf(airspeed_pressure); state[i].raw_airspeed = sqrtf(fabsf(airspeed_pressure) * param[i].ratio); state[i].airspeed = sqrtf(fabsf(state[i].filtered_pressure) * param[i].ratio); break; } if (state[i].last_pressure < -32) { // we're reading more than about -8m/s. The user probably has // the ports the wrong way around state[i].healthy = false; } state[i].last_update_ms = AP_HAL::millis(); }
// read the airspeed sensor void AP_Airspeed::read(void) { float airspeed_pressure; if (!enabled()) { return; } float raw_pressure = get_pressure(); if (_cal.start_ms != 0) { update_calibration(raw_pressure); } airspeed_pressure = raw_pressure - _offset; // remember raw pressure for logging _corrected_pressure = airspeed_pressure; // filter before clamping positive _filtered_pressure = 0.7f * _filtered_pressure + 0.3f * airspeed_pressure; /* we support different pitot tube setups so user can choose if they want to be able to detect pressure on the static port */ switch ((enum pitot_tube_order)_tube_order.get()) { case PITOT_TUBE_ORDER_NEGATIVE: _last_pressure = -airspeed_pressure; _raw_airspeed = sqrtf(MAX(-airspeed_pressure, 0) * _ratio); _airspeed = sqrtf(MAX(-_filtered_pressure, 0) * _ratio); break; case PITOT_TUBE_ORDER_POSITIVE: _last_pressure = airspeed_pressure; _raw_airspeed = sqrtf(MAX(airspeed_pressure, 0) * _ratio); _airspeed = sqrtf(MAX(_filtered_pressure, 0) * _ratio); if (airspeed_pressure < -32) { // we're reading more than about -8m/s. The user probably has // the ports the wrong way around _healthy = false; } break; case PITOT_TUBE_ORDER_AUTO: default: _last_pressure = fabsf(airspeed_pressure); _raw_airspeed = sqrtf(fabsf(airspeed_pressure) * _ratio); _airspeed = sqrtf(fabsf(_filtered_pressure) * _ratio); break; } _last_update_ms = AP_HAL::millis(); }
/** * Called by the GUI when a key event is received, triggers an action in the * controller. * */ void Controller::receive_gui_event(const char *event,const char *value) { if(m_sleeping) return; //TODO: Fix this total mess, refactor into switch, break conditions out into methods. if(strcmp(event,"Sleep" ) == 0) event_sleep(event,value); else if(strcmp(event,"KEYPRESS" ) == 0) m_powerup=true; else if(strcmp(event,"TOTALTIMER" ) == 0) event_totaltimer(event,value); else if(strcmp(event,"Save:PulseWidth") == 0) event_save_pulsewidth(event,value); else if(strcmp(event,"Save:Calib" ) == 0) event_save_calibration(); else if(strcmp(event,"Save:Becq" ) == 0) event_save_becq(event,value); else if(strcmp(event,"Save:UTCOff" ) == 0) event_save_utcoff(event,value); else if(strcmp(event,"Save:Time" ) == 0) save_time(); else if(strcmp(event,"Save:Date" ) == 0) save_date(); else if(strcmp(event,"Save:WarnCPM" ) == 0) save_warncpm(); else if(strcmp(event,"Japanese" ) == 0) event_japanese(event,value); else if(strcmp(event,"Never Dim" ) == 0) event_neverdim(event,value); else if(strcmp(event,"English" ) == 0) event_english(event,value); else if(strcmp(event,"CPM/CPS Auto" ) == 0) event_cpm_cps_auto(event,value); else if(strcmp(event,"Geiger Beep" ) == 0) event_geiger_beep(event,value); else if(strcmp(event,"\x80Sv" ) == 0) event_usv(event,value); else if(strcmp(event,"\x80R" ) == 0) event_rem(event,value); else if(strcmp(event,"Clear Log" ) == 0) event_clear_log(event,value); else if(strcmp(event,"Save:Brightness") == 0) event_save_brightness(event,value); else if(strcmp(event,"Save:LogInter" ) == 0) save_loginterval(); else if(strcmp(event,"CALIBRATE" ) == 0) initialise_calibration(); else if(strcmp(event,"UTCSCREEN" ) == 0) event_utcscreen(event,value); else if(strcmp(event,"TIMESCREEN" ) == 0) event_timescreen(event,value); else if(strcmp(event,"BECQSCREEN" ) == 0) event_becqscreen(event,value); else if(strcmp(event,"LOGINTERVAL" ) == 0) event_loginterval(event,value); else if(strcmp(event,"WARNSCREEN" ) == 0) event_warnscreen(event,value); else if(strcmp(event,"DATESCREEN" ) == 0) event_datescreen(event,value); else if(strcmp(event,"BrightnessSCN" ) == 0) event_brightnessscn(event,value); else if(strcmp(event,"LeftBrightness" ) == 0) event_leftbrightness(event,value); else if(strcmp(event,"QR Transfer" ) == 0) qr_logxfer(); else if(strcmp(event,"Audio Xfer" ) == 0) event_audioxfer(event,value); else if(strcmp(event,"QR Tweet" ) == 0) event_qrtweet(event,value); else if(strcmp(event,"varnumchange" ) == 0) { if(strcmp ("BRIGHTNESS",value) == 0) event_varnum_brightness(event,value); else if(strcmpl("CAL" ,value,3) ) update_calibration(); else if(strcmpl("DATE" ,value,4) ) event_varnum_date(event,value); else if(strcmpl("TIME" ,value,4) ) event_varnum_time(event,value); } }