diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 3d9be88da7..b42e7a6f88 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -237,13 +237,13 @@ Vector2f AP_AHRS::groundspeed_vector(void) // Generate estimate of ground speed vector using air data system Vector2f gndVelADS; Vector2f gndVelGPS; - float airspeed; + float airspeed = 0; const bool gotAirspeed = airspeed_estimate_true(&airspeed); const bool gotGPS = (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D); if (gotAirspeed) { const Vector3f wind = wind_estimate(); const Vector2f wind2d(wind.x, wind.y); - const Vector2f airspeed_vector(cosf(yaw) * airspeed, sinf(yaw) * airspeed); + const Vector2f airspeed_vector(_cos_yaw * airspeed, _sin_yaw * airspeed); gndVelADS = airspeed_vector - wind2d; } @@ -281,6 +281,20 @@ Vector2f AP_AHRS::groundspeed_vector(void) if (!gotAirspeed && gotGPS) { return gndVelGPS; } + + if (airspeed > 0) { + // we have a rough airspeed, and we have a yaw. For + // dead-reckoning purposes we can create a estimated + // groundspeed vector + Vector2f ret(cosf(yaw), sinf(yaw)); + ret *= airspeed; + // adjust for estimated wind + Vector3f wind = wind_estimate(); + ret.x += wind.x; + ret.y += wind.y; + return ret; + } + return Vector2f(0.0f, 0.0f); } diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index c75c87bf25..6ed1f7a731 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -1023,6 +1023,11 @@ bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret) const gnd_speed + _wind_max); *airspeed_ret = true_airspeed / get_EAS2TAS(); } + if (!ret) { + // give the last estimate, but return false. This is used by + // dead-reckoning code + *airspeed_ret = _last_airspeed; + } return ret; }