GPSGlitch: parameters for accel and always-ok-radius

libraries/AP_GPS/AP_GPS_Glitch.cpp

@@ -19,6 +19,24 @@ const AP_Param::GroupInfo GPS_Glitch::var_info[] PROGMEM = {
     // @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: 0 5000
+    // @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: 0 2000
+    // @Increment: 100
+    // @User: Advanced
+    AP_GROUPINFO("ACCEL",   2,  GPS_Glitch,   _accel_max_cmss,   GPS_GLITCH_ACCEL_MAX_CMSS),
+
     AP_GROUPEND
 };
 
@@ -72,11 +90,11 @@ void GPS_Glitch::check_position()
     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) {
+    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 * GPS_GLITCH_MAX_ACCEL_CMSS * sane_dt * sane_dt;
+        accel_based_distance = 0.5f * _accel_max_cmss * sane_dt * sane_dt;
         all_ok = (distance_cm <= accel_based_distance);
     }
 

libraries/AP_GPS/AP_GPS_Glitch.h

@@ -13,8 +13,8 @@
 #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
+#define GPS_GLITCH_ACCEL_MAX_CMSS   1000.0f // vehicle can accelerate at up to 10m/s/s in any direction
+#define GPS_GLITCH_RADIUS_CM        1000.0f // gps movement within 10m of current position are always ok
 
 /// @class	GPS_Glitch
 /// @brief	GPS Glitch protection class
@@ -55,6 +55,8 @@ private:
 
     // 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