Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Airspeed: use non-blocking airspeed calibration 

this prevents the EKF from being upset by a time jump
This commit is contained in:
Andrew Tridgell 2016-05-24 14:22:40 +10:00
parent d3494d1369
commit 4131b98f8c
2 changed files with 38 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_Airspeed/AP_Airspeed.cpp
View File

@ -22,6 +22,7 @@
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL &hal; extern const AP_HAL::HAL &hal;
@ -174,8 +175,6 @@ bool AP_Airspeed::get_temperature(float &temperature)
// the get_airspeed() interface can be used // the get_airspeed() interface can be used
void AP_Airspeed::calibrate(bool in_startup) void AP_Airspeed::calibrate(bool in_startup)
{ {
float sum = 0;
uint8_t count = 0;
if (!_enable) { if (!_enable) {
return; return;
} }
@ -184,24 +183,27 @@ void AP_Airspeed::calibrate(bool in_startup)
} }
// discard first reading // discard first reading
get_pressure(); get_pressure();
for (uint8_t i = 0; i < 10; i++) { _cal.start_ms = AP_HAL::millis();
hal.scheduler->delay(100); _cal.count = 0;
float p = get_pressure(); _cal.sum = 0;
if (_healthy) { }
sum += p;
count++; void AP_Airspeed::update_calibration(float raw_pressure)
{
if (AP_HAL::millis() - _cal.start_ms >= 1000) {
if (_cal.count == 0) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Airspeed sensor unhealthy");
} else {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Airspeed sensor calibrated");
_offset.set_and_save(_cal.sum / _cal.count);
} }
} _cal.start_ms = 0;
if (count == 0) {
// unhealthy sensor
hal.console->println("Airspeed sensor unhealthy");
_offset.set(0);
return; return;
} }
float raw = sum/count; if (_healthy) {
_offset.set_and_save(raw); _cal.sum += raw_pressure;
_airspeed = 0; _cal.count++;
_raw_airspeed = 0; }
} }
// read the airspeed sensor // read the airspeed sensor
@ -211,10 +213,15 @@ void AP_Airspeed::read(void)
if (!_enable) { if (!_enable) {
return; return;
} }
airspeed_pressure = get_pressure() - _offset; float raw_pressure = get_pressure();
if (_cal.start_ms != 0) {
update_calibration(raw_pressure);
}
airspeed_pressure = raw_pressure - _offset;
// remember raw pressure for logging // remember raw pressure for logging
_raw_pressure = airspeed_pressure; _corrected_pressure = airspeed_pressure;
/* /*
we support different pitot tube setups so used can choose if we support different pitot tube setups so used can choose if

16
libraries/AP_Airspeed/AP_Airspeed.h
View File

@ -115,9 +115,9 @@ public:
return _offset; return _offset;
} }
// return the current raw pressure // return the current corrected pressure
float get_raw_pressure(void) const { float get_corrected_pressure(void) const {
return _raw_pressure; return _corrected_pressure;
} }
// set the apparent to true airspeed ratio // set the apparent to true airspeed ratio
@ -164,18 +164,26 @@ private:
float _raw_airspeed; float _raw_airspeed;
float _airspeed; float _airspeed;
float _last_pressure; float _last_pressure;
float _raw_pressure; float _corrected_pressure;
float _EAS2TAS; float _EAS2TAS;
bool _healthy:1; bool _healthy:1;
bool _hil_set:1; bool _hil_set:1;
float _hil_pressure; float _hil_pressure;
uint32_t _last_update_ms; uint32_t _last_update_ms;
// state of runtime calibration
struct {
uint32_t start_ms;
uint16_t count;
float sum;
} _cal;
Airspeed_Calibration _calibration; Airspeed_Calibration _calibration;
float _last_saved_ratio; float _last_saved_ratio;
uint8_t _counter; uint8_t _counter;
float get_pressure(void); float get_pressure(void);
void update_calibration(float raw_pressure);
AP_Airspeed_Analog analog; AP_Airspeed_Analog analog;
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN