From 24044dc0c42d265afc9295a70911f95726a78dd1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 25 Mar 2013 16:24:59 +0900 Subject: [PATCH] AHRS: add support for GPS fix type 2D --- libraries/AP_AHRS/AP_AHRS.cpp | 2 +- libraries/AP_AHRS/AP_AHRS.h | 2 +- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index b6601ce9b2..385243ced7 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -103,7 +103,7 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret) { if (_airspeed && _airspeed->use()) { *airspeed_ret = _airspeed->get_airspeed(); - if (_wind_max > 0 && _gps && _gps->status() == GPS::GPS_OK) { + if (_wind_max > 0 && _gps && _gps->status() >= GPS::GPS_OK_FIX_2D) { // constrain the airspeed by the ground speed // and AHRS_WIND_MAX *airspeed_ret = constrain(*airspeed_ret, diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 375852f9a4..1a4c2549ae 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -120,7 +120,7 @@ public: // otherwise false. This only updates the lat and lng fields // of the Location bool get_position(struct Location *loc) { - if (!_gps || _gps->status() != GPS::GPS_OK) { + if (!_gps || _gps->status() <= GPS::NO_FIX) { return false; } loc->lat = _gps->latitude; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 48342d1ed1..ee2470ec69 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -281,7 +281,7 @@ AP_AHRS_DCM::_P_gain(float spin_rate) // return true if we have and should use GPS bool AP_AHRS_DCM::have_gps(void) { - if (!_gps || _gps->status() != GPS::GPS_OK || !_gps_use) { + if (!_gps || _gps->status() <= GPS::NO_FIX || !_gps_use) { return false; } return true; @@ -730,7 +730,7 @@ bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret) ret = true; } - if (ret && _wind_max > 0 && _gps && _gps->status() == GPS::GPS_OK) { + if (ret && _wind_max > 0 && _gps && _gps->status() >= GPS::GPS_OK_FIX_2D) { // constrain the airspeed by the ground speed // and AHRS_WIND_MAX *airspeed_ret = constrain(*airspeed_ret,