You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
113 lines
3.7 KiB
113 lines
3.7 KiB
// -*- 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), |
|
_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 |
|
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) { |
|
_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; |
|
} |
|
|
|
|