GPS_Glitch: glitch detection class

libraries/AP_GPS/AP_GPS_Glitch.cpp

@@ -0,0 +1,93 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+
+
+#include <AP_Common.h>
+#include <AP_Math.h>
+#include <AP_HAL.h>
+#include <AP_Notify.h>
+#include "AP_GPS_Glitch.h"
+
+extern const AP_HAL::HAL& hal;
+
+// constuctor
+GPS_Glitch::GPS_Glitch(GPS*& gps) :
+    _gps(gps)
+{
+    _flags.enabled = true;
+}
+
+// check_position - returns true if gps position is acceptable, false if not
+void GPS_Glitch::check_position()
+{
+    uint32_t now = hal.scheduler->millis(); // current system time
+    float sane_dt;                  // time since last sane gps reading
+    float accel_based_distance;     // movement based on max acceleration
+    Location curr_pos;              // our current position estimate
+    Location gps_pos;               // gps reported position
+    float distance_cm;              // distance from gps to current position estimate in cm
+    bool all_ok;                    // true if the new gps position passes sanity checks
+
+    // exit immediately if we don't have gps lock
+    if (_gps == NULL || _gps->status() != GPS::GPS_OK_FIX_3D) {
+        _flags.glitching = true;
+        _flags.recovered = false;
+        return;
+    }
+
+    // if not initialised or disabled update last good position and exit
+    if (!_flags.initialised || !_flags.enabled) {
+        _last_good_update = now;
+        _last_good_lat = _gps->latitude;
+        _last_good_lon = _gps->longitude;
+        _last_good_vel.x = _gps->velocity_north();
+        _last_good_vel.y = _gps->velocity_east();
+        _flags.initialised = true;
+        _flags.glitching = false;
+        _flags.recovered = false;
+        return;
+    }
+
+    // calculate time since last sane gps reading in ms
+    sane_dt = (now - _last_good_update) / 1000.0f;
+
+    // project forward our position from last known velocity
+    curr_pos.lat = _last_good_lat;
+    curr_pos.lng = _last_good_lon;
+    location_offset(curr_pos, _last_good_vel.x * sane_dt, _last_good_vel.y * sane_dt);
+
+    // calculate distance from recent gps position to current estimate
+    gps_pos.lat = _gps->latitude;
+    gps_pos.lng = _gps->longitude;
+    distance_cm = get_distance_cm(curr_pos, gps_pos);
+
+    // all ok if within a given hardcoded radius
+    if (distance_cm <= GPS_GLITCH_MIN_RADIUS_CM) {
+        all_ok = true;
+    }else{
+        // or if within the maximum distance we could have moved based on our acceleration
+        accel_based_distance = 0.5f * GPS_GLITCH_MAX_ACCEL_CMSS * sane_dt * sane_dt;
+        all_ok = (distance_cm <= accel_based_distance);
+    }
+
+    // store updates to gps position
+    if (all_ok) {
+        // position is acceptable
+        _last_good_update = now;
+        _last_good_lat = _gps->latitude;
+        _last_good_lon = _gps->longitude;
+        _last_good_vel.x = _gps->velocity_north();
+        _last_good_vel.y = _gps->velocity_east();
+        // if we were glitching, we have now recovered
+        _flags.recovered = _flags.glitching;
+    }else{
+        _flags.recovered = false;
+    }
+
+    // To-Do: we need to stop consumer from seeing 'recovered' flag multiple times because GPS updates
+    // are much slower than inertial updates
+    
+    // update glitching flag
+    _flags.glitching = !all_ok;
+}
+

libraries/AP_GPS/AP_GPS_Glitch.h

@@ -0,0 +1,65 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/// @file	GPS_Glitch.h
+/// @brief	GPS Glitch protection
+
+#ifndef __GPS_GLITCH_H__
+#define __GPS_GLITCH_H__
+
+#include <AP_HAL.h>
+
+#include <inttypes.h>
+#include <AP_Progmem.h>
+#include <AP_Math.h>
+#include <AP_GPS.h>
+
+#define GPS_GLITCH_MAX_ACCEL_CMSS   1000.0f // vehicle can accelerate at up to 10m/s/s in any direction
+#define GPS_GLITCH_MIN_RADIUS_CM    1000.0f // gps movement within 10m of current position are always ok
+
+/// @class	GPS_Glitch
+/// @brief	GPS Glitch protection class
+class GPS_Glitch
+{
+public:
+    // constructor
+	GPS_Glitch(GPS*& gps);
+
+    // check_position - checks latest gps position against last know position, velocity and maximum acceleration and updates glitching and recovered flags
+    void    check_position();
+
+    // enable - enable or disable gps sanity checking
+    void    enable(bool true_or_false) { _flags.enabled = true_or_false; }
+
+    // enabled - returns true if glitch detection is enabled
+    bool    enabled() { return _flags.enabled; }
+
+    // glitching - returns true if we are experiencing a glitch
+    bool    glitching() { return _flags.glitching; }
+
+    // recovered - returns true if we have just recovered from a glitch
+    bool    recovered() { return _flags.recovered; }
+
+    // last_good_update - returns system time of the last good update
+    uint32_t last_good_update() { return _last_good_update; }
+
+private:
+
+    /// external dependencies
+    GPS*&       _gps;               // pointer to gps
+
+    /// structure for holding flags
+    struct GPS_Glitch_flags {
+        uint8_t initialised : 1; // 1 if we have received at least one good gps lock
+        uint8_t enabled     : 1; // 1 if we are enabled
+        uint8_t glitching   : 1; // 1 if we are experiencing a gps glitch
+        uint8_t recovered   : 1; // 1 if we have just recovered from a glitch
+    } _flags;
+
+    // gps sanity check variables
+    uint32_t    _last_good_update;  // system time of last gps update that passed checks
+    int32_t     _last_good_lat;     // last good latitude reported by the gps
+    int32_t     _last_good_lon;     // last good longitude reported by the gps
+    Vector2f    _last_good_vel;     // last good velocity reported by the gps in cm/s in lat and lon directions
+};
+
+#endif // __GPS_H__