From 53ba3f4b74cd96f895c8828741a11a0dc4208a7c Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 22 May 2013 23:20:31 +0200 Subject: [PATCH] removed GPS check from ekf and put into GPS --- flight/modules/GPS/GPS.c | 3 ++- flight/modules/StateEstimation/filterekf.c | 16 ---------------- 2 files changed, 2 insertions(+), 17 deletions(-) diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index 6c36cb5f3..c6d6d04ff 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -262,7 +262,8 @@ static void gpsTask(__attribute__((unused)) void *parameters) // criteria for GPS-OK taken from this post... // http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220 if ((gpspositionsensor.PDOP < 3.5f) && (gpspositionsensor.Satellites >= 7) && - (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D)) { + (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) && + (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) { AlarmsClear(SYSTEMALARMS_ALARM_GPS); #ifdef PIOS_GPS_SETS_HOMELOCATION HomeLocationData home; diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index bbbb870af..b672bffae 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -34,7 +34,6 @@ #include #include -#include #include #include @@ -218,21 +217,6 @@ static int32_t filter(stateFilter *self, stateEstimation *state) return 0; } - if (this->usePos) { - GPSPositionSensorData gpsData; - GPSPositionSensorGet(&gpsData); - // Have a minimum requirement for gps usage - if ((gpsData.Satellites < 7) || - (gpsData.PDOP > 4.0f) || - (gpsData.Latitude == 0 && gpsData.Longitude == 0) || - (this->homeLocation.Set != HOMELOCATION_SET_TRUE)) { - UNSET_MASK(state->updated, SENSORUPDATES_pos); - UNSET_MASK(state->updated, SENSORUPDATES_vel); - UNSET_MASK(this->work.updated, SENSORUPDATES_pos); - UNSET_MASK(this->work.updated, SENSORUPDATES_vel); - } - } - dT = PIOS_DELAY_DiffuS(this->ins_last_time) / 1.0e6f; this->ins_last_time = PIOS_DELAY_GetRaw();