Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Baro_Glitch: initial version of baro glitch protection
Based on GPS Glitch protection library
- Loading branch information
Showing
2 changed files
with
176 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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; | ||
} | ||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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__ |