From 5bb57a31f732bd00d886ec8edba9b0f6e0e5ba0f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 20 Oct 2014 06:22:51 +1100
Subject: [PATCH] AP_Baro: split into frontend/backend

this allows for support of multiple sensors on a board
---
 libraries/AP_Baro/AP_Baro.cpp                 | 238 +++++++----
 libraries/AP_Baro/AP_Baro.h                   | 145 ++++---
 libraries/AP_Baro/AP_Baro_BMP085.cpp          |   3 +-
 libraries/AP_Baro/AP_Baro_BMP085.h            |   1 -
 libraries/AP_Baro/AP_Baro_Backend.cpp         |  23 ++
 libraries/AP_Baro/AP_Baro_Backend.h           |  29 ++
 libraries/AP_Baro/AP_Baro_HIL.cpp             |  99 ++---
 libraries/AP_Baro/AP_Baro_HIL.h               |  30 +-
 libraries/AP_Baro/AP_Baro_MS5611.cpp          | 387 ++++++------------
 libraries/AP_Baro/AP_Baro_MS5611.h            | 123 +++---
 libraries/AP_Baro/AP_Baro_PX4.cpp             |  95 ++---
 libraries/AP_Baro/AP_Baro_PX4.h               |  28 +-
 libraries/AP_Baro/AP_Baro_VRBRAIN.cpp         |  86 ----
 libraries/AP_Baro/AP_Baro_VRBRAIN.h           |  29 --
 .../examples/BARO_generic/BARO_generic.pde    |  25 +-
 .../AP_Baro_BMP085_test.pde                   |   5 +-
 16 files changed, 588 insertions(+), 758 deletions(-)
 create mode 100644 libraries/AP_Baro/AP_Baro_Backend.cpp
 create mode 100644 libraries/AP_Baro/AP_Baro_Backend.h
 delete mode 100644 libraries/AP_Baro/AP_Baro_VRBRAIN.cpp
 delete mode 100644 libraries/AP_Baro/AP_Baro_VRBRAIN.h

diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp
index 97697f4a3..dc861b6f5 100644
--- a/libraries/AP_Baro/AP_Baro.cpp
+++ b/libraries/AP_Baro/AP_Baro.cpp
@@ -36,14 +36,14 @@ const AP_Param::GroupInfo AP_Baro::var_info[] PROGMEM = {
     // @Description: calibrated ground pressure in Pascals
     // @Units: pascals
     // @Increment: 1
-    AP_GROUPINFO("ABS_PRESS", 2, AP_Baro, _ground_pressure, 0),
+    AP_GROUPINFO("ABS_PRESS", 2, AP_Baro, sensors[0].ground_pressure, 0),
 
     // @Param: TEMP
     // @DisplayName: ground temperature
     // @Description: calibrated ground temperature in degrees Celsius
     // @Units: degrees celsius
     // @Increment: 1
-    AP_GROUPINFO("TEMP", 3, AP_Baro, _ground_temperature, 0),
+    AP_GROUPINFO("TEMP", 3, AP_Baro, sensors[0].ground_temperature, 0),
 
     // @Param: ALT_OFFSET
     // @DisplayName: altitude offset
@@ -56,85 +56,95 @@ const AP_Param::GroupInfo AP_Baro::var_info[] PROGMEM = {
     AP_GROUPEND
 };
 
+/*
+  AP_Baro constructor
+ */
+AP_Baro::AP_Baro() :
+        _last_altitude_EAS2TAS(0.0f),
+        _EAS2TAS(0.0f),
+        _num_sensors(0),
+        _num_drivers(0)
+{
+    memset(sensors, 0, sizeof(sensors));
+
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
 // calibrate the barometer. This must be called at least once before
 // the altitude() or climb_rate() interfaces can be used
 void AP_Baro::calibrate()
 {
-    float ground_pressure = 0;
-    float ground_temperature = 0;
-
     // reset the altitude offset when we calibrate. The altitude
     // offset is supposed to be for within a flight
     _alt_offset.set_and_save(0);
 
-    {
-        uint32_t tstart = hal.scheduler->millis();
-        while (ground_pressure == 0 || !_flags.healthy) {
-            read();         // Get initial data from absolute pressure sensor
-            if (hal.scheduler->millis() - tstart > 500) {
-                hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful "
-                        "for more than 500ms in AP_Baro::calibrate [1]\r\n"));
-            }
-            ground_pressure         = get_pressure();
-            ground_temperature      = get_calibration_temperature();
-            hal.scheduler->delay(20);
-        }
-    }
     // let the barometer settle for a full second after startup
     // the MS5611 reads quite a long way off for the first second,
     // leading to about 1m of error if we don't wait
     for (uint8_t i = 0; i < 10; i++) {
         uint32_t tstart = hal.scheduler->millis();
         do {
-            read();
+            update();
             if (hal.scheduler->millis() - tstart > 500) {
                 hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful "
                         "for more than 500ms in AP_Baro::calibrate [2]\r\n"));
             }
-        } while (!_flags.healthy);
-        ground_pressure     = get_pressure();
-        ground_temperature  = get_calibration_temperature();
-
+        } while (!healthy());
         hal.scheduler->delay(100);
     }
 
     // now average over 5 values for the ground pressure and
     // temperature settings
-    for (uint16_t i = 0; i < 5; i++) {
+    float sum_pressure[BARO_MAX_INSTANCES] = {0};
+    float sum_temperature[BARO_MAX_INSTANCES] = {0};
+    const uint8_t num_samples = 5;
+
+    for (uint8_t c = 0; c < num_samples; c++) {
         uint32_t tstart = hal.scheduler->millis();
+        bool all_healthy = false;
         do {
-            read();
+            update();
             if (hal.scheduler->millis() - tstart > 500) {
                 hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful "
                         "for more than 500ms in AP_Baro::calibrate [3]\r\n"));
             }
-        } while (!_flags.healthy);
-        ground_pressure = (ground_pressure * 0.8f) + (get_pressure() * 0.2f);
-        ground_temperature = (ground_temperature * 0.8f) + 
-            (get_calibration_temperature() * 0.2f);
-
+            all_healthy = true;
+            for (uint8_t i=0; i<_num_sensors; i++) {
+                if (!healthy(i)) {
+                    all_healthy = false;
+                }
+            }
+        } while (!all_healthy);
+        for (uint8_t i=0; i<_num_sensors; i++) {
+            sum_pressure[i] += sensors[i].pressure;
+            sum_temperature[i] += sensors[i].temperature;
+        }
         hal.scheduler->delay(100);
     }
-
-    _ground_pressure.set_and_save(ground_pressure);
-    _ground_temperature.set_and_save(ground_temperature);
+    for (uint8_t i=0; i<_num_sensors; i++) {
+            sensors[i].ground_pressure.set_and_save(sum_pressure[i] / num_samples);
+            sensors[i].ground_temperature.set_and_save(sum_temperature[i] / num_samples);
+    }
 }
 
-/**
+/*
    update the barometer calibration
    this updates the baro ground calibration to the current values. It
    can be used before arming to keep the baro well calibrated
 */
 void AP_Baro::update_calibration()
 {
-    float pressure = get_pressure();
-    _ground_pressure.set(pressure);
-    float last_temperature = _ground_temperature;
-    _ground_temperature.set(get_calibration_temperature());
-    if (fabsf(last_temperature - _ground_temperature) > 3) {
-        // reset _EAS2TAS to force it to recalculate. This happens
-        // when a digital airspeed sensor comes online
-        _EAS2TAS = 0;
+    for (uint8_t i=0; i<_num_sensors; i++) {
+        if (healthy(i)) {
+            sensors[i].ground_pressure.set(get_pressure(i));
+        }
+        float last_temperature = sensors[i].ground_temperature;
+        sensors[i].ground_temperature.set(get_calibration_temperature(i));
+        if (fabsf(last_temperature - sensors[i].ground_temperature) > 3) {
+            // reset _EAS2TAS to force it to recalculate. This happens
+            // when a digital airspeed sensor comes online
+            _EAS2TAS = 0;
+        }
     }
 }
 
@@ -146,12 +156,12 @@ float AP_Baro::get_altitude_difference(float base_pressure, float pressure) cons
 #if HAL_CPU_CLASS <= HAL_CPU_CLASS_16
     // on slower CPUs use a less exact, but faster, calculation
     float scaling = base_pressure / pressure;
-    float temp    = _ground_temperature + 273.15f;
+    float temp    = get_calibration_temperature() + 273.15f;
     ret = logf(scaling) * temp * 29.271267f;
 #else
     // on faster CPUs use a more exact calculation
     float scaling = pressure / base_pressure;
-    float temp    = _ground_temperature + 273.15f;
+    float temp    = get_calibration_temperature() + 273.15f;
 
     // This is an exact calculation that is within +-2.5m of the standard atmosphere tables
     // in the troposphere (up to 11,000 m amsl).
@@ -160,54 +170,21 @@ float AP_Baro::get_altitude_difference(float base_pressure, float pressure) cons
     return ret;
 }
 
-// return current altitude estimate relative to time that calibrate()
-// was called. Returns altitude in meters
-// note that this relies on read() being called regularly to get new data
-float AP_Baro::get_altitude(void)
-{
-    if (_ground_pressure == 0) {
-        // called before initialisation
-        return 0;
-    }
-
-    if (_last_altitude_t == _last_update) {
-        // no new information
-        return _altitude + _alt_offset;
-    }
-
-    float pressure = get_pressure();
-    float alt = get_altitude_difference(_ground_pressure, pressure);
-
-    // record that we have consumed latest data
-    _last_altitude_t = _last_update;
-
-    // sanity check altitude
-    if (isnan(alt) || isinf(alt)) {
-        _flags.alt_ok = false;
-    } else {
-        _altitude = alt;
-        _flags.alt_ok = true;
-    }
-
-    // ensure the climb rate filter is updated
-    _climb_rate_filter.update(_altitude, _last_update);
-
-    return _altitude + _alt_offset;
-}
 
 // return current scale factor that converts from equivalent to true airspeed
 // valid for altitudes up to 10km AMSL
 // assumes standard atmosphere lapse rate
 float AP_Baro::get_EAS2TAS(void)
 {
-    if ((fabsf(_altitude - _last_altitude_EAS2TAS) < 100.0f) && (_EAS2TAS != 0.0f)) {
+    float altitude = get_altitude();
+    if ((fabsf(altitude - _last_altitude_EAS2TAS) < 100.0f) && (_EAS2TAS != 0.0f)) {
         // not enough change to require re-calculating
         return _EAS2TAS;
     }
 
-    float tempK = ((float)_ground_temperature) + 273.15f - 0.0065f * _altitude;
+    float tempK = get_calibration_temperature() + 273.15f - 0.0065f * altitude;
     _EAS2TAS = safe_sqrt(1.225f / ((float)get_pressure() / (287.26f * tempK)));
-    _last_altitude_EAS2TAS = _altitude;
+    _last_altitude_EAS2TAS = altitude;
     return _EAS2TAS;
 }
 
@@ -234,7 +211,7 @@ void AP_Baro::set_external_temperature(float temperature)
 /*
   get the temperature in degrees C to be used for calibration purposes
  */
-float AP_Baro::get_calibration_temperature(void) const
+float AP_Baro::get_calibration_temperature(uint8_t instance) const
 {
     // if we have a recent external temperature then use it
     if (_last_external_temperature_ms != 0 && hal.scheduler->millis() - _last_external_temperature_ms < 10000) {
@@ -245,9 +222,104 @@ float AP_Baro::get_calibration_temperature(void) const
     // not just using the baro temperature is it tends to read high,
     // often 30 degrees above the actual temperature. That means the
     // EAS2TAS tends to be off by quite a large margin
-    float ret = get_temperature();
+    float ret = get_temperature(instance);
     if (ret > 25) {
         ret = 25;
     }
     return ret;    
 }
+
+
+/*
+  initialise the barometer object, loading backend drivers
+ */
+void AP_Baro::init(void)
+{
+#if HAL_BARO_DEFAULT == HAL_BARO_PX4 || HAL_BARO_DEFAULT == HAL_BARO_VRBRAIN
+    drivers[0] = new AP_Baro_PX4(*this);
+    _num_drivers = 1;
+#elif HAL_BARO_DEFAULT == HAL_BARO_HIL
+    drivers[0] = new AP_Baro_HIL(*this);
+    _num_drivers = 1;
+#elif HAL_BARO_DEFAULT == HAL_BARO_MS5611
+    {
+        drivers[0] = new AP_Baro_MS5611(*this, new AP_SerialBus_I2C(MS5611_I2C_ADDR));
+        _num_drivers = 1;
+    }
+#elif HAL_BARO_DEFAULT == HAL_BARO_MS5611_SPI
+    {
+        drivers[0] = new AP_Baro_MS5611(*this, 
+                                        new AP_SerialBus_SPI(AP_HAL::SPIDevice_MS5611, 
+                                                             AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH));
+        _num_drivers = 1;
+    }
+#endif    
+    if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == NULL) {
+        hal.scheduler->panic(PSTR("Baro: unable to initialise driver"));
+    }
+}
+
+
+/*
+  call update on all drivers
+ */
+void AP_Baro::update(void)
+{
+    for (uint8_t i=0; i<_num_drivers; i++) {
+        drivers[i]->update();
+    }
+
+    // consider a sensor as healthy if it has had an update in the
+    // last 0.5 seconds
+    uint32_t now = hal.scheduler->millis();
+    for (uint8_t i=0; i<_num_sensors; i++) {
+        sensors[i].healthy = (now - sensors[i].last_update_ms < 500) && sensors[i].pressure != 0;
+    }
+
+    for (uint8_t i=0; i<_num_sensors; i++) {
+        if (sensors[i].healthy) {
+            // update altitude calculation
+            if (sensors[i].ground_pressure == 0) {
+                sensors[i].ground_pressure = sensors[i].pressure;
+            }
+            sensors[i].altitude = get_altitude_difference(sensors[i].ground_pressure, sensors[i].pressure);
+            // sanity check altitude
+            sensors[i].alt_ok = !(isnan(sensors[i].altitude) || isinf(sensors[i].altitude));
+        }
+    }
+
+    // choose primary sensor
+    _primary = 0;
+    for (uint8_t i=0; i<_num_sensors; i++) {
+        if (healthy(i)) {
+            _primary = i;
+            break;
+        }
+    }
+
+    // ensure the climb rate filter is updated
+    _climb_rate_filter.update(get_altitude(), get_last_update());
+}
+
+/*
+  call accululate on all drivers
+ */
+void AP_Baro::accumulate(void)
+{
+    for (uint8_t i=0; i<_num_drivers; i++) {
+        drivers[i]->accumulate();
+    }
+}
+
+
+/* register a new sensor, claiming a sensor slot. If we are out of
+   slots it will panic
+*/
+uint8_t AP_Baro::register_sensor(void)
+{
+    if (_num_sensors >= BARO_MAX_INSTANCES) {
+        hal.scheduler->panic(PSTR("Too many barometers"));
+    }
+    return _num_sensors++;
+}
+
diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h
index 6e395dcfd..b65426c13 100644
--- a/libraries/AP_Baro/AP_Baro.h
+++ b/libraries/AP_Baro/AP_Baro.h
@@ -3,85 +3,92 @@
 #ifndef __AP_BARO_H__
 #define __AP_BARO_H__
 
+#include <AP_HAL.h>
 #include <AP_Param.h>
 #include <Filter.h>
 #include <DerivativeFilter.h>
 
+// maximum number of sensor instances
+#if HAL_CPU_CLASS == HAL_CPU_CLASS_16
+#define BARO_MAX_INSTANCES 1
+#else
+#define BARO_MAX_INSTANCES 2
+#endif
+
+// maximum number of drivers. Note that a single driver can provide
+// multiple sensor instances
+#if HAL_CPU_CLASS == HAL_CPU_CLASS_16
+#define BARO_MAX_DRIVERS 1
+#else
+#define BARO_MAX_DRIVERS 2
+#endif
+
+class AP_Baro_Backend;
+
 class AP_Baro
 {
+    friend class AP_Baro_Backend;
+
 public:
-    AP_Baro() :
-        _last_update(0),
-        _pressure_samples(0),
-        _altitude(0.0f),
-        _last_altitude_EAS2TAS(0.0f),
-        _EAS2TAS(0.0f),
-        _last_altitude_t(0),
-        _last_external_temperature_ms(0)
-    {
-        // initialise flags
-        _flags.healthy = false;
-        _flags.alt_ok = false;
-
-		AP_Param::setup_object_defaults(this, var_info);
-    }
+    // constructor
+    AP_Baro();
 
-    // healthy - returns true if sensor and derived altitude are good
-    bool healthy() const { return _flags.healthy && _flags.alt_ok; }
+    // initialise the barometer object, loading backend drivers
+    void init(void);
 
-    virtual bool            init()=0;
-    virtual uint8_t         read() = 0;
+    // update the barometer object, asking backends to push data to
+    // the frontend
+    void update(void);
+
+    // healthy - returns true if sensor and derived altitude are good
+    bool healthy(void) const { return healthy(_primary); }
+    bool healthy(uint8_t instance) const { return sensors[instance].healthy && sensors[instance].alt_ok; }
 
     // pressure in Pascal. Divide by 100 for millibars or hectopascals
-    virtual float           get_pressure() = 0;
+    float get_pressure(void) const { return get_pressure(_primary); }
+    float get_pressure(uint8_t instance) const { return sensors[instance].pressure; }
 
     // temperature in degrees C
-    virtual float           get_temperature() const = 0;
+    float get_temperature(void) const { return get_temperature(_primary); }
+    float get_temperature(uint8_t instance) const { return sensors[instance].temperature; }
 
-    // accumulate a reading - overridden in some drivers
-    virtual void            accumulate(void) {}
+    // accumulate a reading on sensors. Some backends without their
+    // own thread or a timer may need this.
+    void accumulate(void);
 
     // calibrate the barometer. This must be called on startup if the
     // altitude/climb_rate/acceleration interfaces are ever used
-    // the callback is a delay() like routine
-    void        calibrate();
+    void calibrate(void);
 
     // update the barometer calibration to the current pressure. Can
     // be used for incremental preflight update of baro
-    void        update_calibration();
+    void update_calibration(void);
 
     // get current altitude in meters relative to altitude at the time
     // of the last calibrate() call
-    float        get_altitude(void);
+    float get_altitude(void) const { return get_altitude(_primary); }
+    float get_altitude(uint8_t instance) const { return sensors[instance].altitude; }
 
     // get altitude difference in meters relative given a base
     // pressure in Pascal
     float get_altitude_difference(float base_pressure, float pressure) const;
 
     // get scale factor required to convert equivalent to true airspeed
-    float        get_EAS2TAS(void);
-
-    // return how many pressure samples were used to obtain
-    // the last pressure reading
-    uint8_t        get_pressure_samples(void) {
-        return _pressure_samples;
-    }
+    float get_EAS2TAS(void);
 
     // get current climb rate in meters/s. A positive number means
     // going up
-    float           get_climb_rate(void);
+    float get_climb_rate(void);
 
     // ground temperature in degrees C
     // the ground values are only valid after calibration
-    float           get_ground_temperature(void) {
-        return _ground_temperature.get();
-    }
+    float get_ground_temperature(void) const { return get_ground_temperature(_primary); }
+    float get_ground_temperature(uint8_t i)  const { return sensors[i].ground_temperature.get(); }
 
     // ground pressure in Pascal
     // the ground values are only valid after calibration
-    float           get_ground_pressure(void) {
-        return _ground_pressure.get();
-    }
+    float get_ground_pressure(void) const { return get_ground_pressure(_primary); }
+    float get_ground_pressure(uint8_t i)  const { return sensors[i].ground_pressure.get(); }
 
     // set the temperature to be used for altitude calibration. This
     // allows an external temperature source (such as a digital
@@ -89,41 +96,61 @@ public:
     void set_external_temperature(float temperature);
 
     // get last time sample was taken (in ms)
-    uint32_t        get_last_update() const { return _last_update; };
+    uint32_t get_last_update(void) const { return get_last_update(_primary); }
+    uint32_t get_last_update(uint8_t instance) const { return sensors[_primary].last_update_ms; }
 
-    static const struct AP_Param::GroupInfo        var_info[];
+    // settable parameters
+    static const struct AP_Param::GroupInfo var_info[];
 
-protected:
+    float get_calibration_temperature(void) const { return get_calibration_temperature(_primary); }
+    float get_calibration_temperature(uint8_t instance) const;
 
-    struct Baro_flags {
-        uint8_t healthy :1;             // true if sensor is healthy
-        uint8_t alt_ok  :1;             // true if calculated altitude is ok
-    } _flags;
+    // HIL (and SITL) interface, setting altitude
+    void setHIL(float altitude_msl);
 
-    uint32_t                            _last_update; // in ms
-    uint8_t                             _pressure_samples;
+    // HIL (and SITL) interface, setting pressure and temperature
+    void setHIL(uint8_t instance, float pressure, float temperature);
 
-private:
-    // get the temperature to be used for altitude calibration
-    float                               get_calibration_temperature(void) const;
+    // register a new sensor, claiming a sensor slot. If we are out of
+    // slots it will panic
+    uint8_t register_sensor(void);
 
+private:
+    // how many drivers do we have?
+    uint8_t _num_drivers;
+    AP_Baro_Backend *drivers[BARO_MAX_DRIVERS];
+
+    // how many sensors do we have?
+    uint8_t _num_sensors;
+
+    // what is the primary sensor at the moment?
+    uint8_t _primary;
+
+    struct sensor {
+        uint32_t last_update_ms;        // last update time in ms
+        bool healthy:1;                 // true if sensor is healthy
+        bool alt_ok:1;                  // true if calculated altitude is ok
+        float pressure;                 // pressure in Pascal
+        float temperature;              // temperature in degrees C
+        float altitude;                 // calculated altitude
+        AP_Float ground_temperature;
+        AP_Float ground_pressure;
+    } sensors[BARO_MAX_INSTANCES];
 
-    AP_Float                            _ground_temperature;
-    AP_Float                            _ground_pressure;
     AP_Int8                             _alt_offset;
-    float                               _altitude;
     float                               _last_altitude_EAS2TAS;
     float                               _EAS2TAS;
     float                               _external_temperature;
-    uint32_t                            _last_altitude_t;
     uint32_t                            _last_external_temperature_ms;
     DerivativeFilterFloat_Size7         _climb_rate_filter;
+
+    void SimpleAtmosphere(const float alt, float &sigma, float &delta, float &theta);
 };
 
+#include "AP_Baro_Backend.h"
 #include "AP_Baro_MS5611.h"
-#include "AP_Baro_BMP085.h"
+//#include "AP_Baro_BMP085.h"
 #include "AP_Baro_HIL.h"
 #include "AP_Baro_PX4.h"
-#include "AP_Baro_VRBRAIN.h"
 
 #endif // __AP_BARO_H__
diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp
index 569375c9d..6efbf79a9 100644
--- a/libraries/AP_Baro/AP_Baro_BMP085.cpp
+++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp
@@ -1,4 +1,5 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#if 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
@@ -169,7 +170,6 @@ uint8_t AP_Baro_BMP085::read()
     Temp = 0.1f * _temp_sum / _count;
     Press = _press_sum / _count;
 
-    _pressure_samples = _count;
     _count = 0;
     _temp_sum = 0;
     _press_sum = 0;
@@ -297,3 +297,4 @@ void AP_Baro_BMP085::Calculate()
     }
 }
 
+#endif
diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h
index bc53ddab8..85ee991b4 100644
--- a/libraries/AP_Baro/AP_Baro_BMP085.h
+++ b/libraries/AP_Baro/AP_Baro_BMP085.h
@@ -25,7 +25,6 @@ public:
         ac4(0), ac5(0), ac6(0),
         _retry_time(0)
     {
-        _pressure_samples = 1;
     };       // Constructor
 
 
diff --git a/libraries/AP_Baro/AP_Baro_Backend.cpp b/libraries/AP_Baro/AP_Baro_Backend.cpp
new file mode 100644
index 000000000..13f32c9bb
--- /dev/null
+++ b/libraries/AP_Baro/AP_Baro_Backend.cpp
@@ -0,0 +1,23 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_Baro.h>
+
+extern const AP_HAL::HAL& hal;
+
+// constructor
+AP_Baro_Backend::AP_Baro_Backend(AP_Baro &baro) : 
+    _frontend(baro) 
+{}
+
+/*
+  copy latest data to the frontend from a backend
+ */
+void AP_Baro_Backend::_copy_to_frontend(uint8_t instance, float pressure, float temperature)
+{
+    if (instance >= _frontend._num_sensors) {
+        return;
+    }
+    _frontend.sensors[instance].pressure = pressure;
+    _frontend.sensors[instance].temperature = temperature;
+    _frontend.sensors[instance].last_update_ms = hal.scheduler->millis();
+}
diff --git a/libraries/AP_Baro/AP_Baro_Backend.h b/libraries/AP_Baro/AP_Baro_Backend.h
new file mode 100644
index 000000000..b96f85054
--- /dev/null
+++ b/libraries/AP_Baro/AP_Baro_Backend.h
@@ -0,0 +1,29 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef __AP_BARO_BACKEND_H__
+#define __AP_BARO_BACKEND_H__
+
+#include "AP_Baro.h"
+
+class AP_Baro_Backend
+{
+public:
+    AP_Baro_Backend(AP_Baro &baro);
+
+    // each driver must provide an update method to copy accumulated
+    // data to the frontend
+    virtual void update() = 0;
+
+    // accumulate function. This is used for backends that don't use a
+    // timer, and need to be called regularly by the main code to
+    // trigger them to read the sensor
+    virtual void accumulate(void) {}
+
+protected:
+    // reference to frontend object
+    AP_Baro &_frontend;
+
+    void _copy_to_frontend(uint8_t instance, float pressure, float temperature);
+};
+
+#endif // __AP_BARO_BACKEND_H__
diff --git a/libraries/AP_Baro/AP_Baro_HIL.cpp b/libraries/AP_Baro/AP_Baro_HIL.cpp
index c9e277303..24f95bbe4 100644
--- a/libraries/AP_Baro/AP_Baro_HIL.cpp
+++ b/libraries/AP_Baro/AP_Baro_HIL.cpp
@@ -1,84 +1,51 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
 #include <AP_Baro.h>
-#include "AP_Baro_HIL.h"
 #include <AP_HAL.h>
 
 extern const AP_HAL::HAL& hal;
 
-// Public Methods //////////////////////////////////////////////////////////////
-bool AP_Baro_HIL::init()
+AP_Baro_HIL::AP_Baro_HIL(AP_Baro &baro) :
+    AP_Baro_Backend(baro)
 {
-    _flags.healthy = false;
-    return true;
+    _instance = _frontend.register_sensor();
 }
 
-// Read the sensor. This is a state machine
-// We read one time Temperature (state = 1) and then 4 times Pressure (states 2-5)
-uint8_t AP_Baro_HIL::read()
-{
-    uint8_t result = 0;
-
-    if (_count != 0) {
-        hal.scheduler->suspend_timer_procs();
-        result = 1;
-        Press = _pressure_sum / _count;
-        Temp = _temperature_sum / _count;
-        _pressure_samples = _count;
-        _count = 0;
-        _pressure_sum = 0;
-        _temperature_sum = 0;
-        hal.scheduler->resume_timer_procs();
-    }
-
-    return result;
-}
-
-void AP_Baro_HIL::setHIL(float pressure, float temperature)
-{
-    if (pressure > 0) {
-        _count = 1;
-        _pressure_sum = pressure;
-        _temperature_sum = temperature;
-        _last_update = hal.scheduler->millis();
-        _flags.healthy = true;
-    }
-}
-
-
 // ==========================================================================
 // based on tables.cpp from http://www.pdas.com/atmosdownload.html
 
 /* 
- Compute the temperature, density, and pressure in the standard atmosphere
- Correct to 20 km.  Only approximate thereafter.
+   Compute the temperature, density, and pressure in the standard atmosphere
+   Correct to 20 km.  Only approximate thereafter.
 */
-static void SimpleAtmosphere(
+void AP_Baro::SimpleAtmosphere(
 	const float alt,                           // geometric altitude, km.
 	float& sigma,                   // density/sea-level standard density
 	float& delta,                 // pressure/sea-level standard pressure
 	float& theta)           // temperature/sea-level standard temperature
 {
-  const float REARTH = 6369.0f;        // radius of the Earth (km)
-  const float GMR    = 34.163195f;     // gas constant
-  float h=alt*REARTH/(alt+REARTH);     // geometric to geopotential altitude
-
-  if (h<11.0f)
-    {                                                          // Troposphere
-      theta=(288.15f-6.5f*h)/288.15f;
-      delta=powf(theta, GMR/6.5f);
-    }
-  else
-    {                                                         // Stratosphere
-      theta=216.65f/288.15f;
-      delta=0.2233611f*expf(-GMR*(h-11.0f)/216.65f);
+    const float REARTH = 6369.0f;        // radius of the Earth (km)
+    const float GMR    = 34.163195f;     // gas constant
+    float h=alt*REARTH/(alt+REARTH);     // geometric to geopotential altitude
+
+    if (h < 11.0f) {
+        // Troposphere
+        theta=(288.15f-6.5f*h)/288.15f;
+        delta=powf(theta, GMR/6.5f);
+    } else {
+        // Stratosphere
+        theta=216.65f/288.15f;
+        delta=0.2233611f*expf(-GMR*(h-11.0f)/216.65f);
     }
 
-  sigma=delta/theta;
+    sigma = delta/theta;
 }
 
 
-void AP_Baro_HIL::setHIL(float altitude_msl)
+/*
+  convert an altitude in meters above sea level to a presssure and temperature
+ */
+void AP_Baro::setHIL(float altitude_msl)
 {
     float sigma, delta, theta;
     const float p0 = 101325;
@@ -87,13 +54,19 @@ void AP_Baro_HIL::setHIL(float altitude_msl)
     float p = p0 * delta;
     float T = 303.16f * theta - 273.16f; // Assume 30 degrees at sea level - converted to degrees Kelvin
 
-    setHIL(p, T);
+    setHIL(0, p, T);
 }
 
-float AP_Baro_HIL::get_pressure() {
-    return Press;
-}
-
-float AP_Baro_HIL::get_temperature() const {
-    return Temp;
+/*
+  set HIL pressure and temperature for an instance
+ */
+void AP_Baro::setHIL(uint8_t instance, float pressure, float temperature)
+{
+    if (instance >= _num_sensors) {
+        // invalid
+        return;
+    }
+    sensors[instance].pressure = pressure;
+    sensors[instance].temperature = temperature;
+    sensors[instance].last_update_ms = hal.scheduler->millis();
 }
diff --git a/libraries/AP_Baro/AP_Baro_HIL.h b/libraries/AP_Baro/AP_Baro_HIL.h
index 9853bf221..b88b9d184 100644
--- a/libraries/AP_Baro/AP_Baro_HIL.h
+++ b/libraries/AP_Baro/AP_Baro_HIL.h
@@ -1,26 +1,22 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+  dummy backend for HIL (and SITL). This doesn't actually need to do
+  any work, as setHIL() is in the frontend
+ */
 
-#ifndef __AP_BARO__HIL_H__
-#define __AP_BARO__HIL_H__
+#ifndef __AP_BARO_HIL_H__
+#define __AP_BARO_HIL_H__
 
 #include "AP_Baro.h"
 
-class AP_Baro_HIL : public AP_Baro
+class AP_Baro_HIL : public AP_Baro_Backend
 {
-private:
-    float Temp;
-    float Press;
-    float _pressure_sum;
-    float _temperature_sum;
-    volatile uint8_t _count;
-
 public:
-    bool init();
-    uint8_t read();
-    float get_pressure();
-    float get_temperature() const;
-    void setHIL(float altitude_msl);
-    void setHIL(float pressure, float temperature);
+    AP_Baro_HIL(AP_Baro &baro);
+    void update() {}
+
+private:
+    uint8_t _instance;
 };
 
-#endif //  __AP_BARO__HIL_H__
+#endif //  __AP_BARO_HIL_H__
diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp
index 9b180b0d7..b314e7ba6 100644
--- a/libraries/AP_Baro/AP_Baro_MS5611.cpp
+++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp
@@ -15,35 +15,13 @@
  */
 
 /*
- *       APM_MS5611.cpp - Arduino Library for MS5611-01BA01 absolute pressure sensor
- *       Code by Jose Julio, Pat Hickey and Jordi Muñoz. DIYDrones.com
- *
- *       Sensor is conected to standard SPI port
- *       Chip Select pin: Analog2 (provisional until Jordi defines the pin)!!
- *
- *       Variables:
- *               Temp : Calculated temperature (in Celsius degrees)
- *               Press : Calculated pressure   (in mbar units * 100)
- *
- *
- *       Methods:
- *               init() : Initialization and sensor reset
- *               read() : Read sensor data and _calculate Temperature, Pressure
- *                        This function is optimized so the main host don´t need to wait
- *                                You can call this function in your main loop
- *                                Maximum data output frequency 100Hz - this allows maximum oversampling in the chip ADC
- *                                It returns a 1 if there are new data.
- *               get_pressure() : return pressure in mbar*100 units
- *               get_temperature() : return temperature in celsius degrees*100 units
- *
- *       Internal functions:
- *               _calculate() : Calculate Temperature and Pressure (temperature compensated) in real units
- *
- *
- */
+  originally written by Jose Julio, Pat Hickey and Jordi Muñoz
+
+  Heavily modified by Andrew Tridgell
+*/
 
 #include <AP_HAL.h>
-#include "AP_Baro_MS5611.h"
+#include "AP_Baro.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -59,187 +37,171 @@ extern const AP_HAL::HAL& hal;
 #define CMD_CONVERT_D1_OSR4096 0x48   // Maximum resolution (oversampling)
 #define CMD_CONVERT_D2_OSR4096 0x58   // Maximum resolution (oversampling)
 
-uint32_t volatile AP_Baro_MS5611::_s_D1;
-uint32_t volatile AP_Baro_MS5611::_s_D2;
-uint8_t volatile AP_Baro_MS5611::_d1_count;
-uint8_t volatile AP_Baro_MS5611::_d2_count;
-uint8_t AP_Baro_MS5611::_state;
-uint32_t AP_Baro_MS5611::_timer;
-bool volatile AP_Baro_MS5611::_updated;
-
-AP_Baro_MS5611_Serial* AP_Baro_MS5611::_serial = NULL;
-AP_Baro_MS5611_SPI AP_Baro_MS5611::spi;
-#if MS5611_WITH_I2C
-AP_Baro_MS5611_I2C AP_Baro_MS5611::i2c;
-#endif
-
 // SPI Device //////////////////////////////////////////////////////////////////
 
-void AP_Baro_MS5611_SPI::init()
+AP_SerialBus_SPI::AP_SerialBus_SPI(enum AP_HAL::SPIDevice device, enum AP_HAL::SPIDeviceDriver::bus_speed speed) :
+    _device(device),
+    _speed(speed),
+    _spi(NULL),
+    _spi_sem(NULL)
 {
-    _spi = hal.spi->device(AP_HAL::SPIDevice_MS5611);
+}
+
+void AP_SerialBus_SPI::init()
+{
+    _spi = hal.spi->device(_device);
     if (_spi == NULL) {
-        hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611 did not get "
-                    "valid SPI device driver!"));
-        return; /* never reached */
+        hal.scheduler->panic(PSTR("did not get valid SPI device driver!"));
     }
     _spi_sem = _spi->get_semaphore();
     if (_spi_sem == NULL) {
-        hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611 did not get "
-                    "valid SPI semaphroe!"));
-        return; /* never reached */
-
+        hal.scheduler->panic(PSTR("AP_SerialBus_SPI did not get valid SPI semaphroe!"));
     }
-
-    // now that we have initialised, we set the SPI bus speed to high
-    // (8MHz on APM2)
-    _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
+    _spi->set_bus_speed(_speed);
 }
 
-uint16_t AP_Baro_MS5611_SPI::read_16bits(uint8_t reg)
+uint16_t AP_SerialBus_SPI::read_16bits(uint8_t reg)
 {
-    uint8_t tx[3];
+    uint8_t tx[3] = { reg, 0, 0 };
     uint8_t rx[3];
-    tx[0] = reg; tx[1] = 0; tx[2] = 0;
     _spi->transaction(tx, rx, 3);
     return ((uint16_t) rx[1] << 8 ) | ( rx[2] );
 }
 
-uint32_t AP_Baro_MS5611_SPI::read_adc()
+uint32_t AP_SerialBus_SPI::read_24bits(uint8_t reg)
 {
-    uint8_t tx[4];
+    uint8_t tx[4] = { reg, 0, 0, 0 };
     uint8_t rx[4];
-    memset(tx, 0, 4); /* first byte is addr = 0 */
     _spi->transaction(tx, rx, 4);
     return (((uint32_t)rx[1])<<16) | (((uint32_t)rx[2])<<8) | ((uint32_t)rx[3]);
 }
 
-
-void AP_Baro_MS5611_SPI::write(uint8_t reg)
+void AP_SerialBus_SPI::write(uint8_t reg)
 {
-    uint8_t tx[1];
-    tx[0] = reg;
+    uint8_t tx[1] = { reg };
     _spi->transaction(tx, NULL, 1);
 }
 
-bool AP_Baro_MS5611_SPI::sem_take_blocking() {
+bool AP_SerialBus_SPI::sem_take_blocking() 
+{
     return _spi_sem->take(10);
 }
 
-bool AP_Baro_MS5611_SPI::sem_take_nonblocking()
+bool AP_SerialBus_SPI::sem_take_nonblocking()
 {
-    /**
-     * Take nonblocking from a TimerProcess context &
-     * monitor for bad failures
-     */
-    static int semfail_ctr = 0;
-    bool got = _spi_sem->take_nonblocking();
-    if (!got) {
-        if (!hal.scheduler->system_initializing()) {
-            semfail_ctr++;
-            if (semfail_ctr > 100) {
-                hal.scheduler->panic(PSTR("PANIC: failed to take _spi_sem "
-                                          "100 times in a row, in "
-                                          "AP_Baro_MS5611::_update"));
-            }
-        }
-        return false; /* never reached */
-    } else {
-        semfail_ctr = 0;
-    }
-    return got;
+    return _spi_sem->take_nonblocking();
 }
 
-void AP_Baro_MS5611_SPI::sem_give()
+void AP_SerialBus_SPI::sem_give()
 {
     _spi_sem->give();
 }
 
-// I2C Device //////////////////////////////////////////////////////////////////
-#if MS5611_WITH_I2C
 
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
-#define MS5611_ADDR 0x77
-#else
-#define MS5611_ADDR 0x76 /** I2C address of the MS5611 on the PX4 board. */
-#endif
+/// I2C SerialBus
+AP_SerialBus_I2C::AP_SerialBus_I2C(uint8_t addr) :
+    _addr(addr),
+    _i2c_sem(NULL) 
+{
+}
 
-void AP_Baro_MS5611_I2C::init()
+void AP_SerialBus_I2C::init()
 {
     _i2c_sem = hal.i2c->get_semaphore();
     if (_i2c_sem == NULL) {
-        hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611 did not get "
-                                  "valid I2C semaphroe!"));
-        return; /* never reached */
+        hal.scheduler->panic(PSTR("AP_SerialBus_I2C did not get valid I2C semaphroe!"));
     }
 }
 
-uint16_t AP_Baro_MS5611_I2C::read_16bits(uint8_t reg)
+uint16_t AP_SerialBus_I2C::read_16bits(uint8_t reg)
 {
     uint8_t buf[2];
-
-    if (hal.i2c->readRegisters(MS5611_ADDR, reg, sizeof(buf), buf) == 0)
+    if (hal.i2c->readRegisters(_addr, reg, sizeof(buf), buf) == 0) {
         return (((uint16_t)(buf[0]) << 8) | buf[1]);
-
+    }
     return 0;
 }
 
-uint32_t AP_Baro_MS5611_I2C::read_adc()
+uint32_t AP_SerialBus_I2C::read_24bits(uint8_t reg)
 {
     uint8_t buf[3];
-
-    if (hal.i2c->readRegisters(MS5611_ADDR, 0x00, sizeof(buf), buf) == 0)
+    if (hal.i2c->readRegisters(_addr, reg, sizeof(buf), buf) == 0) {
         return (((uint32_t)buf[0]) << 16) | (((uint32_t)buf[1]) << 8) | buf[2];
-
+    }
     return 0;
 }
 
-void AP_Baro_MS5611_I2C::write(uint8_t reg)
+void AP_SerialBus_I2C::write(uint8_t reg)
 {
-    hal.i2c->write(MS5611_ADDR, 1, &reg);
+    hal.i2c->write(_addr, 1, &reg);
 }
 
-bool AP_Baro_MS5611_I2C::sem_take_blocking() {
+bool AP_SerialBus_I2C::sem_take_blocking() 
+{
     return _i2c_sem->take(10);
 }
 
-bool AP_Baro_MS5611_I2C::sem_take_nonblocking()
+bool AP_SerialBus_I2C::sem_take_nonblocking()
 {
-    /**
-     * Take nonblocking from a TimerProcess context &
-     * monitor for bad failures
-     */
-    static int semfail_ctr = 0;
-    bool got = _i2c_sem->take_nonblocking();
-    if (!got) {
-        if (!hal.scheduler->system_initializing()) {
-            semfail_ctr++;
-            if (semfail_ctr > 100) {
-                hal.scheduler->panic(PSTR("PANIC: failed to take _i2c_sem "
-                                          "100 times in a row, in "
-                                          "AP_Baro_MS5611::_update"));
-            }
-        }
-        return false; /* never reached */
-    } else {
-        semfail_ctr = 0;
-    }
-    return got;
+    return _i2c_sem->take_nonblocking();
 }
 
-void AP_Baro_MS5611_I2C::sem_give()
+void AP_SerialBus_I2C::sem_give()
 {
     _i2c_sem->give();
 }
-#endif // MS5611_WITH_I2C
 
-// Public Methods //////////////////////////////////////////////////////////////
+/*
+  constructor
+ */
+AP_Baro_MS5611::AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial) :
+    AP_Baro_Backend(baro),
+    _serial(serial),
+    _updated(false),
+    _state(0),
+    _last_timer(0)
+{
+    _instance = _frontend.register_sensor();
+    _serial->init();
+    if (!_serial->sem_take_blocking()){
+        hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611: failed to take serial semaphore for init"));
+    }
+
+    _serial->write(CMD_MS5611_RESET);
+    hal.scheduler->delay(4);
+
+    // We read the factory calibration
+    // The on-chip CRC is not used
+    C1 = _serial->read_16bits(CMD_MS5611_PROM_C1);
+    C2 = _serial->read_16bits(CMD_MS5611_PROM_C2);
+    C3 = _serial->read_16bits(CMD_MS5611_PROM_C3);
+    C4 = _serial->read_16bits(CMD_MS5611_PROM_C4);
+    C5 = _serial->read_16bits(CMD_MS5611_PROM_C5);
+    C6 = _serial->read_16bits(CMD_MS5611_PROM_C6);
+
+    if (!_check_crc()) {
+        hal.scheduler->panic(PSTR("Bad CRC on MS5611"));
+    }
+
+    // Send a command to read Temp first
+    _serial->write(CMD_CONVERT_D2_OSR4096);
+    _last_timer = hal.scheduler->micros();
+    _state = 0;
+
+    _s_D1 = 0;
+    _s_D2 = 0;
+    _d1_count = 0;
+    _d2_count = 0;
+
+    _serial->sem_give();
+
+    hal.scheduler->register_timer_process( AP_HAL_MEMBERPROC(&AP_Baro_MS5611::_timer));
+}
 
-#if CONFIG_HAL_BOARD != HAL_BOARD_APM2
 /**
  * MS5611 crc4 method based on PX4Firmware code
  */
-bool AP_Baro_MS5611::check_crc(void)
+bool AP_Baro_MS5611::_check_crc(void)
 {
     int16_t cnt;
     uint16_t n_rem;
@@ -282,83 +244,17 @@ bool AP_Baro_MS5611::check_crc(void)
     /* return true if CRCs match */
     return (0x000F & crc_read) == (n_rem ^ 0x00);
 }
-#endif
 
-// SPI should be initialized externally
-bool AP_Baro_MS5611::init()
-{
-    hal.scheduler->suspend_timer_procs();
-    if (_serial == NULL) {
-        hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611: NULL serial driver"));
-        return false; /* never reached */
-    }
 
-    _serial->init();
-    if (!_serial->sem_take_blocking()){
-        hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611: failed to take "
-                    "serial semaphore for init"));
-        return false; /* never reached */
-    }
-
-    _serial->write(CMD_MS5611_RESET);
-    hal.scheduler->delay(4);
-
-    // We read the factory calibration
-    // The on-chip CRC is not used
-    C1 = _serial->read_16bits(CMD_MS5611_PROM_C1);
-    C2 = _serial->read_16bits(CMD_MS5611_PROM_C2);
-    C3 = _serial->read_16bits(CMD_MS5611_PROM_C3);
-    C4 = _serial->read_16bits(CMD_MS5611_PROM_C4);
-    C5 = _serial->read_16bits(CMD_MS5611_PROM_C5);
-    C6 = _serial->read_16bits(CMD_MS5611_PROM_C6);
-
-    // if not on APM2 then check CRC
-#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
-    if (!check_crc()) {
-        hal.scheduler->panic("Bad CRC on MS5611");
-    }
-#endif
-
-    //Send a command to read Temp first
-    _serial->write(CMD_CONVERT_D2_OSR4096);
-    _timer = hal.scheduler->micros();
-    _state = 0;
-    Temp=0;
-    Press=0;
-
-    _s_D1 = 0;
-    _s_D2 = 0;
-    _d1_count = 0;
-    _d2_count = 0;
-
-    hal.scheduler->resume_timer_procs();
-    hal.scheduler->register_timer_process( AP_HAL_MEMBERPROC(&AP_Baro_MS5611::_update));
-    _serial->sem_give();
-
-    // wait for at least one value to be read
-    uint32_t tstart = hal.scheduler->millis();
-    while (!_updated) {
-        hal.scheduler->delay(10);
-        if (hal.scheduler->millis() - tstart > 1000) {
-            hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611 took more than "
-                        "1000ms to initialize"));
-            _flags.healthy = false;
-            return false;
-        }
-    }
-
-    _flags.healthy = true;
-    return true;
-}
-
-
-// Read the sensor. This is a state machine
-// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
-// temperature does not change so quickly...
-void AP_Baro_MS5611::_update(void)
+/*
+  Read the sensor. This is a state machine
+  We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
+  temperature does not change so quickly...
+*/
+void AP_Baro_MS5611::_timer(void)
 {
     // Throttle read rate to 100hz maximum.
-    if (hal.scheduler->micros() - _timer < 10000) {
+    if (hal.scheduler->micros() - _last_timer < 10000) {
         return;
     }
 
@@ -368,7 +264,7 @@ void AP_Baro_MS5611::_update(void)
 
     if (_state == 0) {
         // On state 0 we read temp
-        uint32_t d2 = _serial->read_adc();
+        uint32_t d2 = _serial->read_24bits(0);
         if (d2 != 0) {
             _s_D2 += d2;
             _d2_count++;
@@ -383,7 +279,7 @@ void AP_Baro_MS5611::_update(void)
         _state++;
         _serial->write(CMD_CONVERT_D1_OSR4096);      // Command to read pressure
     } else {
-        uint32_t d1 = _serial->read_adc();;
+        uint32_t d1 = _serial->read_24bits(0);;
         if (d1 != 0) {
             // occasional zero values have been seen on the PXF
             // board. These may be SPI errors, but safest to ignore
@@ -408,42 +304,35 @@ void AP_Baro_MS5611::_update(void)
         }
     }
 
-    _timer = hal.scheduler->micros();
+    _last_timer = hal.scheduler->micros();
     _serial->sem_give();
 }
 
-uint8_t AP_Baro_MS5611::read()
+void AP_Baro_MS5611::update()
 {
-    bool updated = _updated;
-    if (updated) {
-        uint32_t sD1, sD2;
-        uint8_t d1count, d2count;
-
-        // Suspend timer procs because these variables are written to
-        // in "_update".
-        hal.scheduler->suspend_timer_procs();
-        sD1 = _s_D1; _s_D1 = 0;
-        sD2 = _s_D2; _s_D2 = 0;
-        d1count = _d1_count; _d1_count = 0;
-        d2count = _d2_count; _d2_count = 0;
-        _updated = false;
-        hal.scheduler->resume_timer_procs();
-
-        if (d1count != 0) {
-            D1 = ((float)sD1) / d1count;
-        }
-        if (d2count != 0) {
-            D2 = ((float)sD2) / d2count;
-        }
-        _pressure_samples = d1count;
-        _raw_press = D1;
-        _raw_temp = D2;
+    if (!_updated) {
+        return;
     }
-    _calculate();
-    if (updated) {
-        _last_update = hal.scheduler->millis();
+    uint32_t sD1, sD2;
+    uint8_t d1count, d2count;
+
+    // Suspend timer procs because these variables are written to
+    // in "_update".
+    hal.scheduler->suspend_timer_procs();
+    sD1 = _s_D1; _s_D1 = 0;
+    sD2 = _s_D2; _s_D2 = 0;
+    d1count = _d1_count; _d1_count = 0;
+    d2count = _d2_count; _d2_count = 0;
+    _updated = false;
+    hal.scheduler->resume_timer_procs();
+    
+    if (d1count != 0) {
+        D1 = ((float)sD1) / d1count;
+    }
+    if (d2count != 0) {
+        D2 = ((float)sD2) / d2count;
     }
-    return updated ? 1 : 0;
+    _calculate();
 }
 
 // Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
@@ -453,7 +342,6 @@ void AP_Baro_MS5611::_calculate()
     float TEMP;
     float OFF;
     float SENS;
-    float P;
 
     // Formulas from manufacturer datasheet
     // sub -20c temperature compensation is not included
@@ -478,18 +366,7 @@ void AP_Baro_MS5611::_calculate()
         SENS = SENS - SENS2;
     }
 
-    P = (D1*SENS/2097152 - OFF)/32768;
-    Temp = (TEMP + 2000) * 0.01f;
-    Press = P;
-}
-
-float AP_Baro_MS5611::get_pressure()
-{
-    return Press;
-}
-
-float AP_Baro_MS5611::get_temperature() const
-{
-    // temperature in degrees C units
-    return Temp;
+    float pressure = (D1*SENS/2097152 - OFF)/32768;
+    float temperature = (TEMP + 2000) * 0.01f;
+    _copy_to_frontend(_instance, pressure, temperature);
 }
diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h
index 959dcb2ba..abebe2d51 100644
--- a/libraries/AP_Baro/AP_Baro_MS5611.h
+++ b/libraries/AP_Baro/AP_Baro_MS5611.h
@@ -6,15 +6,10 @@
 #include <AP_HAL.h>
 #include "AP_Baro.h"
 
-#if CONFIG_HAL_BOARD != HAL_BOARD_APM2 && CONFIG_HAL_BOARD != HAL_BOARD_APM1
-#define MS5611_WITH_I2C 1
-#else
-#define MS5611_WITH_I2C 0
-#endif
+#define MS5611_I2C_ADDR 0x77
 
-
-/** Abstract serial device driver for MS5611. */
-class AP_Baro_MS5611_Serial
+/** Abstract serial bus device driver for I2C/SPI. */
+class AP_SerialBus
 {
 public:
     /** Initialize the driver. */
@@ -23,107 +18,87 @@ public:
     /** Read a 16-bit value from register "reg". */
     virtual uint16_t read_16bits(uint8_t reg) = 0;
 
-    /** Read a 24-bit value from the ADC. */
-    virtual uint32_t read_adc() = 0;
+    /** Read a 24-bit value */
+    virtual uint32_t read_24bits(uint8_t reg) = 0;
 
-    /** Write a single byte command. */
+    /** Write to a register with no data. */
     virtual void write(uint8_t reg) = 0;
 
     /** Acquire the internal semaphore for this device.
      * take_nonblocking should be used from the timer process,
      * take_blocking from synchronous code (i.e. init) */
-    virtual bool sem_take_nonblocking() { return true; }
-    virtual bool sem_take_blocking() { return true; }
+    virtual bool sem_take_nonblocking() = 0;
+    virtual bool sem_take_blocking() = 0;
 
     /** Release the internal semaphore for this device. */
-    virtual void sem_give() {}
+    virtual void sem_give() = 0;
 };
 
 /** SPI serial device. */
-class AP_Baro_MS5611_SPI : public AP_Baro_MS5611_Serial
+class AP_SerialBus_SPI : public AP_SerialBus
 {
 public:
-    virtual void init();
-    virtual uint16_t read_16bits(uint8_t reg);
-    virtual uint32_t read_adc();
-    virtual void write(uint8_t reg);
-    virtual bool sem_take_nonblocking();
-    virtual bool sem_take_blocking();
-    virtual void sem_give();
+    AP_SerialBus_SPI(enum AP_HAL::SPIDevice device, enum AP_HAL::SPIDeviceDriver::bus_speed speed);
+    void init();
+    uint16_t read_16bits(uint8_t reg);
+    uint32_t read_24bits(uint8_t reg);
+    uint32_t read_adc(uint8_t reg);
+    void write(uint8_t reg);
+    bool sem_take_nonblocking();
+    bool sem_take_blocking();
+    void sem_give();
 
 private:
+    enum AP_HAL::SPIDeviceDriver::bus_speed _speed;
+    enum AP_HAL::SPIDevice _device;
     AP_HAL::SPIDeviceDriver *_spi;
     AP_HAL::Semaphore *_spi_sem;
 };
 
-#if MS5611_WITH_I2C
 /** I2C serial device. */
-class AP_Baro_MS5611_I2C : public AP_Baro_MS5611_Serial
+class AP_SerialBus_I2C : public AP_SerialBus
 {
 public:
-    virtual void init();
-    virtual uint16_t read_16bits(uint8_t reg);
-    virtual uint32_t read_adc();
-    virtual void write(uint8_t reg);
-    virtual bool sem_take_nonblocking();
-    virtual bool sem_take_blocking();
-    virtual void sem_give();
+    AP_SerialBus_I2C(uint8_t addr);
+    void init();
+    uint16_t read_16bits(uint8_t reg);
+    uint32_t read_24bits(uint8_t reg);
+    void write(uint8_t reg);
+    bool sem_take_nonblocking();
+    bool sem_take_blocking();
+    void sem_give();
 
 private:
+    uint8_t _addr;
     AP_HAL::Semaphore *_i2c_sem;
 };
-#endif // MS5611_WITH_I2C
 
-class AP_Baro_MS5611 : public AP_Baro
+class AP_Baro_MS5611 : public AP_Baro_Backend
 {
 public:
-    AP_Baro_MS5611(AP_Baro_MS5611_Serial *serial)
-    {
-        _serial = serial;
-    }
-
-    /* AP_Baro public interface: */
-    bool            init();
-    uint8_t         read();
-    float           get_pressure(); // in mbar*100 units
-    float           get_temperature() const; // in celsius degrees
-
-
-    /* Serial port drivers to pass to "init". */
-    static AP_Baro_MS5611_SPI spi;
-#if MS5611_WITH_I2C
-    static AP_Baro_MS5611_I2C i2c;
-#endif
+    AP_Baro_MS5611(AP_Baro &baro, AP_SerialBus *serial);
+    void update();
 
 private:
-    void            _calculate();
-    /* Asynchronous handler functions: */
-    void                            _update();
+    AP_SerialBus *_serial;
+    uint8_t _instance;
 
-#if CONFIG_HAL_BOARD != HAL_BOARD_APM2
-    bool check_crc(void);
-#endif
+    void _calculate();
+    bool _check_crc();
+
+    void _timer();
 
     /* Asynchronous state: */
-    static volatile bool            _updated;
-    static volatile uint8_t         _d1_count;
-    static volatile uint8_t         _d2_count;
-    static volatile uint32_t        _s_D1, _s_D2;
-    static uint8_t                  _state;
-    static uint32_t                 _timer;
-    static AP_Baro_MS5611_Serial   *_serial;
-    /* Gates access to asynchronous state: */
-    static bool                     _sync_access;
-
-    float                           Temp;
-    float                           Press;
-
-    int32_t                         _raw_press;
-    int32_t                         _raw_temp;
-    // Internal calibration registers
-    uint16_t                        C1,C2,C3,C4,C5,C6;
-    float                           D1,D2;
+    volatile bool            _updated;
+    volatile uint8_t         _d1_count;
+    volatile uint8_t         _d2_count;
+    volatile uint32_t        _s_D1, _s_D2;
+    uint8_t                  _state;
+    uint32_t                 _last_timer;
 
+    // Internal calibration registers
+    uint16_t                 C1,C2,C3,C4,C5,C6;
+    float                    D1,D2;
 };
 
 #endif //  __AP_BARO_MS5611_H__
diff --git a/libraries/AP_Baro/AP_Baro_PX4.cpp b/libraries/AP_Baro/AP_Baro_PX4.cpp
index 24d046215..8847c93f0 100644
--- a/libraries/AP_Baro/AP_Baro_PX4.cpp
+++ b/libraries/AP_Baro/AP_Baro_PX4.cpp
@@ -2,7 +2,7 @@
 
 #include <AP_HAL.h>
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
 #include <AP_Baro.h>
 #include "AP_Baro_PX4.h"
 
@@ -16,71 +16,62 @@
 
 extern const AP_HAL::HAL& hal;
 
-// Public Methods //////////////////////////////////////////////////////////////
-bool AP_Baro_PX4::init(void)
+/*
+  constructor - opens the PX4 drivers
+ */
+AP_Baro_PX4::AP_Baro_PX4(AP_Baro &baro) :
+    AP_Baro_Backend(baro),
+    _num_instances(0)
 {
-    if (_baro_fd <= 0) {
-        _baro_fd = open(BARO_DEVICE_PATH, O_RDONLY);
-        if (_baro_fd < 0) {
-            hal.scheduler->panic("Unable to open " BARO_DEVICE_PATH);
+    memset(instances, 0, sizeof(instances));
+    instances[0].fd = open(BARO_DEVICE_PATH, O_RDONLY);
+    instances[1].fd = open(BARO_DEVICE_PATH"1", O_RDONLY);
+
+    for (uint8_t i=0; i<2; i++) {
+        if (instances[i].fd != -1) {
+            _num_instances = i+1;
+        } else {
+            break;
         }
+    }
+
+    for (uint8_t i=0; i<_num_instances; i++) {
+        instances[i].instance = _frontend.register_sensor();
 
         /* set the driver to poll at 150Hz */
-        ioctl(_baro_fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX);
+        ioctl(instances[i].fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX);
 
         // average over up to 20 samples
-        ioctl(_baro_fd, SENSORIOCSQUEUEDEPTH, 20);
-
-        // give the timer a chance to run and gather one sample
-        hal.scheduler->delay(40);
-        _accumulate();
+        ioctl(instances[i].fd, SENSORIOCSQUEUEDEPTH, 20);
     }
-
-    return true;
 }
 
 // Read the sensor
-uint8_t AP_Baro_PX4::read(void)
+void AP_Baro_PX4::update(void)
 {
-    // try to accumulate one more sample, so we have the latest data
-    _accumulate();
-
-    // consider the baro healthy if we got a reading in the last 0.2s
-    _flags.healthy = (hal.scheduler->micros64() - _last_timestamp < 200000);
-    if (!_flags.healthy || _sum_count == 0) {
-        return _flags.healthy;
+    for (uint8_t i=0; i<_num_instances; i++) {
+        struct baro_report baro_report;
+        struct px4_instance &instance = instances[i];
+        while (::read(instance.fd, &baro_report, sizeof(baro_report)) == sizeof(baro_report) &&
+               baro_report.timestamp != instance.last_timestamp) {
+            instance.pressure_sum += baro_report.pressure; // Pressure in mbar
+            instance.temperature_sum += baro_report.temperature; // degrees celcius
+            instance.sum_count++;
+            instance.last_timestamp = baro_report.timestamp;
+        }
     }
 
-    _pressure    = (_pressure_sum / _sum_count) * 100.0f;
-    _temperature = _temperature_sum / _sum_count;
-    _pressure_samples = _sum_count;
-    _last_update = (uint32_t)_last_timestamp/1000;
-    _pressure_sum = 0;
-    _temperature_sum = 0;
-    _sum_count = 0;
-
-    return 1;
-}
-
-// accumulate sensor values
-void AP_Baro_PX4::_accumulate(void)
-{
-    struct baro_report baro_report;
-    while (::read(_baro_fd, &baro_report, sizeof(baro_report)) == sizeof(baro_report) &&
-           baro_report.timestamp != _last_timestamp) {
-		_pressure_sum += baro_report.pressure; // Pressure in mbar
-		_temperature_sum += baro_report.temperature; // degrees celcius
-        _sum_count++;
-        _last_timestamp = baro_report.timestamp;
+    for (uint8_t i=0; i<_num_instances; i++) {
+        struct px4_instance &instance = instances[i];
+        if (instance.sum_count > 0) {
+            float pressure = (instance.pressure_sum / instance.sum_count) * 100;
+            float temperature = instance.temperature_sum / instance.sum_count;
+            instance.pressure_sum = 0;
+            instance.temperature_sum = 0;
+            instance.sum_count = 0;
+            _copy_to_frontend(instance.instance, pressure, temperature);
+        }
     }
 }
 
-float AP_Baro_PX4::get_pressure() {
-    return _pressure;
-}
-
-float AP_Baro_PX4::get_temperature() const {
-    return _temperature;
-}
-
 #endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_Baro/AP_Baro_PX4.h b/libraries/AP_Baro/AP_Baro_PX4.h
index c9f7a5b75..b7df09cb0 100644
--- a/libraries/AP_Baro/AP_Baro_PX4.h
+++ b/libraries/AP_Baro/AP_Baro_PX4.h
@@ -5,25 +5,23 @@
 
 #include "AP_Baro.h"
 
-class AP_Baro_PX4 : public AP_Baro
+class AP_Baro_PX4 : public AP_Baro_Backend
 {
 public:
-    bool init();
-    uint8_t read();
-    float get_pressure();
-    float get_temperature() const;
+    AP_Baro_PX4(AP_Baro &);
+    void update();
 
 private:
-    float _temperature;
-    float _pressure;
-    float _pressure_sum;
-    float _temperature_sum;
-    uint32_t _sum_count;
-    void _accumulate(void);
-    void _baro_timer(uint32_t now);
-    uint64_t _last_timestamp;
-    // baro driver handle
-    int _baro_fd;
+    uint8_t _num_instances;
+
+    struct px4_instance {
+        uint8_t instance;
+        int fd;
+        float pressure_sum;
+        float temperature_sum;
+        uint32_t sum_count;
+        uint64_t last_timestamp;
+    } instances[BARO_MAX_INSTANCES];
 };
 
 #endif //  __AP_BARO_PX4_H__
diff --git a/libraries/AP_Baro/AP_Baro_VRBRAIN.cpp b/libraries/AP_Baro/AP_Baro_VRBRAIN.cpp
deleted file mode 100644
index ef27b63be..000000000
--- a/libraries/AP_Baro/AP_Baro_VRBRAIN.cpp
+++ /dev/null
@@ -1,86 +0,0 @@
-/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-#include <AP_HAL.h>
-
-#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
-#include <AP_Baro.h>
-#include "AP_Baro_VRBRAIN.h"
-
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-#include <unistd.h>
-
-#include <drivers/drv_baro.h>
-#include <drivers/drv_hrt.h>
-
-extern const AP_HAL::HAL& hal;
-
-// Public Methods //////////////////////////////////////////////////////////////
-bool AP_Baro_VRBRAIN::init(void)
-{
-    if (_baro_fd <= 0) {
-        _baro_fd = open(BARO_DEVICE_PATH, O_RDONLY);
-        if (_baro_fd < 0) {
-            hal.scheduler->panic("Unable to open " BARO_DEVICE_PATH);
-        }
-
-        /* set the driver to poll at 150Hz */
-        ioctl(_baro_fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX);
-
-        // average over up to 20 samples
-        ioctl(_baro_fd, SENSORIOCSQUEUEDEPTH, 20);
-
-        // give the timer a chance to run and gather one sample
-        hal.scheduler->delay(40);
-        _accumulate();
-    }
-
-    return true;
-}
-
-// Read the sensor
-uint8_t AP_Baro_VRBRAIN::read(void)
-{
-    // try to accumulate one more sample, so we have the latest data
-    _accumulate();
-
-    // consider the baro healthy if we got a reading in the last 0.2s
-    _flags.healthy = (hal.scheduler->micros64() - _last_timestamp < 200000);
-    if (!_flags.healthy || _sum_count == 0) {
-        return _flags.healthy;
-    }
-
-    _pressure    = (_pressure_sum / _sum_count) * 100.0f;
-    _temperature = _temperature_sum / _sum_count;
-    _pressure_samples = _sum_count;
-    _last_update = (uint32_t)_last_timestamp/1000;
-    _pressure_sum = 0;
-    _temperature_sum = 0;
-    _sum_count = 0;
-
-    return 1;
-}
-
-// accumulate sensor values
-void AP_Baro_VRBRAIN::_accumulate(void)
-{
-    struct baro_report baro_report;
-    while (::read(_baro_fd, &baro_report, sizeof(baro_report)) == sizeof(baro_report) &&
-           baro_report.timestamp != _last_timestamp) {
-		_pressure_sum += baro_report.pressure; // Pressure in mbar
-		_temperature_sum += baro_report.temperature; // degrees celcius
-        _sum_count++;
-        _last_timestamp = baro_report.timestamp;
-    }
-}
-
-float AP_Baro_VRBRAIN::get_pressure() {
-    return _pressure;
-}
-
-float AP_Baro_VRBRAIN::get_temperature() const {
-    return _temperature;
-}
-
-#endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_Baro/AP_Baro_VRBRAIN.h b/libraries/AP_Baro/AP_Baro_VRBRAIN.h
deleted file mode 100644
index 794cddc87..000000000
--- a/libraries/AP_Baro/AP_Baro_VRBRAIN.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-#ifndef __AP_BARO_VRBRAIN_H__
-#define __AP_BARO_VRBRAIN_H__
-
-#include "AP_Baro.h"
-
-class AP_Baro_VRBRAIN : public AP_Baro
-{
-public:
-    bool init();
-    uint8_t read();
-    float get_pressure();
-    float get_temperature() const;
-
-private:
-    float _temperature;
-    float _pressure;
-    float _pressure_sum;
-    float _temperature_sum;
-    uint32_t _sum_count;
-    void _accumulate(void);
-    void _baro_timer(uint32_t now);
-    uint64_t _last_timestamp;
-    // baro driver handle
-    int _baro_fd;
-};
-
-#endif //  __AP_BARO_VRBRAIN_H__
diff --git a/libraries/AP_Baro/examples/BARO_generic/BARO_generic.pde b/libraries/AP_Baro/examples/BARO_generic/BARO_generic.pde
index 6a9b2c9d8..f6c08a4fd 100644
--- a/libraries/AP_Baro/examples/BARO_generic/BARO_generic.pde
+++ b/libraries/AP_Baro/examples/BARO_generic/BARO_generic.pde
@@ -38,21 +38,7 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
 
 #define CONFIG_BARO HAL_BARO_DEFAULT
 
-#if CONFIG_BARO == HAL_BARO_BMP085
-static AP_Baro_BMP085 barometer;
-#elif CONFIG_BARO == HAL_BARO_PX4
-static AP_Baro_PX4 barometer;
-#elif CONFIG_BARO == HAL_BARO_VRBRAIN
-static AP_Baro_VRBRAIN barometer;
-#elif CONFIG_BARO == HAL_BARO_HIL
-static AP_Baro_HIL barometer;
-#elif CONFIG_BARO == HAL_BARO_MS5611
-static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::i2c);
-#elif CONFIG_BARO == HAL_BARO_MS5611_SPI
-static AP_Baro_MS5611 barometer(&AP_Baro_MS5611::spi);
-#else
- #error Unrecognized CONFIG_BARO setting
-#endif
+static AP_Baro barometer;
 
 static uint32_t timer;
 
@@ -78,7 +64,7 @@ void loop()
 {
     if((hal.scheduler->micros() - timer) > 100000UL) {
         timer = hal.scheduler->micros();
-        barometer.read();
+        barometer.update();
         uint32_t read_time = hal.scheduler->micros() - timer;
         float alt = barometer.get_altitude();
         if (!barometer.healthy()) {
@@ -91,10 +77,9 @@ void loop()
         hal.console->print(barometer.get_temperature());
         hal.console->print(" Altitude:");
         hal.console->print(alt);
-        hal.console->printf(" climb=%.2f t=%u samples=%u",
-                      barometer.get_climb_rate(),
-                      (unsigned)read_time,
-                      (unsigned)barometer.get_pressure_samples());
+        hal.console->printf(" climb=%.2f t=%u",
+                            barometer.get_climb_rate(),
+                            (unsigned)read_time);
         hal.console->println();
     }
 }
diff --git a/libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde b/libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde
index f41c8b359..e8d681317 100644
--- a/libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde
+++ b/libraries/AP_HAL_FLYMAPLE/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde
@@ -75,9 +75,8 @@ void loop()
         tmp_float = pow(tmp_float, 0.190295);
         float alt = 44330.0 * (1.0 - tmp_float);
         hal.console->print(alt);
-        hal.console->printf(" t=%lu samples=%u", 
-			    read_time, 
-			    (unsigned)bmp085.get_pressure_samples());
+        hal.console->printf(" t=%lu", 
+			    read_time);
         hal.console->println();
     }
 }
-- 
GitLab