Moved the bulk of sensor drivers to the new world

1 files changed, 1214 insertions, 0 deletions

diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
new file mode 100644
index 000000000..59ab5936e
--- /dev/null
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -0,0 +1,1214 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ms5611.cpp
+ * Driver for the MS5611 barometric pressure sensor connected via I2C.
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_baro.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+/**
+ * Calibration PROM as reported by the device.
+ */
+#pragma pack(push,1)
+struct ms5611_prom_s {
+	uint16_t factory_setup;
+	uint16_t c1_pressure_sens;
+	uint16_t c2_pressure_offset;
+	uint16_t c3_temp_coeff_pres_sens;
+	uint16_t c4_temp_coeff_pres_offset;
+	uint16_t c5_reference_temp;
+	uint16_t c6_temp_coeff_temp;
+	uint16_t serial_and_crc;
+};
+
+/**
+ * Grody hack for crc4()
+ */
+union ms5611_prom_u {
+	uint16_t c[8];
+	struct ms5611_prom_s s;
+};
+#pragma pack(pop)
+
+class MS5611 : public device::I2C
+{
+public:
+	MS5611(int bus);
+	virtual ~MS5611();
+
+	virtual int		init();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void			print_info();
+
+protected:
+	virtual int		probe();
+
+private:
+	union ms5611_prom_u	_prom;
+
+	struct work_s		_work;
+	unsigned		_measure_ticks;
+
+	unsigned		_num_reports;
+	volatile unsigned	_next_report;
+	volatile unsigned	_oldest_report;
+	struct baro_report	*_reports;
+
+	bool			_collect_phase;
+	unsigned		_measure_phase;
+
+	/* intermediate temperature values per MS5611 datasheet */
+	int32_t			_TEMP;
+	int64_t			_OFF;
+	int64_t			_SENS;
+
+	/* altitude conversion calibration */
+	unsigned		_msl_pressure;	/* in kPa */
+
+	orb_advert_t		_baro_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_measure_perf;
+	perf_counter_t		_comms_errors;
+	perf_counter_t		_buffer_overflows;
+
+	/**
+	 * Test whether the device supported by the driver is present at a
+	 * specific address.
+	 *
+	 * @param address	The I2C bus address to probe.
+	 * @return		True if the device is present.
+	 */
+	int			probe_address(uint8_t address);
+
+	/**
+	 * Initialise the automatic measurement state machine and start it.
+	 *
+	 * @note This function is called at open and error time.  It might make sense
+	 *       to make it more aggressive about resetting the bus in case of errors.
+	 */
+	void			start_cycle();
+
+	/**
+	 * Stop the automatic measurement state machine.
+	 */
+	void			stop_cycle();
+
+	/**
+	 * Perform a poll cycle; collect from the previous measurement
+	 * and start a new one.
+	 *
+	 * This is the heart of the measurement state machine.  This function
+	 * alternately starts a measurement, or collects the data from the
+	 * previous measurement.
+	 *
+	 * When the interval between measurements is greater than the minimum
+	 * measurement interval, a gap is inserted between collection
+	 * and measurement to provide the most recent measurement possible
+	 * at the next interval.
+	 */
+	void			cycle();
+
+	/**
+	 * Static trampoline from the workq context; because we don't have a
+	 * generic workq wrapper yet.
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		cycle_trampoline(void *arg);
+
+	/**
+	 * Issue a measurement command for the current state.
+	 *
+	 * @return		OK if the measurement command was successful.
+	 */
+	int			measure();
+
+	/**
+	 * Collect the result of the most recent measurement.
+	 */
+	int			collect();
+
+	/**
+	 * Send a reset command to the MS5611.
+	 *
+	 * This is required after any bus reset.
+	 */
+	int			cmd_reset();
+
+	/**
+	 * Read the MS5611 PROM
+	 *
+	 * @return		OK if the PROM reads successfully.
+	 */
+	int			read_prom();
+
+	/**
+	 * PROM CRC routine ported from MS5611 application note
+	 *
+	 * @param n_prom	Pointer to words read from PROM.
+	 * @return		True if the CRC matches.
+	 */
+	bool			crc4(uint16_t *n_prom);
+
+};
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
+
+/* helper macro for arithmetic - returns the square of the argument */
+#define POW2(_x)		((_x) * (_x))
+
+/*
+ * MS5611 internal constants and data structures.
+ */
+
+/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
+#define MS5611_CONVERSION_INTERVAL	10000	/* microseconds */
+#define MS5611_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
+
+#define MS5611_BUS			PX4_I2C_BUS_ONBOARD
+#define MS5611_ADDRESS_1		PX4_I2C_OBDEV_MS5611 /* address select pins pulled high (PX4FMU series v1.6+) */
+#define MS5611_ADDRESS_2		0x77    /* address select pins pulled low (PX4FMU prototypes) */
+
+#define ADDR_RESET_CMD			0x1E	/* write to this address to reset chip */
+#define ADDR_CMD_CONVERT_D1		0x48	/* write to this address to start temperature conversion */
+#define ADDR_CMD_CONVERT_D2		0x58	/* write to this address to start pressure conversion */
+#define ADDR_DATA			0x00	/* address of 3 bytes / 32bit pressure data */
+#define ADDR_PROM_SETUP			0xA0	/* address of 8x 2 bytes factory and calibration data */
+#define ADDR_PROM_C1			0xA2	/* address of 6x 2 bytes calibration data */
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
+
+
+MS5611::MS5611(int bus) :
+	I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000),
+	_measure_ticks(0),
+	_num_reports(0),
+	_next_report(0),
+	_oldest_report(0),
+	_reports(nullptr),
+	_collect_phase(false),
+	_measure_phase(0),
+	_TEMP(0),
+	_OFF(0),
+	_SENS(0),
+	_msl_pressure(101325),
+	_baro_topic(-1),
+	_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
+	_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
+	_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
+{
+	// enable debug() calls
+	_debug_enabled = true;
+
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+MS5611::~MS5611()
+{
+	/* make sure we are truly inactive */
+	stop_cycle();
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete[] _reports;
+}
+
+int
+MS5611::init()
+{
+	int ret = ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK)
+		goto out;
+
+	/* allocate basic report buffers */
+	_num_reports = 2;
+	_reports = new struct baro_report[_num_reports];
+
+	if (_reports == nullptr)
+		goto out;
+
+	_oldest_report = _next_report = 0;
+
+	/* get a publish handle on the baro topic */
+	memset(&_reports[0], 0, sizeof(_reports[0]));
+	_baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]);
+
+	if (_baro_topic < 0)
+		debug("failed to create sensor_baro object");
+
+	ret = OK;
+out:
+	return ret;
+}
+
+int
+MS5611::probe()
+{
+	_retries = 10;
+
+	if ((OK == probe_address(MS5611_ADDRESS_1)) ||
+	    (OK == probe_address(MS5611_ADDRESS_2))) {
+		/*
+	    	 * Disable retries; we may enable them selectively in some cases,
+		 * but the device gets confused if we retry some of the commands.
+	    	 */
+		_retries = 0;
+		return OK;
+	}
+
+	return -EIO;
+}
+
+int
+MS5611::probe_address(uint8_t address)
+{
+	/* select the address we are going to try */
+	set_address(address);
+
+	/* send reset command */
+	if (OK != cmd_reset())
+		return -EIO;
+
+	/* read PROM */
+	if (OK != read_prom())
+		return -EIO;
+
+	return OK;
+}
+
+ssize_t
+MS5611::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct baro_report);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_measure_ticks > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the workq thread while we are doing this;
+		 * we are careful to avoid racing with them.
+		 */
+		while (count--) {
+			if (_oldest_report != _next_report) {
+				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
+				ret += sizeof(_reports[0]);
+				INCREMENT(_oldest_report, _num_reports);
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement - run one conversion */
+	/* XXX really it'd be nice to lock against other readers here */
+	do {
+		_measure_phase = 0;
+		_oldest_report = _next_report = 0;
+
+		/* do temperature first */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		usleep(MS5611_CONVERSION_INTERVAL);
+
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* now do a pressure measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		usleep(MS5611_CONVERSION_INTERVAL);
+
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		memcpy(buffer, _reports, sizeof(*_reports));
+		ret = sizeof(*_reports);
+
+	} while (0);
+
+	return ret;
+}
+
+int
+MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+				/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop_cycle();
+				_measure_ticks = 0;
+				return OK;
+
+				/* external signalling not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+				/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+				/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start_cycle();
+
+					return OK;
+				}
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL))
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start_cycle();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_measure_ticks == 0)
+			return SENSOR_POLLRATE_MANUAL;
+
+		return (1000 / _measure_ticks);
+
+	case SENSORIOCSQUEUEDEPTH: {
+			/* add one to account for the sentinel in the ring */
+			arg++;
+
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 2) || (arg > 100))
+				return -EINVAL;
+
+			/* allocate new buffer */
+			struct baro_report *buf = new struct baro_report[arg];
+
+			if (nullptr == buf)
+				return -ENOMEM;
+
+			/* reset the measurement state machine with the new buffer, free the old */
+			stop_cycle();
+			delete[] _reports;
+			_num_reports = arg;
+			_reports = buf;
+			start_cycle();
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _num_reports - 1;
+
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+
+	case BAROIOCSMSLPRESSURE:
+
+		/* range-check for sanity */
+		if ((arg < 80000) || (arg > 120000))
+			return -EINVAL;
+
+		_msl_pressure = arg;
+		return OK;
+
+	case BAROIOCGMSLPRESSURE:
+		return _msl_pressure;
+
+	default:
+		break;
+	}
+
+	/* give it to the superclass */
+	return I2C::ioctl(filp, cmd, arg);
+}
+
+void
+MS5611::start_cycle()
+{
+
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_measure_phase = 0;
+	_oldest_report = _next_report = 0;
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
+}
+
+void
+MS5611::stop_cycle()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+MS5611::cycle_trampoline(void *arg)
+{
+	MS5611 *dev = (MS5611 *)arg;
+
+	dev->cycle();
+}
+
+void
+MS5611::cycle()
+{
+	int ret;
+
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		ret = collect();
+		if (ret != OK) {
+			if (ret == -6) {
+				/*
+				 * The ms5611 seems to regularly fail to respond to
+				 * its address; this happens often enough that we'd rather not
+				 * spam the console with the message.
+				 */
+			} else {
+				//log("collection error %d", ret);
+			}
+			/* reset the collection state machine and try again */
+			start_cycle();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 * Don't inject one after temperature measurements, so we can keep
+		 * doing pressure measurements at something close to the desired rate.
+		 */
+		if ((_measure_phase != 0) &&
+		    (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&MS5611::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	ret = measure();
+	if (ret != OK) {
+		//log("measure error %d", ret);
+		/* reset the collection state machine and try again */
+		start_cycle();
+		return;
+	}
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&MS5611::cycle_trampoline,
+		   this,
+		   USEC2TICK(MS5611_CONVERSION_INTERVAL));
+}
+
+int
+MS5611::measure()
+{
+	int ret;
+
+	perf_begin(_measure_perf);
+
+	/*
+	 * In phase zero, request temperature; in other phases, request pressure.
+	 */
+	uint8_t	cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
+
+	/*
+	 * Send the command to begin measuring.
+	 *
+	 * Disable retries on this command; we can't know whether failure 
+	 * means the device did or did not see the write.
+	 */
+	_retries = 0;
+	ret = transfer(&cmd_data, 1, nullptr, 0);
+
+	if (OK != ret)
+		perf_count(_comms_errors);
+
+	perf_end(_measure_perf);
+
+	return ret;
+}
+
+int
+MS5611::collect()
+{
+	int ret;
+	uint8_t cmd;
+	uint8_t data[3];
+	union {
+		uint8_t	b[4];
+		uint32_t w;
+	} cvt;
+
+	/* read the most recent measurement */
+	cmd = 0;
+
+	perf_begin(_sample_perf);
+
+	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
+	_reports[_next_report].timestamp = hrt_absolute_time();
+
+	ret = transfer(&cmd, 1, &data[0], 3);
+	if (ret != OK) {
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+
+	/* fetch the raw value */
+	cvt.b[0] = data[2];
+	cvt.b[1] = data[1];
+	cvt.b[2] = data[0];
+	cvt.b[3] = 0;
+	uint32_t raw = cvt.w;
+
+	/* handle a measurement */
+	if (_measure_phase == 0) {
+
+		/* temperature offset (in ADC units) */
+		int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8);
+
+		/* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */
+		_TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23);
+
+		/* base sensor scale/offset values */
+		_SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8);
+		_OFF  = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7);
+
+		/* temperature compensation */
+		if (_TEMP < 2000) {
+
+			int32_t T2 = POW2(dT) >> 31;
+
+			int64_t f = POW2((int64_t)_TEMP - 2000);
+			int64_t OFF2 = 5 * f >> 1;
+			int64_t SENS2 = 5 * f >> 2;
+
+			if (_TEMP < -1500) {
+				int64_t f2 = POW2(_TEMP + 1500);
+				OFF2 += 7 * f2;
+				SENS2 += 11 * f2 >> 1;
+			}
+
+			_TEMP -= T2;
+			_OFF  -= OFF2;
+			_SENS -= SENS2;
+		}
+
+	} else {
+
+		/* pressure calculation, result in Pa */
+		int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
+
+		/* generate a new report */
+		_reports[_next_report].temperature = _TEMP / 100.0f;
+		_reports[_next_report].pressure = P / 100.0f;		/* convert to millibar */
+
+		/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
+
+		/*
+		 * PERFORMANCE HINT:
+		 *
+		 * The single precision calculation is 50 microseconds faster than the double
+		 * precision variant. It is however not obvious if double precision is required.
+		 * Pending more inspection and tests, we'll leave the double precision variant active.
+		 *
+		 * Measurements:
+		 * 	double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us
+		 *	single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us
+		 */
+#if 0/* USE_FLOAT */
+		/* tropospheric properties (0-11km) for standard atmosphere */
+		const float T1 = 15.0f + 273.15f;	/* temperature at base height in Kelvin */
+		const float a  = -6.5f / 1000f;	/* temperature gradient in degrees per metre */
+		const float g  = 9.80665f;	/* gravity constant in m/s/s */
+		const float R  = 287.05f;	/* ideal gas constant in J/kg/K */
+
+		/* current pressure at MSL in kPa */
+		float p1 = _msl_pressure / 1000.0f;
+
+		/* measured pressure in kPa */
+		float p = P / 1000.0f;
+
+		/*
+		 * Solve:
+		 *
+		 *     /        -(aR / g)     \
+		 *    | (p / p1)          . T1 | - T1
+		 *     \                      /
+		 * h = -------------------------------  + h1
+		 *                   a
+		 */
+		_reports[_next_report].altitude = (((powf((p / p1), (-(a * R) / g))) * T1) - T1) / a;
+#else
+		/* tropospheric properties (0-11km) for standard atmosphere */
+		const double T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
+		const double a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
+		const double g  = 9.80665;	/* gravity constant in m/s/s */
+		const double R  = 287.05;	/* ideal gas constant in J/kg/K */
+
+		/* current pressure at MSL in kPa */
+		double p1 = _msl_pressure / 1000.0;
+
+		/* measured pressure in kPa */
+		double p = P / 1000.0;
+
+		/*
+		 * Solve:
+		 *
+		 *     /        -(aR / g)     \
+		 *    | (p / p1)          . T1 | - T1
+		 *     \                      /
+		 * h = -------------------------------  + h1
+		 *                   a
+		 */
+		_reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
+#endif
+		/* publish it */
+		orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]);
+
+		/* post a report to the ring - note, not locked */
+		INCREMENT(_next_report, _num_reports);
+
+		/* if we are running up against the oldest report, toss it */
+		if (_next_report == _oldest_report) {
+			perf_count(_buffer_overflows);
+			INCREMENT(_oldest_report, _num_reports);
+		}
+
+		/* notify anyone waiting for data */
+		poll_notify(POLLIN);
+	}
+
+	/* update the measurement state machine */
+	INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1);
+
+	perf_end(_sample_perf);
+
+	return OK;
+}
+
+int
+MS5611::cmd_reset()
+{
+	unsigned	old_retrycount = _retries;
+	uint8_t		cmd = ADDR_RESET_CMD;
+	int		result;
+
+	/* bump the retry count */
+	_retries = 10;
+	result = transfer(&cmd, 1, nullptr, 0);
+	_retries = old_retrycount;
+
+	return result;
+}
+
+int
+MS5611::read_prom()
+{
+	uint8_t		prom_buf[2];
+	union {
+		uint8_t		b[2];
+		uint16_t	w;
+	} cvt;
+
+	/*
+	 * Wait for PROM contents to be in the device (2.8 ms) in the case we are
+	 * called immediately after reset.
+	 */
+	usleep(3000);
+
+	/* read and convert PROM words */
+	for (int i = 0; i < 8; i++) {
+		uint8_t cmd = ADDR_PROM_SETUP + (i * 2);
+
+		if (OK != transfer(&cmd, 1, &prom_buf[0], 2))
+			break;
+
+		/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
+		cvt.b[0] = prom_buf[1];
+		cvt.b[1] = prom_buf[0];
+		_prom.c[i] = cvt.w;
+	}
+
+	/* calculate CRC and return success/failure accordingly */
+	return crc4(&_prom.c[0]) ? OK : -EIO;
+}
+
+bool
+MS5611::crc4(uint16_t *n_prom)
+{
+	int16_t cnt;
+	uint16_t n_rem;
+	uint16_t crc_read;
+	uint8_t n_bit;
+
+	n_rem = 0x00;
+
+	/* save the read crc */
+	crc_read = n_prom[7];
+
+	/* remove CRC byte */
+	n_prom[7] = (0xFF00 & (n_prom[7]));
+
+	for (cnt = 0; cnt < 16; cnt++) {
+		/* uneven bytes */
+		if (cnt & 1) {
+			n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF);
+
+		} else {
+			n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8);
+		}
+
+		for (n_bit = 8; n_bit > 0; n_bit--) {
+			if (n_rem & 0x8000) {
+				n_rem = (n_rem << 1) ^ 0x3000;
+
+			} else {
+				n_rem = (n_rem << 1);
+			}
+		}
+	}
+
+	/* final 4 bit remainder is CRC value */
+	n_rem = (0x000F & (n_rem >> 12));
+	n_prom[7] = crc_read;
+
+	/* return true if CRCs match */
+	return (0x000F & crc_read) == (n_rem ^ 0x00);
+}
+
+void
+MS5611::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	printf("report queue:   %u (%u/%u @ %p)\n",
+	       _num_reports, _oldest_report, _next_report, _reports);
+	printf("TEMP:           %d\n", _TEMP);
+	printf("SENS:           %lld\n", _SENS);
+	printf("OFF:            %lld\n", _OFF);
+	printf("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));
+
+	printf("factory_setup             %u\n", _prom.s.factory_setup);
+	printf("c1_pressure_sens          %u\n", _prom.s.c1_pressure_sens);
+	printf("c2_pressure_offset        %u\n", _prom.s.c2_pressure_offset);
+	printf("c3_temp_coeff_pres_sens   %u\n", _prom.s.c3_temp_coeff_pres_sens);
+	printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset);
+	printf("c5_reference_temp         %u\n", _prom.s.c5_reference_temp);
+	printf("c6_temp_coeff_temp        %u\n", _prom.s.c6_temp_coeff_temp);
+	printf("serial_and_crc            %u\n", _prom.s.serial_and_crc);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace ms5611
+{
+
+MS5611	*g_dev;
+
+void	start();
+void	test();
+void	reset();
+void	info();
+void	calibrate(unsigned altitude);
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new MS5611(MS5611_BUS);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(BARO_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		goto fail;
+
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	struct baro_report report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report))
+		err(1, "immediate read failed");
+
+	warnx("single read");
+	warnx("pressure:    %10.4f", (double)report.pressure);
+	warnx("altitude:    %11.4f", (double)report.altitude);
+	warnx("temperature: %8.4f", (double)report.temperature);
+	warnx("time:        %lld", report.timestamp);
+
+	/* set the queue depth to 10 */
+	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
+		errx(1, "failed to set queue depth");
+
+	/* start the sensor polling at 2Hz */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+		errx(1, "failed to set 2Hz poll rate");
+
+	/* read the sensor 5x and report each value */
+	for (unsigned i = 0; i < 5; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 2000);
+
+		if (ret != 1)
+			errx(1, "timed out waiting for sensor data");
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report))
+			err(1, "periodic read failed");
+
+		warnx("periodic read %u", i);
+		warnx("pressure:    %10.4f", (double)report.pressure);
+		warnx("altitude:    %11.4f", (double)report.altitude);
+		warnx("temperature: %8.4f", (double)report.temperature);
+		warnx("time:        %lld", report.timestamp);
+	}
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "driver poll restart failed");
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+/**
+ * Calculate actual MSL pressure given current altitude
+ */
+void
+calibrate(unsigned altitude)
+{
+	struct baro_report report;
+	float	pressure;
+	float	p1;
+
+	int fd = open(BARO_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
+
+	/* start the sensor polling at max */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX))
+		errx(1, "failed to set poll rate");
+
+	/* average a few measurements */
+	pressure = 0.0f;
+
+	for (unsigned i = 0; i < 20; i++) {
+		struct pollfd fds;
+		int ret;
+		ssize_t sz;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 1000);
+
+		if (ret != 1)
+			errx(1, "timed out waiting for sensor data");
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report))
+			err(1, "sensor read failed");
+
+		pressure += report.pressure;
+	}
+
+	pressure /= 20;		/* average */
+	pressure /= 10;		/* scale from millibar to kPa */
+
+	/* tropospheric properties (0-11km) for standard atmosphere */
+	const float T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
+	const float a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
+	const float g  = 9.80665f;	/* gravity constant in m/s/s */
+	const float R  = 287.05f;	/* ideal gas constant in J/kg/K */
+
+	warnx("averaged pressure %10.4fkPa at %um", pressure, altitude);
+
+	p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R))));
+
+	warnx("calculated MSL pressure %10.4fkPa", p1);
+
+	/* save as integer Pa */
+	p1 *= 1000.0f;
+
+	if (ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK)
+		err(1, "BAROIOCSMSLPRESSURE");
+
+	exit(0);
+}
+
+} // namespace
+
+int
+ms5611_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start"))
+		ms5611::start();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		ms5611::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		ms5611::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info"))
+		ms5611::info();
+
+	/*
+	 * Perform MSL pressure calibration given an altitude in metres
+	 */
+	if (!strcmp(argv[1], "calibrate")) {
+		if (argc < 2)
+			errx(1, "missing altitude");
+
+		long altitude = strtol(argv[2], nullptr, 10);
+
+		ms5611::calibrate(altitude);
+	}
+
+	errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
+}