Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Baro: remove glitch detection

Browse Source
This commit is contained in:
Randy Mackay 2015-03-12 20:11:09 +09:00
parent d7f624be39
commit 4e7d92094c
2 changed files with 0 additions and 192 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

121
libraries/AP_Baro/AP_Baro_Glitch.cpp
View File

@ -1,121 +0,0 @@
// -*- 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
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;
}

71
libraries/AP_Baro/AP_Baro_Glitch.h
View File

@ -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 <AP_HAL.h>
#include <inttypes.h>
#include <AP_Progmem.h>
#include <AP_Math.h>
#include <AP_Baro.h>
#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__