INav: record error_count when GPS msg is late

libraries/AP_InertialNav/AP_InertialNav.cpp

@@ -39,6 +39,11 @@ void AP_InertialNav::update(float dt)
         return;
     }
 
+    // decrement ignore error count if required
+    if (_flags.ignore_error > 0) {
+        _flags.ignore_error--;
+    }
+
     // check if new baro readings have arrived and use them to correct vertical accelerometer offsets.
     check_baro();
 
@@ -137,6 +142,10 @@ void AP_InertialNav::check_gps()
         if (now - _gps_last_update > AP_INTERTIALNAV_GPS_TIMEOUT_MS) {
             _position_error.x *= 0.9886;
             _position_error.y *= 0.9886;
+            // increment error count
+            if (_flags.ignore_error == 0 && _error_count < 255 && _xy_enabled) {
+                _error_count++;
+            }
         }
     }
 }

libraries/AP_InertialNav/AP_InertialNav.h

@@ -43,7 +43,8 @@ public:
         _gps_last_time(0),
         _historic_xy_counter(0),
         _baro_last_update(0),
-        _glitch_detector(gps_glitch)
+        _glitch_detector(gps_glitch),
+        _error_count(0)
         {
             AP_Param::setup_object_defaults(this, var_info);
         }
@@ -235,6 +236,16 @@ public:
      */
     void        set_velocity_z( float new_velocity );
 
+    /**
+     * error_count - returns number of missed updates from GPS
+     */
+    uint8_t     error_count() const { return _error_count; }
+
+    /**
+     * ignore_next_error - the next error (if it occurs immediately) will not be added to the error count
+     */
+    void        ignore_next_error() { _flags.ignore_error = 7; }
+
     // class level parameters
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -265,7 +276,8 @@ protected:
 
     // structure for holding flags
     struct InertialNav_flags {
-        uint8_t gps_glitching   : 1;                    // 1 if glitch detector was previously indicating a gps glitch
+        uint8_t gps_glitching       : 1;                // 1 if glitch detector was previously indicating a gps glitch
+        uint8_t ignore_error        : 3;                // the number of iterations for which we should ignore errors
     } _flags;
 
     const AP_AHRS*    const _ahrs;                      // pointer to ahrs object
@@ -302,8 +314,10 @@ protected:
     Vector3f                _position_error;            // current position error in cm - is set by the check_* methods and used by update method to calculate the correction terms
     Vector3f                _position;                  // sum(_position_base, _position_correction) - corrected position estimate in cm - relative to the home location (_base_lat, _base_lon, 0)
 
-    // GPS Glitch detector
-    GPS_Glitch&             _glitch_detector;
+    // error handling
+    GPS_Glitch&             _glitch_detector;           // GPS Glitch detector
+    uint8_t                 _error_count;               // number of missed GPS updates
+
 };
 
 #endif // __AP_INERTIALNAV_H__