From 5f8633e3221f9d5970daca8b8b5b39211bb572ee Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Wed, 24 May 2017 10:42:03 -0700 Subject: [PATCH] AP_GPS: Allow backends to report their lag confidences --- libraries/AP_GPS/AP_GPS.cpp | 18 +++++++++++------- libraries/AP_GPS/AP_GPS.h | 1 + libraries/AP_GPS/AP_GPS_SBF.h | 4 ++-- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 18 +++++++++++++----- libraries/AP_GPS/AP_GPS_UBLOX.h | 8 +++++--- libraries/AP_GPS/GPS_Backend.h | 4 ++-- 6 files changed, 34 insertions(+), 19 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index b90bf79f6f..2c138c008f 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -228,6 +228,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Units: ms // @Range: 0 250 // @User: Advanced + // @RebootRequired: True AP_GROUPINFO("DELAY_MS", 18, AP_GPS, _delay_ms[0], 0), // @Param: DELAY_MS2 @@ -236,6 +237,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Units: ms // @Range: 0 250 // @User: Advanced + // @RebootRequired: True AP_GROUPINFO("DELAY_MS2", 19, AP_GPS, _delay_ms[1], 0), // @Param: BLEND_MASK @@ -1052,15 +1054,17 @@ bool AP_GPS::get_lag(uint8_t instance, float &lag_sec) const // the user is always right !! return true; } else if (drivers[instance] == nullptr || state[instance].status == NO_GPS) { - // no GPS was detected in this instance so return a default delay of 1 measurement interval - lag_sec = 0.001f * (float)get_rate_ms(instance); - // check lack of GPS is consistent with user expectation - return state[instance].status == NO_GPS; + // no GPS was detected in this instance so return the worst possible lag term + if (_type[instance] == GPS_TYPE_NONE) { + lag_sec = 0.0f; + return true; + } else { + lag_sec = GPS_WORST_LAG_SEC; + } + return _type[instance] == GPS_TYPE_AUTO; } else { // the user has not specified a delay so we determine it from the GPS type - lag_sec = drivers[instance]->get_lag(); - // check for a valid GPS configuration - return drivers[instance]->is_configured(); + return drivers[instance]->get_lag(lag_sec); } } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 61d5786b94..1657a9303f 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -33,6 +33,7 @@ #define GPS_RTK_INJECT_TO_ALL 127 #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 // the number of GPS leap seconds #define GPS_LEAPSECONDS_MILLIS 18000ULL diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index a2756a7d29..4a7d49316f 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -43,8 +43,8 @@ public: void broadcast_configuration_failure_reason(void) const override; - // return velocity lag - float get_lag(void) const override { return 0.08f; } ; + // get the velocity lag, returns true if the driver is confident in the returned value + bool get_lag(float &lag_sec) const override { lag_sec = 0.08f; return true; } ; private: diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 8af3af9f76..3a9b4b9cd7 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -59,7 +59,7 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART _ublox_port(255), _have_version(false), _unconfigured_messages(CONFIG_ALL), - _hardware_generation(0), + _hardware_generation(UBLOX_UNKNOWN_HARDWARE_GENERATION), _new_position(0), _new_speed(0), _disable_counter(0), @@ -104,7 +104,7 @@ AP_GPS_UBLOX::_request_next_config(void) break; case STEP_POLL_SVINFO: // not required once we know what generation we are on - if(_hardware_generation == 0) { + if(_hardware_generation == UBLOX_UNKNOWN_HARDWARE_GENERATION) { if (!_send_message(CLASS_NAV, MSG_NAV_SVINFO, 0, 0)) { _next_message--; } @@ -1307,19 +1307,27 @@ AP_GPS_UBLOX::broadcast_configuration_failure_reason(void) const { /* return velocity lag in seconds */ -float AP_GPS_UBLOX::get_lag(void) const +bool AP_GPS_UBLOX::get_lag(float &lag_sec) const { switch (_hardware_generation) { + case UBLOX_UNKNOWN_HARDWARE_GENERATION: + lag_sec = 0.22f; + // always bail out in this case, it's used to indicate we have yet to receive a valid + // hardware generation, however the user may have inhibited us detecting the generation + // so if we aren't allowed to do configuration, we will accept this as the default delay + return gps._auto_config != 1; case UBLOX_5: case UBLOX_6: default: - return 0.22f; + lag_sec = 0.22f; + break; case UBLOX_7: case UBLOX_M8: // based on flight logs the 7 and 8 series seem to produce about 120ms lag - return 0.12f; + lag_sec = 0.12f; break; }; + return true; } void AP_GPS_UBLOX::Write_DataFlash_Log_Startup_messages() const diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 9705042c2a..d4024ec247 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -116,8 +116,8 @@ public: void broadcast_configuration_failure_reason(void) const override; void Write_DataFlash_Log_Startup_messages() const override; - // return velocity lag - float get_lag(void) const override; + // get the velocity lag, returns true if the driver is confident in the returned value + bool get_lag(float &lag_sec) const override; const char *name() const override { return "u-blox"; } @@ -477,7 +477,9 @@ private: UBLOX_5, UBLOX_6, UBLOX_7, - UBLOX_M8 + UBLOX_M8, + UBLOX_UNKNOWN_HARDWARE_GENERATION = 0xff // not in the ublox spec used for + // flagging state in the driver }; enum config_step { diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 8094cfafb1..1606d45c9f 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -53,8 +53,8 @@ public: virtual void handle_msg(const mavlink_message_t *msg) { return ; } virtual void handle_gnss_msg(const AP_GPS::GPS_State &msg) { return ; } - // driver specific lag - virtual float get_lag(void) const { return 0.2f; } + // driver specific lag, returns true if the driver is confident in the provided lag + virtual bool get_lag(float &lag) const { lag = 0.2f; return true; } virtual const char *name() const = 0;