Redo the math in the ms5611 driver to a) avoid re-computing scaling factors for every pressure measurement, b) be perhaps more readable and follow the data sheet more closely, and c) support calibration of the MSL pressure.

2 files changed, 232 insertions, 50 deletions

diff --git a/apps/drivers/drv_baro.h b/apps/drivers/drv_baro.h
index b79d5b5b0..176f798c0 100644
--- a/apps/drivers/drv_baro.h
+++ b/apps/drivers/drv_baro.h
@@ -69,6 +69,10 @@ ORB_DECLARE(sensor_baro);
 #define _BAROIOCBASE		(0x2200)
 #define _BAROIOC(_n)		(_IOC(_BAROIOCBASE, _n))
 
-/* currently no baro-specific ioctls */
+/** set corrected MSL pressure in pascals */
+#define BAROIOCSMSLPRESSURE	_BAROIOC(0)
+
+/** get current MSL pressure in pascals */
+#define BAROIOCGMSLPRESSURE	_BAROIOC(1)
 
 #endif /* _DRV_BARO_H */
diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp
index 39ace2806..4639ae497 100644
--- a/apps/drivers/ms5611/ms5611.cpp
+++ b/apps/drivers/ms5611/ms5611.cpp
@@ -127,8 +127,13 @@ private:
 	bool			_collect_phase;
 	unsigned		_measure_phase;
 
-	int32_t			_dT;
-	int64_t			_temp64;
+	/* 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;
 
@@ -194,6 +199,13 @@ private:
 	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.
@@ -213,6 +225,9 @@ private:
 /* 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.
  */
@@ -224,12 +239,12 @@ private:
 #define MS5611_ADDRESS_1		0x76    /* 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 /* read from this address to reset chip (0b0011110 on bus) */
-#define ADDR_CMD_CONVERT_D1		0x48 /* 4096 samples to this address to start conversion (0b01001000 on bus) */
-#define ADDR_CMD_CONVERT_D2		0x58 /* 4096 samples */
-#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 */
+#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.
@@ -246,8 +261,10 @@ MS5611::MS5611(int bus) :
 	_reports(nullptr),
 	_collect_phase(false),
 	_measure_phase(0),
-	_dT(0),
-	_temp64(0),
+	_TEMP(0),
+	_OFF(0),
+	_SENS(0),
+	_msl_pressure(101325),
 	_baro_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
@@ -314,18 +331,13 @@ MS5611::probe()
 int
 MS5611::probe_address(uint8_t address)
 {
-	uint8_t cmd = ADDR_RESET_CMD;
-
 	/* select the address we are going to try */
 	set_address(address);
 
 	/* send reset command */
-	if (OK != transfer(&cmd, 1, nullptr, 0))
+	if (OK != cmd_reset())
 		return -EIO;
 
-	/* wait for PROM contents to be in the device (2.8 ms) */
-	usleep(3000);
-
 	/* read PROM */
 	if (OK != read_prom())
 		return -EIO;
@@ -501,10 +513,22 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* 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:
-		/* give it to the superclass */
-		return I2C::ioctl(filp, cmd, arg);
+		break;
 	}
+
+	/* give it to the superclass */
+	return I2C::ioctl(filp, cmd, arg);
 }
 
 void
@@ -610,6 +634,10 @@ MS5611::collect()
 {
 	uint8_t cmd;
 	uint8_t data[3];
+	union {
+		uint8_t	b[4];
+		uint32_t w;
+	} cvt;
 
 	/* read the most recent measurement */
 	cmd = 0;
@@ -625,40 +653,77 @@ MS5611::collect()
 	}
 
 	/* fetch the raw value */
-	uint32_t raw = (((uint32_t)data[0]) << 16) | (((uint32_t)data[1]) << 8) | ((uint32_t)data[2]);
+	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 calculation */
-		_dT = raw - (((int32_t)_prom.s.c5_reference_temp) * 256);
-		_temp64 = 2000 + (((int64_t)_dT) * _prom.s.c6_temp_coeff_temp) / 8388608;
+		/* temperature offset (in ADC units) */
+		int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8);
 
-	} else {
+		/* 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;
+			}
 
-		/* pressure calculation */
-		int64_t offset = (int64_t)_prom.s.c2_pressure_offset * 65536 + ((int64_t)_dT * _prom.s.c4_temp_coeff_pres_offset) / 128;
-		int64_t sens = (int64_t)_prom.s.c1_pressure_sens * 32768 + ((int64_t)_dT * _prom.s.c3_temp_coeff_pres_sens) / 256;
-
-		/* it's pretty cold, second order temperature compensation needed */
-		if (_temp64 < 2000) {
-			/* second order temperature compensation */
-			int64_t temp2 = (((int64_t)_dT) * _dT) >> 31;
-			int64_t tmp_64 = (_temp64 - 2000) * (_temp64 - 2000);
-			int64_t offset2 = (5 * tmp_64) >> 1;
-			int64_t sens2 = (5 * tmp_64) >> 2;
-			_temp64 = _temp64 - temp2;
-			offset = offset - offset2;
-			sens = sens - sens2;
+			_TEMP -= T2;
+			_OFF  -= OFF2;
+			_SENS -= SENS2;
 		}
+	} else {
 
-		int64_t press_int64 = (((raw * sens) / 2097152 - offset) / 32768);
+		/* pressure calculation, result in Pa */
+		int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
 
 		/* generate a new report */
-		_reports[_next_report].temperature = _temp64 / 100.0f;
-		_reports[_next_report].pressure = press_int64 / 100.0f;
-		/* convert as double for max. precision, store as float (more than enough precision) */
-		_reports[_next_report].altitude = (44330.0 * (1.0 - pow((press_int64 / 101325.0), 0.190295)));
+		_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 */
+
+		/* 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 */
+
+		/* 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;
 
 		/* publish it */
 		orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]);
@@ -685,11 +750,36 @@ MS5611::collect()
 }
 
 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()
 {
-	/* read PROM data */
-	uint8_t prom_buf[2] = {255, 255};
+	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);
 
@@ -697,11 +787,12 @@ MS5611::read_prom()
 			break;
 
 		/* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */
-		_prom.c[i] = (((uint16_t)prom_buf[0]) << 8) | ((uint16_t)prom_buf[1]);
-
+		cvt.b[0] = prom_buf[1];
+		cvt.b[1] = prom_buf[0];
+		_prom.c[i] = cvt.w;
 	}
 
-	/* calculate CRC and return false */
+	/* calculate CRC and return success/failure accordingly */
 	return crc4(&_prom.c[0]) ? OK : -EIO;
 }
 
@@ -757,7 +848,19 @@ MS5611::print_info()
 	printf("poll interval:  %u ticks\n", _measure_ticks);
 	printf("report queue:   %u (%u/%u @ %p)\n",
 	       _num_reports, _oldest_report, _next_report, _reports);
-	printf("dT/temp64:      %d/%lld\n", _dT, _temp64);
+	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);
 }
 
 /**
@@ -772,6 +875,7 @@ void	start();
 void	test();
 void	reset();
 void	info();
+void	calibrate(unsigned altitude);
 
 /**
  * Start the driver.
@@ -824,7 +928,7 @@ test()
 
 	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);
+		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));
@@ -905,6 +1009,68 @@ 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
 
@@ -935,5 +1101,17 @@ ms5611_main(int argc, char *argv[])
 	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'");
 }