diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index a3b3bba8cf..522c7f16e1 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -32,44 +32,12 @@ extern const AP_HAL::HAL &hal; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #define ARSPD_DEFAULT_PIN 1 -#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - #include - #include - #include - #include - #include - #include - #include -#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) - #define ARSPD_DEFAULT_PIN 0 -#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) - #define ARSPD_DEFAULT_PIN 0 -#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) - #define ARSPD_DEFAULT_PIN 0 -#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51) - #define ARSPD_DEFAULT_PIN 0 -#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) - #define ARSPD_DEFAULT_PIN 0 -#elif defined(CONFIG_ARCH_BOARD_VRCORE_V10) - #define ARSPD_DEFAULT_PIN 0 -#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V54) - #define ARSPD_DEFAULT_PIN 0 -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - #define ARSPD_DEFAULT_PIN 11 #else - #define ARSPD_DEFAULT_PIN 15 + #define ARSPD_DEFAULT_PIN AP_AIRSPEED_I2C_PIN #endif -#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX - #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO - #define ARSPD_DEFAULT_PIN 5 - #else - #define ARSPD_DEFAULT_PIN AP_AIRSPEED_I2C_PIN - #endif - #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO - #define PSI_RANGE_DEFAULT 0.05 - #endif -#else - #define ARSPD_DEFAULT_PIN 0 + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO +#define PSI_RANGE_DEFAULT 0.05 #endif #ifndef PSI_RANGE_DEFAULT diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 4248c0e067..b775529876 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -8,7 +8,6 @@ #include "AP_Airspeed_Backend.h" #include "AP_Airspeed_I2C.h" -#include "AP_Airspeed_PX4.h" #include "AP_Airspeed_analog.h" class Airspeed_Calibration { @@ -180,9 +179,5 @@ private: void update_calibration(float raw_pressure); AP_Airspeed_Analog analog{_pin, _psi_range}; -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - AP_Airspeed_PX4 digital{_psi_range}; -#else AP_Airspeed_I2C digital{_psi_range}; -#endif }; diff --git a/libraries/AP_Airspeed/AP_Airspeed_I2C.cpp b/libraries/AP_Airspeed/AP_Airspeed_I2C.cpp index 4dddd6e34d..694764b1fc 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_I2C.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_I2C.cpp @@ -50,13 +50,20 @@ bool AP_Airspeed_I2C::init() return false; } + // lots of retries during probe + _dev->set_retries(5); + _measure(); hal.scheduler->delay(10); _collect(); _dev->get_semaphore()->give(); + // drop to 2 retries for runtime + _dev->set_retries(2); + if (_last_sample_time_ms != 0) { - hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Airspeed_I2C::_timer, void)); + _dev->register_periodic_callback(20000, + FUNCTOR_BIND_MEMBER(&AP_Airspeed_I2C::_timer, bool)); return true; } return false; @@ -110,27 +117,47 @@ void AP_Airspeed_I2C::_collect() _pressure = diff_press_PSI * PSI_to_Pa; _temperature = ((200.0f * dT_raw) / 2047) - 50; + _voltage_correction(_pressure, _temperature); + _last_sample_time_ms = AP_HAL::millis(); } -// 1kHz timer -void AP_Airspeed_I2C::_timer() -{ - if (!_dev->get_semaphore()->take_nonblocking()) { - return; - } +/** + correct for 5V rail voltage if the system_power ORB topic is + available + See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of + offset versus voltage for 3 sensors + */ +void AP_Airspeed_I2C::_voltage_correction(float &diff_press_pa, float &temperature) +{ + const float slope = 65.0f; + const float temp_slope = 0.887f; + + /* + apply a piecewise linear correction within range given by above graph + */ + float voltage_diff = hal.analogin->board_voltage() - 5.0f; + + voltage_diff = constrain_float(voltage_diff, -0.7f, 0.5f); + + diff_press_pa -= voltage_diff * slope; + temperature -= voltage_diff * temp_slope; +} + +// 50Hz timer +bool AP_Airspeed_I2C::_timer() +{ if (_measurement_started_ms == 0) { _measure(); - _dev->get_semaphore()->give(); - return; + return true; } if ((AP_HAL::millis() - _measurement_started_ms) > 10) { _collect(); // start a new measurement _measure(); } - _dev->get_semaphore()->give(); + return true; } // return the current differential_pressure in Pascal diff --git a/libraries/AP_Airspeed/AP_Airspeed_I2C.h b/libraries/AP_Airspeed/AP_Airspeed_I2C.h index 0eae1a87c6..976866a708 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_I2C.h +++ b/libraries/AP_Airspeed/AP_Airspeed_I2C.h @@ -45,7 +45,9 @@ public: private: void _measure(); void _collect(); - void _timer(); + bool _timer(); + void _voltage_correction(float &diff_press_pa, float &temperature); + float _temperature; float _pressure; uint32_t _last_sample_time_ms; diff --git a/libraries/AP_Airspeed/AP_Airspeed_PX4.cpp b/libraries/AP_Airspeed/AP_Airspeed_PX4.cpp deleted file mode 100644 index c69f5c7485..0000000000 --- a/libraries/AP_Airspeed/AP_Airspeed_PX4.cpp +++ /dev/null @@ -1,88 +0,0 @@ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ -/* - * PX4 airspeed backend - */ - - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - -#include "AP_Airspeed_PX4.h" - -#include -#include -#include -#include -#include -#include - -extern const AP_HAL::HAL &hal; - -bool AP_Airspeed_PX4::init() -{ - _fd = open(AIRSPEED0_DEVICE_PATH, O_RDONLY); - if (_fd == -1) { - return false; - } - if (OK != ioctl(_fd, SENSORIOCSPOLLRATE, 100) || - OK != ioctl(_fd, SENSORIOCSQUEUEDEPTH, 15)) { - hal.console->println("Failed to setup airspeed driver rate and queue"); - } - return true; -} - -// read the airspeed sensor -bool AP_Airspeed_PX4::get_differential_pressure(float &pressure) -{ - if (_fd == -1) { - return false; - } - - // read from the PX4 airspeed sensor - float psum = 0; - float tsum = 0; - uint16_t count = 0; - struct differential_pressure_s report; - - while (::read(_fd, &report, sizeof(report)) == sizeof(report) && - report.timestamp != _last_timestamp) { - psum += report.differential_pressure_raw_pa / _psi_range.get(); - tsum += report.temperature; - count++; - _last_timestamp = report.timestamp; - } - if (count == 0) { - return false; - } - pressure = psum / count; - _temperature = tsum / count; - return true; -} - -// read the temperature -bool AP_Airspeed_PX4::get_temperature(float &temperature) -{ - if (_temperature < -80) { - // almost certainly a bad reading. The ETS driver on PX4 - // returns -1000 - return false; - } - temperature = _temperature; - return true; -} - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_Airspeed/AP_Airspeed_PX4.h b/libraries/AP_Airspeed/AP_Airspeed_PX4.h deleted file mode 100644 index 4766920058..0000000000 --- a/libraries/AP_Airspeed/AP_Airspeed_PX4.h +++ /dev/null @@ -1,45 +0,0 @@ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ -#pragma once - -/* - backend driver for airspeed from PX4Firmware - */ - -#include -#include -#include "AP_Airspeed_Backend.h" - -class AP_Airspeed_PX4 : public AP_Airspeed_Backend { -public: - // constructor - AP_Airspeed_PX4(const AP_Float &psi_range) : - _psi_range(psi_range) {} - - // probe and initialise the sensor - bool init(void); - - // return the current differential_pressure in Pascal - bool get_differential_pressure(float &pressure); - - // return the current temperature in degrees C, if available - bool get_temperature(float &temperature); - -private: - int _fd = -1; - uint64_t _last_timestamp; - float _temperature; - const AP_Float &_psi_range; -};