AP_Airspeed: split up airspeed driver

this gives us separate backends for PX4, analog and I2C. This allows
the MS airspeed sensor to work on Linux, and it should work on APM2 as well.

libraries/AP_Airspeed/AP_Airspeed.cpp

@@ -29,7 +29,6 @@ extern const AP_HAL::HAL& hal;
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
  #include <AP_ADC_AnalogSource.h>
  #define ARSPD_DEFAULT_PIN 64
- extern AP_ADC_ADS7844 apm1_adc;
 #elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
  #define ARSPD_DEFAULT_PIN 0
 #elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
@@ -49,6 +48,8 @@ extern const AP_HAL::HAL& hal;
 #endif
 #elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
  #define ARSPD_DEFAULT_PIN 16
+#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+ #define ARSPD_DEFAULT_PIN 65
 #else
  #define ARSPD_DEFAULT_PIN 0
 #endif
@@ -110,28 +111,8 @@ void AP_Airspeed::init()
     _last_saved_ratio = _ratio;
     _counter = 0;
     
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
-    if (_pin == 65) {
-        _ets_fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
-        if (_ets_fd == -1) {
-            hal.console->println("Failed to open ETS airspeed driver");
-            _enable.set(0);
-            return;
-        }
-        if (OK != ioctl(_ets_fd, SENSORIOCSPOLLRATE, 100) ||
-            OK != ioctl(_ets_fd, SENSORIOCSQUEUEDEPTH, 15)) {
-            hal.console->println("Failed to setup ETS driver rate and queue");
-        }
-        return;
-    }
-#endif
-#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
-    if (_pin == 64) {
-        _source = new AP_ADC_AnalogSource( &apm1_adc, 7, 1.0f);
-        return;
-    }
-#endif
-    _source = hal.analogin->channel(_pin);
+    analog.init();
+    digital.init();
 }
 
 // read the airspeed sensor
@@ -140,36 +121,13 @@ float AP_Airspeed::get_pressure(void)
     if (!_enable) {
         return 0;
     }
-
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
-    if (_ets_fd != -1) {
-        // read from the ETS airspeed sensor
-        float sum = 0;
-        uint16_t count = 0;
-        struct differential_pressure_s report;
-        static uint64_t last_timestamp;
-
-        while (::read(_ets_fd, &report, sizeof(report)) == sizeof(report) &&
-               report.timestamp != last_timestamp) {
-            sum += report.differential_pressure_pa;
-            count++;
-            last_timestamp = report.timestamp;
+    float pressure = 0;
+    if (_pin == 65) {
+        _healthy = digital.get_differential_pressure(pressure);
+    } else {
+        _healthy = analog.get_differential_pressure(pressure);
     }
-        // hal.console->printf("count=%u\n", (unsigned)count);
-        if (count == 0) {
-            return _last_pressure;
-        }
-        _last_pressure = sum / count;
-        return _last_pressure;
-    }
-#endif
-
-    if (_source == NULL) {
-        return 0;
-    }
-    _source->set_pin(_pin);
-    _last_pressure = _source->voltage_average_ratiometric() * SCALING_OLD_CALIBRATION;
-    return _last_pressure;
+    return pressure;
 }
 
 // calibrate the airspeed. This must be called at least once before
@@ -177,17 +135,28 @@ float AP_Airspeed::get_pressure(void)
 void AP_Airspeed::calibrate()
 {
     float sum = 0;
+    uint8_t count = 0;
     uint8_t c;
     if (!_enable) {
         return;
     }
     // discard first reading
     get_pressure();
-    for (c = 0; c < 10; c++) {
+    for (uint8_t i = 0; i < 10; i++) {
         hal.scheduler->delay(100);
-        sum += get_pressure();
+        float p = get_pressure();
+        if (_healthy) {
+            sum += p;
+            count++;
         }
-    float raw = sum/c;
+    }
+    if (count == 0) {
+        // unhealthy sensor
+        hal.console->printf_P(PSTR("Airspeed sensor unhealthy\n"));
+        _offset.set(0);
+        return;
+    }
+    float raw = sum/count;
     _offset.set_and_save(raw);
     _airspeed = 0;
     _raw_airspeed = 0;
@@ -200,8 +169,8 @@ void AP_Airspeed::read(void)
     if (!_enable) {
         return;
     }
-    float raw               = get_pressure();
-    airspeed_pressure       = max(raw - _offset, 0);
+    airspeed_pressure       = get_pressure();
+    airspeed_pressure       = max(airspeed_pressure - _offset, 0);
     _raw_airspeed           = sqrtf(airspeed_pressure * _ratio);
     _airspeed               = 0.7f * _airspeed  +  0.3f * _raw_airspeed;
 }

libraries/AP_Airspeed/AP_Airspeed.h

@@ -8,6 +8,10 @@
 #include <AP_Param.h>
 #include <GCS_MAVLink.h>
 #include <AP_Vehicle.h>
+#include <AP_Airspeed_Backend.h>
+#include <AP_Airspeed_analog.h>
+#include <AP_Airspeed_PX4.h>
+#include <AP_Airspeed_I2C.h>
 
 class Airspeed_Calibration {
 public:
@@ -37,9 +41,10 @@ class AP_Airspeed
 public:
     // constructor
     AP_Airspeed(const AP_Vehicle::FixedWing &parms) : 
-        _ets_fd(-1),
         _EAS2TAS(1.0f),
-        _calibration(parms)
+        _calibration(parms),
+        analog(_pin),
+        _healthy(false)
     {
 		AP_Param::setup_object_defaults(this, var_info);
     };
@@ -80,7 +85,7 @@ public:
 
     // return true if airspeed is enabled, and airspeed use is set
     bool        use(void) const {
-        return _enable && _use && _offset != 0;
+        return _enable && _use && _offset != 0 && _healthy;
     }
 
     // return true if airspeed is enabled
@@ -124,7 +129,6 @@ public:
 
 
 private:
-    AP_HAL::AnalogSource *_source;
     AP_Float        _offset;
     AP_Float        _ratio;
     AP_Int8         _use;
@@ -133,16 +137,22 @@ private:
     AP_Int8         _autocal;
     float           _raw_airspeed;
     float           _airspeed;
-    int			    _ets_fd;
     float			_last_pressure;
     float           _EAS2TAS;
+    bool		    _healthy;
 
     Airspeed_Calibration _calibration;
     float _last_saved_ratio;
     uint8_t _counter;
 
-    // return raw differential pressure in Pascal
     float get_pressure(void);
+
+    AP_Airspeed_Analog analog;
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    AP_Airspeed_PX4    digital;
+#else
+    AP_Airspeed_I2C    digital;
+#endif
 };
 
 #endif // __AP_AIRSPEED_H__

libraries/AP_Airspeed/AP_Airspeed_Backend.h

@@ -0,0 +1,40 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+
+/*
+  backend driver class for airspeed
+ */
+
+#ifndef __AP_AIRSPEED_BACKEND_H__
+#define __AP_AIRSPEED_BACKEND_H__
+
+#include <AP_Common.h>
+#include <AP_HAL.h>
+
+class AP_Airspeed_Backend {
+public:
+    // probe and initialise the sensor
+    virtual bool init(void) = 0;
+
+    // return the current differential_pressure in Pascal
+    virtual bool get_differential_pressure(float &pressure) = 0;
+
+    // return the current temperature in degrees C, if available
+    virtual bool get_temperature(float &temperature) = 0;
+};
+
+#endif // __AP_AIRSPEED_BACKEND_H__

libraries/AP_Airspeed/AP_Airspeed_I2C.cpp

@@ -0,0 +1,116 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+
+/*
+  backend driver for airspeed from a I2C MS4525D0 sensor
+ */
+
+#include <AP_Common.h>
+#include <AP_HAL.h>
+#include <AP_Math.h>
+#include <AP_Airspeed_I2C.h>
+
+extern const AP_HAL::HAL& hal;
+
+#define I2C_ADDRESS_MS4525DO	0x28
+
+// probe and initialise the sensor
+bool AP_Airspeed_I2C::init(void)
+{
+    _measure();
+    hal.scheduler->delay(10);
+    _collect();
+    if (_last_sample_time_ms != 0) {
+        hal.scheduler->register_timer_process(reinterpret_cast<AP_HAL::TimedProc>(&AP_Airspeed_I2C::_timer), this);
+        return true;
+    }
+    return false;
+}
+
+// start a measurement
+void AP_Airspeed_I2C::_measure(void)
+{
+    _measurement_started_ms = 0;
+    if (hal.i2c->writeRegisters(I2C_ADDRESS_MS4525DO, 0, 0, NULL) != 0) {
+        return;
+    }
+    _measurement_started_ms = hal.scheduler->millis();
+}
+
+// read the values from the sensor
+void AP_Airspeed_I2C::_collect(void)
+{
+    uint8_t data[4];
+    _measurement_started_ms = 0;
+
+    if (hal.i2c->read(I2C_ADDRESS_MS4525DO, 4, data) != 0) {
+        return;
+    }
+    
+	uint8_t status = data[0] & 0xC0;
+	if (status == 2) {
+        return;
+	} else if (status == 3) {
+        return;
+	}
+
+	int16_t dp_raw, dT_raw;
+	dp_raw = (data[0] << 8) + data[1];
+	dp_raw = 0x3FFF & dp_raw;
+	dT_raw = (data[2] << 8) + data[3];
+	dT_raw = (0xFFE0 & dT_raw) >> 5;
+
+	_temperature = ((200 * dT_raw) / 2047) - 50;
+	_pressure = fabs(dp_raw - (16384 / 2.0f));
+    _last_sample_time_ms = hal.scheduler->millis();
+}
+
+// 1kHz timer
+void AP_Airspeed_I2C::_timer(void)
+{
+    if (_measurement_started_ms == 0) {
+        _measure();
+        return;
+    }
+    if ((hal.scheduler->millis() - _measurement_started_ms) > 10) {
+        _collect();
+        // start a new measurement
+        _measure();
+    }
+}
+
+// return the current differential_pressure in Pascal
+bool AP_Airspeed_I2C::get_differential_pressure(float &pressure)
+{
+    if ((hal.scheduler->millis() - _last_sample_time_ms) > 100) {
+        return false;
+    }
+    pressure = _pressure;
+    return true;
+}
+
+// return the current temperature in degrees C, if available
+bool AP_Airspeed_I2C::get_temperature(float &temperature)
+{
+    if ((hal.scheduler->millis() - _last_sample_time_ms) > 100) {
+        return false;
+    }
+    temperature = _temperature;
+    return true;
+}
+
+

libraries/AP_Airspeed/AP_Airspeed_I2C.h

@@ -0,0 +1,52 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+
+/*
+  backend driver for airspeed from I2C
+ */
+
+#ifndef __AP_AIRSPEED_I2C_H__
+#define __AP_AIRSPEED_I2C_H__
+
+#include <AP_HAL.h>
+#include <AP_Airspeed_Backend.h>
+
+class AP_Airspeed_I2C : public AP_Airspeed_Backend 
+{
+public:
+    // 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:
+    void _measure(void);
+    void _collect(void);
+    void _timer(void);
+    float _temperature;
+    float _pressure;
+    uint32_t _last_sample_time_ms;
+    uint32_t _measurement_started_ms;
+};
+
+#endif // __AP_AIRSPEED_I2C_H__
+
+

libraries/AP_Airspeed/AP_Airspeed_I2C_PX4.h

@@ -0,0 +1,54 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+
+/*
+  backend driver for airspeed from I2C
+ */
+
+#ifndef __AP_AIRSPEED_I2C_H__
+#define __AP_AIRSPEED_I2C_H__
+
+#include <AP_Common.h>
+#include <AP_HAL.h>
+
+class AP_Airspeed_I2C_PX4 : AP_Airspeed_I2C {
+public:
+    // constructor
+    AP_Airspeed_I2C();
+
+    // 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:
+    void _measure(void);
+    void _collect(void);
+    void _timer(void);
+    float _temperature;
+    float _pressure;
+    uint32_t _last_sample_time_ms;
+    uint32_t _measurement_started_ms;
+};
+
+#endif // __AP_AIRSPEED_I2C_H__
+
+

libraries/AP_Airspeed/AP_Airspeed_PX4.cpp

@@ -0,0 +1,82 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+ *   PX4 airspeed backend
+ */
+
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+
+#include <AP_Airspeed_PX4.h>
+#include <drivers/drv_airspeed.h>
+#include <uORB/topics/differential_pressure.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+extern const AP_HAL::HAL& hal;
+
+bool AP_Airspeed_PX4::init()
+{
+    _fd = open(AIRSPEED_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_pa;
+        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)
+{
+    return _temperature;
+}
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_Airspeed/AP_Airspeed_PX4.h

@@ -0,0 +1,48 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+
+/*
+  backend driver for airspeed from I2C
+ */
+
+#ifndef __AP_AIRSPEED_PX4_H__
+#define __AP_AIRSPEED_PX4_H__
+
+#include <AP_HAL.h>
+#include <AP_Airspeed_Backend.h>
+
+class AP_Airspeed_PX4 : public AP_Airspeed_Backend {
+public:
+    // constructor
+    AP_Airspeed_PX4() : _fd(-1) {}
+
+    // 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;
+    uint64_t _last_timestamp;
+    float _temperature;
+};
+
+#endif // __AP_AIRSPEED_PX4_H__

libraries/AP_Airspeed/AP_Airspeed_analog.cpp

@@ -0,0 +1,59 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+ *   analog airspeed driver
+ */
+
+
+#include <AP_HAL.h>
+#include <AP_Math.h>
+#include <AP_Common.h>
+#include <AP_ADC.h>
+#include <AP_ADC_AnalogSource.h>
+#include <AP_Airspeed.h>
+
+extern const AP_HAL::HAL& hal;
+
+// scaling for 3DR analog airspeed sensor
+#define VOLTS_TO_PASCAL 819
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
+extern AP_ADC_ADS7844 apm1_adc;
+#endif
+
+bool AP_Airspeed_Analog::init()
+{
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
+    if (_pin == 64) {
+        _source = new AP_ADC_AnalogSource( &apm1_adc, 7, 1.0f);
+        return true;
+    }
+#endif
+    _source = hal.analogin->channel(_pin);
+    return true;
+}
+
+// read the airspeed sensor
+bool AP_Airspeed_Analog::get_differential_pressure(float &pressure)
+{
+    if (_source == NULL) {
+        return false;
+    }
+    _source->set_pin(_pin);
+    pressure = _source->voltage_average_ratiometric() * VOLTS_TO_PASCAL;
+    return true;
+}
+

libraries/AP_Airspeed/AP_Airspeed_analog.h

@@ -0,0 +1,33 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef __AP_AIRSPEED_ANALOG_H__
+#define __AP_AIRSPEED_ANALOG_H__
+
+#include <AP_HAL.h>
+#include <AP_Airspeed_Backend.h>
+
+class AP_Airspeed_Analog : public AP_Airspeed_Backend
+{
+public:
+    AP_Airspeed_Analog(const AP_Int8 &pin) : 
+        _source(NULL),
+        _pin(pin),
+        _last_pin(-1)
+    {}
+
+    // probe and initialise the sensor
+    bool init(void);
+
+    // return the current differential_pressure in Pascal
+    bool get_differential_pressure(float &pressure);
+
+    // temperature not available via analog backend
+    bool get_temperature(float &temperature) { return false; }
+
+private:
+    AP_HAL::AnalogSource *_source;
+    const AP_Int8 &_pin;
+    int8_t _last_pin;
+};
+
+#endif // __AP_AIRSPEED_ANALOG_H__

libraries/AP_Airspeed/examples/Airspeed/Airspeed.pde

@@ -25,6 +25,8 @@
 #include <AP_Math.h>
 #include <AP_HAL.h>
 #include <AP_HAL_AVR.h>
+#include <AP_HAL_Linux.h>
+#include <AP_HAL_Empty.h>
 #include <AP_ADC.h>
 #include <AP_ADC_AnalogSource.h>
 #include <Filter.h>
@@ -63,6 +65,7 @@ void loop(void)
         airspeed.read();
         hal.console->printf("airspeed %.2f\n", airspeed.get_airspeed());
     }
+    hal.scheduler->delay(1);
 }
 
 AP_HAL_MAIN();