From 4e7d92094c8d74aa4e896a2538cc33f093cddfe6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 12 Mar 2015 20:11:09 +0900 Subject: [PATCH] Baro: remove glitch detection --- libraries/AP_Baro/AP_Baro_Glitch.cpp | 121 --------------------------- libraries/AP_Baro/AP_Baro_Glitch.h | 71 ---------------- 2 files changed, 192 deletions(-) delete mode 100644 libraries/AP_Baro/AP_Baro_Glitch.cpp delete 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 deleted file mode 100644 index 45b87f80f5..0000000000 --- a/libraries/AP_Baro/AP_Baro_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_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), - _last_good_update(0), - _last_good_alt(0), - _last_good_vel(0.0f) -{ - AP_Param::setup_object_defaults(this, var_info); - - // initialise flags - _flags.initialised = 0; - _flags.glitching = 0; -} - -// 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 - int32_t 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()) { - _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; - - // baro may have become unhealthy when calculating altitude - if (!_baro.healthy()) { - _flags.glitching = true; - return; - } - - // calculte distance from projected distance - 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 deleted file mode 100644 index 9673fab449..0000000000 --- a/libraries/AP_Baro/AP_Baro_Glitch.h +++ /dev/null @@ -1,71 +0,0 @@ -// -*- 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 1500.0f // vehicle can accelerate at up to 15m/s/s vertically -#define BARO_GLITCH_DISTANCE_OK_CM 500.0f // baro movement within 5m 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; } - - // reset - resets glitch detection to start from current baro alt - // should be called after barometer altitude is reset during arming - void reset() { _flags.initialised = false; } - - // 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__