diff --git a/libraries/AP_GPS/AP_GPS_Glitch.cpp b/libraries/AP_GPS/AP_GPS_Glitch.cpp deleted file mode 100644 index d2844c3309..0000000000 --- a/libraries/AP_GPS/AP_GPS_Glitch.cpp +++ /dev/null @@ -1,121 +0,0 @@ -// -*- 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; - -// table of user settable parameters -const AP_Param::GroupInfo GPS_Glitch::var_info[] PROGMEM = { - // @Param: ENABLE - // @DisplayName: GPS Glitch protection enable/disable - // @Description: Allows you to enable (1) or disable (0) gps glitch protection - // @Values: 0:Disabled,1:Enabled - // @User: Standard - AP_GROUPINFO("ENABLE", 0, GPS_Glitch, _enabled, 1), - - // @Param: RADIUS - // @DisplayName: GPS glitch protection radius within which all new positions are accepted - // @Description: GPS glitch protection radius within which all new positions are accepted - // @Units: cm - // @Range: 100 2000 - // @Increment: 100 - // @User: Advanced - AP_GROUPINFO("RADIUS", 1, GPS_Glitch, _radius_cm, GPS_GLITCH_RADIUS_CM), - - // @Param: ACCEL - // @DisplayName: GPS glitch protection's max vehicle acceleration assumption - // @Description: GPS glitch protection's max vehicle acceleration assumption - // @Units: cm/s/s - // @Range: 100 2000 - // @Increment: 100 - // @User: Advanced - AP_GROUPINFO("ACCEL", 2, GPS_Glitch, _accel_max_cmss, GPS_GLITCH_ACCEL_MAX_CMSS), - - AP_GROUPEND -}; - -// constuctor -GPS_Glitch::GPS_Glitch(const AP_GPS &gps) : - _gps(gps), - _last_good_update(0), - _last_good_lat(0), - _last_good_lon(0) -{ - AP_Param::setup_object_defaults(this, var_info); -} - -// 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.status() < AP_GPS::GPS_OK_FIX_3D) { - _flags.glitching = true; - return; - } - - // if not initialised or disabled update last good position and exit - if (!_flags.initialised || !_enabled) { - const Location &loc = _gps.location(); - const Vector3f &vel = _gps.velocity(); - _last_good_update = now; - _last_good_lat = loc.lat; - _last_good_lon = loc.lng; - _last_good_vel.x = vel.x; - _last_good_vel.y = vel.y; - _flags.initialised = true; - _flags.glitching = 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 - const Location &loc = _gps.location(); - gps_pos.lat = loc.lat; - gps_pos.lng = loc.lng; - distance_cm = get_distance_cm(curr_pos, gps_pos); - - // all ok if within a given hardcoded radius - if (distance_cm <= _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 * _accel_max_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 = loc.lat; - _last_good_lon = loc.lng; - const Vector3f &vel = _gps.velocity(); - _last_good_vel.x = vel.x; - _last_good_vel.y = vel.y; - } - - // 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 deleted file mode 100644 index 5e7a2ea2a0..0000000000 --- a/libraries/AP_GPS/AP_GPS_Glitch.h +++ /dev/null @@ -1,68 +0,0 @@ -// -*- 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_ACCEL_MAX_CMSS 1000.0f // vehicle can accelerate at up to 5m/s/s in any direction -#define GPS_GLITCH_RADIUS_CM 200.0f // gps movement within 2m of current position are always ok - -/// @class GPS_Glitch -/// @brief GPS Glitch protection class -class GPS_Glitch -{ -public: - // constructor - GPS_Glitch(const AP_GPS &gps); - - // check_position - checks latest gps position against last know position, velocity and maximum acceleration and updates glitching flag - void check_position(); - - // enable - enable or disable gps sanity checking - void enable(bool true_or_false) { _enabled = true_or_false; } - - // enabled - returns true if glitch detection is enabled - bool enabled() const { return _enabled; } - - // glitching - returns true if we are experiencing a glitch - bool glitching() const { return _flags.glitching; } - - // last_good_update - returns system time of the last good update - uint32_t last_good_update() const { return _last_good_update; } - - // class level parameters - static const struct AP_Param::GroupInfo var_info[]; - -private: - - /// external dependencies - const AP_GPS &_gps; // reference 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 glitching : 1; // 1 if we are experiencing a gps glitch - } _flags; - - // parameters - AP_Int8 _enabled; // top level enable/disable control - AP_Float _radius_cm; // radius in cm within which all new positions from GPS are accepted - AP_Float _accel_max_cmss; // vehicles maximum acceleration in cm/s/s - - // 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__