Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Airspeed: use non-blocking airspeed calibration

Browse Source 
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_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL &hal;
@ -174,8 +175,6 @@ bool AP_Airspeed::get_temperature(float &temperature)
// the get_airspeed() interface can be used
void AP_Airspeed::calibrate(bool in_startup)
{
float sum = 0;
uint8_t count = 0;
if (!_enable) {
return;
}
@ -184,24 +183,27 @@ void AP_Airspeed::calibrate(bool in_startup)
}
// discard first reading
get_pressure();
for (uint8_t i = 0; i < 10; i++) {
hal.scheduler->delay(100);
float p = get_pressure();
if (_healthy) {
sum += p;
count++;
_cal.start_ms = AP_HAL::millis();
_cal.count = 0;
_cal.sum = 0;
}
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);
}
}
if (count == 0) {
// unhealthy sensor
hal.console->println("Airspeed sensor unhealthy");
_offset.set(0);
_cal.start_ms = 0;
return;
}
float raw = sum/count;
_offset.set_and_save(raw);
_airspeed = 0;
_raw_airspeed = 0;
if (_healthy) {
_cal.sum += raw_pressure;
_cal.count++;
}
}
// read the airspeed sensor
@ -211,10 +213,15 @@ void AP_Airspeed::read(void)
if (!_enable) {
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
_raw_pressure = airspeed_pressure;
_corrected_pressure = airspeed_pressure;
/*
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 the current raw pressure
float get_raw_pressure(void) const {
return _raw_pressure;
// return the current corrected pressure
float get_corrected_pressure(void) const {
return _corrected_pressure;
}
// set the apparent to true airspeed ratio
@ -164,18 +164,26 @@ private:
float _raw_airspeed;
float _airspeed;
float _last_pressure;
float _raw_pressure;
float _corrected_pressure;
float _EAS2TAS;
bool _healthy:1;
bool _hil_set:1;
float _hil_pressure;
uint32_t _last_update_ms;
// state of runtime calibration
struct {
uint32_t start_ms;
uint16_t count;
float sum;
} _cal;
Airspeed_Calibration _calibration;
float _last_saved_ratio;
uint8_t _counter;
float get_pressure(void);
void update_calibration(float raw_pressure);
AP_Airspeed_Analog analog;
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN