Baro_Glitch: initial version of baro glitch protection

Based on GPS Glitch protection library

libraries/AP_Baro/AP_Baro_Glitch.cpp

@@ -0,0 +1,109 @@
+// -*- 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_Baro_Glitch.h"
+
+extern const AP_HAL::HAL& hal;
+
+// table of user settable parameters
+const AP_Param::GroupInfo Baro_Glitch::var_info[] PROGMEM = {
+    // @Param: ENABLE
+    // @DisplayName: Baro Glitch protection enable/disable
+    // @Description: Allows you to enable (1) or disable (0) baro glitch protection
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Standard
+    AP_GROUPINFO("ENABLE",      0,  Baro_Glitch,   _enabled,   1),
+
+    // @Param: DIST
+    // @DisplayName: Baro glitch protection distance within which alt update is immediately accepted
+    // @Description: Baro glitch protection distance within which alt update is immediately accepted
+    // @Units: cm
+    // @Range: 100 2000
+    // @Increment: 100
+    // @User: Advanced
+    AP_GROUPINFO("DIST",  1,  Baro_Glitch,   _dist_ok_cm,   BARO_GLITCH_DISTANCE_OK_CM),
+
+    // @Param: ACCEL
+    // @DisplayName: Baro glitch protection's max vehicle acceleration assumption
+    // @Description: Baro glitch protection's max vehicle acceleration assumption
+    // @Units: cm/s/s
+    // @Range: 100 2000
+    // @Increment: 100
+    // @User: Advanced
+    AP_GROUPINFO("ACCEL",   2,  Baro_Glitch,    _accel_max_cmss,   BARO_GLITCH_ACCEL_MAX_CMSS),
+
+    AP_GROUPEND
+};
+
+// constuctor
+Baro_Glitch::Baro_Glitch(AP_Baro &baro) :
+    _baro(baro)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+// check_alt - checks latest baro altitude against last know alt, velocity and maximum acceleration and updates glitching flag
+void Baro_Glitch::check_alt()
+{
+    uint32_t now = hal.scheduler->millis(); // current system time
+    float sane_dt;                  // time since last sane baro reading
+    float accel_based_distance;     // movement based on max acceleration
+    int32_t alt_projected;          // altitude estimate projected from previous iteration
+    float distance_cm;              // distance from baro alt to current alt estimate in cm
+    bool all_ok;                    // true if the new baro alt passes sanity checks
+
+    // exit immediately if baro is unhealthy
+    if (!_baro.healthy) {
+        if (!_flags.glitching) {
+            hal.console->printf_P(PSTR("\nPGlitching 'cuz unhealthy!"));
+        }
+        _flags.glitching = true;
+        return;
+    }
+
+    // if not initialised or disabled update last good alt and exit
+    if (!_flags.initialised || !_enabled) {
+        _last_good_update = now;
+        _last_good_alt = _baro.get_altitude();
+        _last_good_vel = _baro.get_climb_rate();
+        _flags.initialised = true;
+        _flags.glitching = false;
+        return;
+    }
+
+    // calculate time since last sane baro reading in ms
+    sane_dt = (now - _last_good_update) / 1000.0f;
+
+    // estimate our alt from last known alt and velocity
+    alt_projected = _last_good_alt + (_last_good_vel * sane_dt);
+
+    // calculate distance from recent baro alt to current estimate
+    int32_t baro_alt = _baro.get_altitude() * 100.0f;
+    distance_cm = labs(alt_projected - baro_alt);
+
+    // all ok if within a given hardcoded radius
+    if (distance_cm <= _dist_ok_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 baro position
+    if (all_ok) {
+        // position is acceptable
+        _last_good_update = now;
+        _last_good_alt = baro_alt;
+        _last_good_vel = _baro.get_climb_rate();
+    }
+
+    // update glitching flag
+    _flags.glitching = !all_ok;
+}
+

libraries/AP_Baro/AP_Baro_Glitch.h

@@ -0,0 +1,67 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/// @file	AP_Baro_Glitch.h
+/// @brief	Barometer Glitch protection
+
+#ifndef __BARO_GLITCH_H__
+#define __BARO_GLITCH_H__
+
+#include <AP_HAL.h>
+
+#include <inttypes.h>
+#include <AP_Progmem.h>
+#include <AP_Math.h>
+#include <AP_Baro.h>
+
+#define BARO_GLITCH_ACCEL_MAX_CMSS  1000.0f // vehicle can accelerate at up to 10m/s/s vertically
+#define BARO_GLITCH_DISTANCE_OK_CM  200.0f  // baro movement within 2m of current position is always ok
+
+/// @class	Baro_Glitch
+/// @brief	Baro Glitch protection class
+class Baro_Glitch
+{
+public:
+    // constructor
+	Baro_Glitch(AP_Baro &baro);
+
+    // check_alt - checks latest baro altitude against last know alt, velocity and maximum acceleration and updates glitching flag
+    void    check_alt();
+
+    // enable - enable or disable baro 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
+    AP_Baro     &_baro;             // reference to barometer
+
+    /// structure for holding flags
+    struct Baro_Glitch_flags {
+        uint8_t initialised : 1;    // 1 if we have received at least one good baro alt
+        uint8_t glitching   : 1;    // 1 if we are experiencing a baro glitch
+    } _flags;
+
+    // parameters
+    AP_Int8     _enabled;           // top level enable/disable control
+    AP_Float    _dist_ok_cm;        // distance in cm within which all new positions from Baro are accepted
+    AP_Float    _accel_max_cmss;    // vehicles maximum acceleration in cm/s/s
+
+    // baro sanity check variables
+    uint32_t    _last_good_update;  // system time of last baro update that passed checks
+    int32_t     _last_good_alt;     // last good altitude reported by the baro
+    float       _last_good_vel;     // last good velocity reported by the baro in cm/s
+};
+
+#endif // __BARO_GLITCH_H__