AP_GPS: Allow backends to report their lag confidences

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);
     }
 }
 

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

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:
 

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

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 {

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;