From 04eb7f411edbb77a6082674d8de96de61ef688cb Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Sat, 8 Jul 2017 16:06:19 -0700 Subject: [PATCH] AP_GPS: Track the delta time between the last GPS positions --- libraries/AP_GPS/AP_GPS.cpp | 7 ++++++- libraries/AP_GPS/AP_GPS.h | 16 ++++++++++++++++ 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 79c45f69f3..2a990dd7e0 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -44,6 +44,7 @@ #endif #define GPS_BAUD_TIME_MS 1200 +#define GPS_TIMEOUT_MS 4000u // defines used to specify the mask position for use of different accuracy metrics in the blending algorithm #define BLEND_MASK_USE_HPOS_ACC 1 @@ -564,6 +565,7 @@ found_gps: state[instance].status = NO_FIX; drivers[instance] = new_gps; timing[instance].last_message_time_ms = now; + timing[instance].delta_time_ms = GPS_TIMEOUT_MS; new_gps->broadcast_gps_type(); } } @@ -616,7 +618,7 @@ void AP_GPS::update_instance(uint8_t instance) // has expired, re-initialise the GPS. This will cause GPS // detection to run again if (!result) { - if (tnow - timing[instance].last_message_time_ms > 4000) { + if (tnow - timing[instance].last_message_time_ms > GPS_TIMEOUT_MS) { // free the driver before we run the next detection, so we // don't end up with two allocated at any time delete drivers[instance]; @@ -626,8 +628,11 @@ void AP_GPS::update_instance(uint8_t instance) state[instance].hdop = GPS_UNKNOWN_DOP; state[instance].vdop = GPS_UNKNOWN_DOP; timing[instance].last_message_time_ms = tnow; + timing[instance].delta_time_ms = GPS_TIMEOUT_MS; } } else { + // delta will only be correct after parsing two messages + timing[instance].delta_time_ms = tnow - timing[instance].last_message_time_ms; timing[instance].last_message_time_ms = tnow; if (state[instance].status >= GPS_OK_FIX_2D) { timing[instance].last_fix_time_ms = tnow; diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 8dc45b478a..8808853a5e 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -34,6 +34,7 @@ #define GPS_MAX_RATE_MS 200 // maximum value of rate_ms (i.e. slowest update rate) is 5hz or 200ms #define GPS_UNKNOWN_DOP UINT16_MAX // set unknown DOP's to maximum value, which is also correct for MAVLink #define GPS_WORST_LAG_SEC 0.22f // worst lag value any GPS driver is expected to return, expressed in seconds +#define GPS_MAX_DELTA_MS 245 // 200 ms (5Hz) + 45 ms buffer // the number of GPS leap seconds #define GPS_LEAPSECONDS_MILLIS 18000ULL @@ -301,6 +302,14 @@ public: return last_message_time_ms(primary_instance); } + // system time delta between the last two reported positions + uint16_t last_message_delta_time_ms(uint8_t instance) const { + return timing[instance].delta_time_ms; + } + uint16_t last_message_delta_time_ms(void) const { + return last_message_delta_time_ms(primary_instance); + } + // return true if the GPS supports vertical velocity values bool have_vertical_velocity(uint8_t instance) const { return state[instance].have_vertical_velocity; @@ -388,6 +397,10 @@ public: // indicate which bit in LOG_BITMASK indicates gps logging enabled void set_log_gps_bit(uint32_t bit) { _log_gps_bit = bit; } + // report if the gps is healthy (this is defined as having received an update at a rate greater than 4Hz) + bool is_healthy(uint8_t instance) const { return last_message_delta_time_ms(instance) < GPS_MAX_DELTA_MS; } + bool is_healthy(void) const { return is_healthy(primary_instance); } + protected: // configuration parameters @@ -425,6 +438,9 @@ private: // the time we got our last message in system milliseconds uint32_t last_message_time_ms; + + // delta time between the last pair of GPS updates in system milliseconds + uint16_t delta_time_ms; }; // Note allowance for an additional instance to contain blended data GPS_timing timing[GPS_MAX_RECEIVERS+1];