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