Merge pull request #2366 from PX4/ekf_airspeed

EKF unfiltered airspeed
This commit is contained in:
Lorenz Meier 2015-06-18 08:36:11 +02:00
commit f619c1390e
2 changed files with 6 additions and 2 deletions

View File

@ -1062,7 +1062,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
}
// Fuse Airspeed Measurements
if (fuseAirSpeed && _ekf->VtasMeas > 7.0f) {
if (fuseAirSpeed && _airspeed.true_airspeed_m_s > 5.0f) {
_ekf->fuseVtasData = true;
_ekf->RecallStates(_ekf->statesAtVtasMeasTime,
(IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
@ -1320,7 +1320,7 @@ void AttitudePositionEstimatorEKF::pollData()
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
perf_count(_perf_airspeed);
_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
_ekf->VtasMeas = _airspeed.true_airspeed_unfiltered_m_s;
}

View File

@ -1288,6 +1288,10 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
_airspeed.true_airspeed_m_s = math::max(0.0f,
calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
_airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f,
calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
_airspeed.air_temperature_celsius = air_temperature_celsius;
/* announce the airspeed if needed, just publish else */