From 2289b98d34532f4adafaef29ac63e9e8ca5db228 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 28 Jul 2014 17:33:30 +0900 Subject: [PATCH] Baro_Glitch: initial version of baro glitch protection Based on GPS Glitch protection library --- libraries/AP_Baro/AP_Baro_Glitch.cpp | 109 +++++++++++++++++++++++++++ libraries/AP_Baro/AP_Baro_Glitch.h | 67 ++++++++++++++++ 2 files changed, 176 insertions(+) create mode 100644 libraries/AP_Baro/AP_Baro_Glitch.cpp create mode 100644 libraries/AP_Baro/AP_Baro_Glitch.h diff --git a/libraries/AP_Baro/AP_Baro_Glitch.cpp b/libraries/AP_Baro/AP_Baro_Glitch.cpp new file mode 100644 index 0000000000000..db902a23fa04f --- /dev/null +++ b/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 +#include +#include +#include +#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; +} + diff --git a/libraries/AP_Baro/AP_Baro_Glitch.h b/libraries/AP_Baro/AP_Baro_Glitch.h new file mode 100644 index 0000000000000..f3b9e34811781 --- /dev/null +++ b/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 + +#include +#include +#include +#include + +#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__