Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Track the delta time between the last GPS positions

Browse Source
This commit is contained in:
Michael du Breuil 2017-07-08 16:06:19 -07:00 committed by Francisco Ferreira
parent ebea292d63
commit 04eb7f411e
2 changed files with 22 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_GPS/AP_GPS.cpp
View File

@ -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;

16
libraries/AP_GPS/AP_GPS.h
View File

@ -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];