AP_AHRS: fixed wind reporting with EKF and no airspeed sensor

the EKF does not estimate wind without an airspeed sensor
This commit is contained in:
Andrew Tridgell 2014-04-10 06:49:13 +10:00
parent b78e59ea30
commit 27a3b5fb36
1 changed files with 3 additions and 1 deletions

View File

@ -122,7 +122,9 @@ float AP_AHRS_NavEKF::get_error_yaw(void)
// return a wind estimation vector, in m/s
Vector3f AP_AHRS_NavEKF::wind_estimate(void)
{
if (!using_EKF()) {
if (!using_EKF() || !_airspeed || !_airspeed->use()) {
// EKF does not estimate wind speed when there is no airspeed
// sensor active
return AP_AHRS_DCM::wind_estimate();
}
Vector3f wind;