mirror of https://github.com/ArduPilot/ardupilot
GPS_Glitch: glitch detection class
This commit is contained in:
parent
cc00ac96ca
commit
4dcfce7104
|
@ -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;
|
||||
}
|
||||
|
|
@ -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__
|
Loading…
Reference in New Issue