From 4dcfce71041ecff4f5a493837be42bb9b9d48ca7 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 19 Sep 2013 15:52:13 +0900 Subject: [PATCH] GPS_Glitch: glitch detection class --- libraries/AP_GPS/AP_GPS_Glitch.cpp | 93 ++++++++++++++++++++++++++++++ libraries/AP_GPS/AP_GPS_Glitch.h | 65 +++++++++++++++++++++ 2 files changed, 158 insertions(+) create mode 100644 libraries/AP_GPS/AP_GPS_Glitch.cpp create mode 100644 libraries/AP_GPS/AP_GPS_Glitch.h diff --git a/libraries/AP_GPS/AP_GPS_Glitch.cpp b/libraries/AP_GPS/AP_GPS_Glitch.cpp new file mode 100644 index 0000000000..5bfa5a4ec0 --- /dev/null +++ b/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 +#include +#include +#include +#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; +} + diff --git a/libraries/AP_GPS/AP_GPS_Glitch.h b/libraries/AP_GPS/AP_GPS_Glitch.h new file mode 100644 index 0000000000..4be42c5f34 --- /dev/null +++ b/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 + +#include +#include +#include +#include + +#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__