diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 73e0d1d05d..6c09b49168 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -40,6 +40,12 @@ else
 	adc start
 fi
 
+if ver hwcmp NXPHLITE_V3
+then
+	# Internal I2C
+	mpl3115a2 -I start
+fi
+
 if ver hwcmp AUAV_X21
 then
 	# External I2C bus
@@ -146,21 +152,6 @@ then
 	fi
 fi
 
-if ver hwcmp PX4_SAME70XPLAINED_V1
-then
-	# External I2C bus
-	if hmc5883 -C -T -X start
-	then
-	fi
-
-	set BOARD_FMUV3 false
-
-	# Internal SPI bus mpu9250 is rotated 90 deg yaw
-	if mpu9250 -R 2 start
-	then
-	fi
-fi
-
 if ver hwcmp PX4FMU_V4
 then
 	# External I2C bus
diff --git a/src/drivers/mpl3115a2/mpl3115a2.cpp b/src/drivers/mpl3115a2/mpl3115a2.cpp
index 5d7fb22201..654b692bf0 100644
--- a/src/drivers/mpl3115a2/mpl3115a2.cpp
+++ b/src/drivers/mpl3115a2/mpl3115a2.cpp
@@ -98,13 +98,11 @@ enum MPL3115A2_BUS {
  * MPL3115A2 internal constants and data structures.
  */
 
-/*
- * Maximum internal conversion time for TBD
- */
 #define MPL3115A2_CONVERSION_INTERVAL	10000	/* microseconds */
-#define MPL3115A2_MEASUREMENT_RATIO	3	/* pressure measurements per temperature measurement */
-#define MPL3115A2_BARO_DEVICE_PATH_EXT	"/dev/mpl3115a2_ext"
-#define MPL3115A2_BARO_DEVICE_PATH_INT	"/dev/mpl3115a2_int"
+#define MPL3115A2_OSR                   0       /* Over Sample rate of 1 6MS Minimum time between data samples */
+#define MPL3115A2_CTRL_TRIGGER          (CTRL_REG1_OST | CTRL_REG1_OS(MPL3115A2_OSR))
+#define MPL3115A2_BARO_DEVICE_PATH_EXT  "/dev/mpl3115a2_ext"
+#define MPL3115A2_BARO_DEVICE_PATH_INT  "/dev/mpl3115a2_int"
 
 class MPL3115A2 : public device::CDev
 {
@@ -130,12 +128,8 @@ protected:
 
 	ringbuffer::RingBuffer	*_reports;
 	bool			_collect_phase;
-	unsigned		_measure_phase;
 
-	/* intermediate TBD! temperature values per MPL3115A2 datasheet */
-	int32_t			_TEMP;
-	int64_t			_OFF;
-	int64_t			_SENS;
+	/*  */
 	float			_P;
 	float			_T;
 
@@ -206,6 +200,7 @@ protected:
 	 * Collect the result of the most recent measurement.
 	 */
 	virtual int		collect();
+
 };
 
 /*
@@ -219,10 +214,8 @@ MPL3115A2::MPL3115A2(device::Device *interface, const char *path) :
 	_measure_ticks(0),
 	_reports(nullptr),
 	_collect_phase(false),
-	_measure_phase(0),
-	_TEMP(0),
-	_OFF(0),
-	_SENS(0),
+	_P(0),
+	_T(0),
 	_msl_pressure(101325),
 	_baro_topic(nullptr),
 	_orb_class_instance(-1),
@@ -288,33 +281,34 @@ MPL3115A2::init()
 	_class_instance = register_class_devname(BARO_BASE_DEVICE_PATH);
 
 	struct baro_report brp;
-	/* do a first measurement cycle to populate reports with valid data */
-	_measure_phase = 0;
 	_reports->flush();
 
 	while (true) {
-		/* do temperature first */
-		if (OK != measure()) {
-			ret = -EIO;
-			break;
-		}
 
-		usleep(MPL3115A2_CONVERSION_INTERVAL);
+		/* First reading */
+		do  {
+			ret = measure();
 
-		if (OK != collect()) {
-			ret = -EIO;
-			break;
-		}
+			if (ret == -EAGAIN) {
+				usleep(500);
+			}
+		} while (ret == -EAGAIN);
 
-		/* now do a pressure measurement */
-		if (OK != measure()) {
+		if (ret != OK) {
 			ret = -EIO;
 			break;
 		}
 
-		usleep(MPL3115A2_CONVERSION_INTERVAL);
+		/* First reading */
+		do  {
+			ret = collect();
 
-		if (OK != collect()) {
+			if (ret == -EAGAIN) {
+				usleep(500);
+			}
+		} while (ret == -EAGAIN);
+
+		if (ret != OK) {
 			ret = -EIO;
 			break;
 		}
@@ -376,30 +370,22 @@ MPL3115A2::read(struct file *filp, char *buffer, size_t buflen)
 
 	/* manual measurement - run one conversion */
 	do {
-		_measure_phase = 0;
 		_reports->flush();
 
-		/* do temperature first */
-		if (OK != measure()) {
-			ret = -EIO;
-			break;
-		}
-
-		usleep(MPL3115A2_CONVERSION_INTERVAL);
+		/* First reading */
+		do  {
+			ret = measure();
 
-		if (OK != collect()) {
-			ret = -EIO;
-			break;
-		}
+			if (ret == -EAGAIN) {
+				usleep(1000);
+			}
+		} while (ret == -EAGAIN);
 
-		/* now do a pressure measurement */
-		if (OK != measure()) {
+		if (ret != OK) {
 			ret = -EIO;
 			break;
 		}
 
-		usleep(MPL3115A2_CONVERSION_INTERVAL);
-
 		if (OK != collect()) {
 			ret = -EIO;
 			break;
@@ -506,12 +492,15 @@ MPL3115A2::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCGQUEUEDEPTH:
 		return _reports->size();
 
-	case SENSORIOCRESET:
-		/*
-		 * Since we are initialized, we do not need to do anything, since the
-		 * PROM is correctly read and the part does not need to be configured.
-		 */
-		return OK;
+	case SENSORIOCRESET: {
+			/*
+			 * Since we are initialized, we do not need to do anything, since the
+			 * PROM is correctly read and the part does not need to be configured.
+			 */
+			unsigned int dummy;
+			_interface->ioctl(IOCTL_RESET, dummy);
+			return OK;
+		}
 
 	case BAROIOCSMSLPRESSURE:
 
@@ -541,7 +530,6 @@ MPL3115A2::start_cycle(unsigned delay_ticks)
 
 	/* reset the report ring and state machine */
 	_collect_phase = false;
-	_measure_phase = 0;
 	_reports->flush();
 
 	/* schedule a cycle to start things */
@@ -574,17 +562,7 @@ MPL3115A2::cycle()
 		/* perform collection */
 		ret = collect();
 
-		if (ret != OK) {
-			if (ret == -6) {
-				/*
-				 * The mpl3115a2 seems to regularly fail to respond to
-				 * its address; this happens often enough that we'd rather not
-				 * spam the console with a message for this.
-				 */
-			} else {
-				//DEVICE_LOG("collection error %d", ret);
-			}
-
+		if (ret == -EIO) {
 			/* issue a reset command to the sensor */
 			_interface->ioctl(IOCTL_RESET, dummy);
 			/* reset the collection state machine and try again - we need
@@ -595,32 +573,27 @@ MPL3115A2::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(MPL3115A2_CONVERSION_INTERVAL))) {
+		if (ret == -EAGAIN) {
 
-			/* schedule a fresh cycle call when we are ready to measure again */
+			/* Ready read it on next cycle */
 			work_queue(HPWORK,
 				   &_work,
 				   (worker_t)&MPL3115A2::cycle_trampoline,
 				   this,
-				   _measure_ticks - USEC2TICK(MPL3115A2_CONVERSION_INTERVAL));
-
+				   USEC2TICK(MPL3115A2_CONVERSION_INTERVAL));
 			return;
 		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
 	}
 
-	/* measurement phase */
+	/* Look for a ready condition */
+
 	ret = measure();
 
-	if (ret != OK) {
+	if (ret == -EIO) {
 		/* issue a reset command to the sensor */
 		_interface->ioctl(IOCTL_RESET, dummy);
 		/* reset the collection state machine and try again */
@@ -628,7 +601,7 @@ MPL3115A2::cycle()
 		return;
 	}
 
-	/* next phase is collection */
+	/* next phase is measurement */
 	_collect_phase = true;
 
 	/* schedule a fresh cycle call when the measurement is done */
@@ -647,149 +620,115 @@ MPL3115A2::measure()
 	perf_begin(_measure_perf);
 
 	/*
-	 * In phase TBD
-	 */
-	unsigned addr = 0;
-
-	/*
-	 * Send the command to begin measuring.
+	 * Send the command to read the ADC for P and T.
 	 */
+	unsigned addr = (MPL3115A2_CTRL_REG1 << 8) | MPL3115A2_CTRL_TRIGGER;
 	ret = _interface->ioctl(IOCTL_MEASURE, addr);
 
-	if (OK != ret) {
+	if (ret == -EIO) {
 		perf_count(_comms_errors);
 	}
 
 	perf_end(_measure_perf);
 
-	return ret;
+	return PX4_OK;
 }
 
 int
 MPL3115A2::collect()
 {
 	int ret;
-	uint32_t raw;
+	MPL3115A2_data_t reading;
+	uint8_t ctrl;
 
 	perf_begin(_sample_perf);
 
+	ret = _interface->read(MPL3115A2_CTRL_REG1, (void *)&ctrl, 1);
+
+	if (ret == -EIO) {
+		perf_end(_sample_perf);
+		return ret;
+	}
+
+	if (ctrl & CTRL_REG1_OST) {
+		perf_end(_sample_perf);
+		return -EAGAIN;
+	}
+
 	struct baro_report report;
+
 	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
 	report.timestamp = hrt_absolute_time();
+
 	report.error_count = perf_event_count(_comms_errors);
 
 	/* read the most recent measurement - read offset/size are hardcoded in the interface */
-	ret = _interface->read(0, (void *)&raw, 0);
+	ret = _interface->read(OUT_P_MSB, (void *)&reading, sizeof(reading));
 
-	if (ret < 0) {
+	if (ret == -EIO) {
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
 		return ret;
 	}
 
-	/* handle a measurement */
-	if (_measure_phase == 0) {
-
-		/* temperature offset (in ADC units) */
-		int32_t dT = (int32_t)raw;
-
-		/* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */
-		_TEMP = 2000 + (int32_t)(((int64_t)dT * 1) >> 23);
-
-		/* base sensor scale/offset values */
 
-		/* Perform MPL3115A2 Caculation */
+	_T = (float) reading.temperature.b[1] + ((float)(reading.temperature.b[0]) / 16.0f);
 
-		_OFF  = ((int64_t) 1 << 16) + (((int64_t)1 * dT) >> 7);
-		_SENS = ((int64_t)1 << 15) + (((int64_t)1 * dT) >> 8);
+	_P = (float)(reading.pressure.q >> 8) + ((float)(reading.pressure.b[0]) / 4.0f);
 
-		/* MPL3115A2 temperature compensation */
+	report.temperature = _T;
+	report.pressure = _P / 100.0f;		/* convert to millibar */
 
-		if (_TEMP < 2000) {
+	/* return device ID */
+	report.device_id = _device_id.devid;
 
-			int32_t T2 = POW2(dT) >> 31;
+	/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
 
-			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;
-		_P = P * 0.01f;
-		_T = _TEMP * 0.01f;
-
-		/* generate a new report */
-		report.temperature = _TEMP / 100.0f;
-		report.pressure = P / 100.0f;		/* convert to millibar */
-
-		/* return device ID */
-		report.device_id = _device_id.devid;
-
-		/* 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: mpl3115a2_read: 992 events, 258641us elapsed, min 202us max 305us
-		 *	single precision: mpl3115a2_read: 963 events, 208066us elapsed, min 202us max 241us
-		 */
+	/*
+	 * 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: mpl3115a2_read: 992 events, 258641us elapsed, min 202us max 305us
+	 *	single precision: mpl3115a2_read: 963 events, 208066us elapsed, min 202us max 241us
+	 */
 
-		/* 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 */
+	/* 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;
+	/* current pressure at MSL in kPa */
+	double p1 = _msl_pressure / 1000.0;
 
-		/* measured pressure in kPa */
-		double p = P / 1000.0;
+	/* measured pressure in kPa */
+	double p = (double) _P / 1000.0;
 
-		/*
-		 * Solve:
-		 *
-		 *     /        -(aR / g)     \
-		 *    | (p / p1)          . T1 | - T1
-		 *     \                      /
-		 * h = -------------------------------  + h1
-		 *                   a
-		 */
-		report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
+	/*
+	 * Solve:
+	 *
+	 *     /        -(aR / g)     \
+	 *    | (p / p1)          . T1 | - T1
+	 *     \                      /
+	 * h = -------------------------------  + h1
+	 *                   a
+	 */
+	report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
 
+	/* publish it */
+	if (!(_pub_blocked) && _baro_topic != nullptr) {
 		/* publish it */
-		if (!(_pub_blocked) && _baro_topic != nullptr) {
-			/* publish it */
-			orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
-		}
-
-		_reports->force(&report);
-
-		/* notify anyone waiting for data */
-		poll_notify(POLLIN);
+		orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
 	}
 
-	/* update the measurement state machine */
-	INCREMENT(_measure_phase, MPL3115A2_MEASUREMENT_RATIO + 1);
+	_reports->force(&report);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
 
 	perf_end(_sample_perf);
 
@@ -804,9 +743,6 @@ MPL3115A2::print_info()
 	printf("poll interval:  %u ticks\n", _measure_ticks);
 	_reports->print_info("report queue");
 	printf("device:         mpl3115a2\n");
-	printf("TEMP:           %d\n", _TEMP);
-	printf("SENS:           %lld\n", _SENS);
-	printf("OFF:            %lld\n", _OFF);
 	printf("P:              %.3f\n", (double)_P);
 	printf("T:              %.3f\n", (double)_T);
 	printf("MSL pressure:   %10.4f\n", (double)(_msl_pressure / 100.f));
@@ -1144,14 +1080,10 @@ calibrate(unsigned altitude, enum MPL3115A2_BUS busid)
 void
 usage()
 {
-	warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'");
+	warnx("missing command: try 'start', 'info', 'test', 'reset', 'calibrate'");
 	warnx("options:");
 	warnx("    -X    (external I2C bus)");
 	warnx("    -I    (intternal I2C bus)");
-	warnx("    -S    (external SPI bus)");
-	warnx("    -s    (internal SPI bus)");
-	warnx("    -T    5611|5607 (default 5611)");
-	warnx("    -T    0 (autodetect version)");
 
 }
 
@@ -1220,10 +1152,11 @@ mpl3115a2_main(int argc, char *argv[])
 			errx(1, "missing altitude");
 		}
 
-		long altitude = strtol(argv[optind + 1], nullptr, 10);
+		long altitude = strtol(argv[myoptind + 1], nullptr, 10);
 
 		mpl3115a2::calibrate(altitude, busid);
 	}
 
-	errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
+	mpl3115a2::usage();
+	exit(1);
 }
diff --git a/src/drivers/mpl3115a2/mpl3115a2.h b/src/drivers/mpl3115a2/mpl3115a2.h
index b354bb4f63..fb451bd3a8 100644
--- a/src/drivers/mpl3115a2/mpl3115a2.h
+++ b/src/drivers/mpl3115a2/mpl3115a2.h
@@ -37,9 +37,37 @@
  * Shared defines for the mpl3115a2 driver.
  */
 
+#define MPL3115A2_REG_WHO_AM_I   0x0c
+#define MPL3115A2_WHO_AM_I       0xC4
+
+#define OUT_P_MSB                0x01
+
+#define MPL3115A2_CTRL_REG1      0x26
+#  define CTRL_REG1_ALT          (1 << 7)
+#  define CTRL_REG1_RAW          (1 << 6)
+#  define CTRL_REG1_OS_SHIFTS    (3)
+#  define CTRL_REG1_OS_MASK      (0x7 << CTRL_REG1_OS_SHIFTS)
+#  define CTRL_REG1_OS(n)        (((n)& 0x7) << CTRL_REG1_OS_SHIFTS)
+#  define CTRL_REG1_RST          (1 << 2)
+#  define CTRL_REG1_OST          (1 << 1)
+#  define CTRL_REG1_SBYB         (1 << 0)
+
 /* interface ioctls */
-#define IOCTL_RESET			2
-#define IOCTL_MEASURE			3
+#define IOCTL_RESET         1
+#define IOCTL_MEASURE       2
+
+typedef begin_packed_struct struct MPL3115A2_data_t {
+	union {
+		uint32_t q;
+		uint16_t w[sizeof(q) / sizeof(uint16_t)];
+		uint8_t  b[sizeof(q) / sizeof(uint8_t)];
+	} pressure;
+
+	union {
+		uint16_t w;
+		uint8_t  b[sizeof(w)];
+	} temperature;
+} end_packed_struct MPL3115A2_data_t;
 
 /* interface factories */
 extern device::Device *MPL3115A2_i2c_interface(uint8_t busnum);
diff --git a/src/drivers/mpl3115a2/mpl3115a2_i2c.cpp b/src/drivers/mpl3115a2/mpl3115a2_i2c.cpp
index 8ab0f168fd..664635f457 100644
--- a/src/drivers/mpl3115a2/mpl3115a2_i2c.cpp
+++ b/src/drivers/mpl3115a2/mpl3115a2_i2c.cpp
@@ -57,8 +57,6 @@
 #include "board_config.h"
 
 #define MPL3115A2_ADDRESS        0x60
-#define MPL3115A2_REG_WHO_AM_I   0x0c
-#define MPL3115A2_WHO_AM_I       0xC4
 
 device::Device *MPL3115A2_i2c_interface(uint8_t busnum);
 
@@ -84,7 +82,9 @@ private:
 	 */
 	int		_measure(unsigned addr);
 
-	int reg_read(unsigned reg, void *data, unsigned count);
+	int reg_read(uint8_t reg, void *data, unsigned count = 1);
+	int reg_write(uint8_t reg, uint8_t data);
+	int	reset();
 
 };
 
@@ -111,25 +111,54 @@ MPL3115A2_I2C::init()
 	return I2C::init();
 }
 
+int
+MPL3115A2_I2C::reset()
+{
+	int max = 10;
+	reg_write(MPL3115A2_CTRL_REG1, CTRL_REG1_RST);
+	int rv = CTRL_REG1_RST;
+	int ret = 1;
+
+	while (ret == 1 && (rv & CTRL_REG1_RST) && max--) {
+		usleep(4000);
+		ret = reg_read(MPL3115A2_CTRL_REG1, &rv);
+	}
+
+	return ret == 1 ? PX4_OK : ret;
+}
+
 int
 MPL3115A2_I2C::read(unsigned offset, void *data, unsigned count)
 {
-	union _cvt {
-		uint8_t	b[4];
-		uint32_t w;
-	} *cvt = (_cvt *)data;
-	uint8_t buf[3];
-
-	/* read the most recent measurement */
-	uint8_t cmd = 0;
-	int ret = transfer(&cmd, 1, &buf[0], 3);
-
-	if (ret == PX4_OK) {
-		/* fetch the raw value */
-		cvt->b[0] = buf[2];
-		cvt->b[1] = buf[1];
-		cvt->b[2] = buf[0];
-		cvt->b[3] = 0;
+
+	int ret = -EINVAL;
+
+	switch (offset) {
+	case MPL3115A2_CTRL_REG1:
+		ret = reg_read(offset, data, count);
+		break;
+
+	case OUT_P_MSB: {
+			union _cvt {
+				MPL3115A2_data_t reading;
+			} *cvt = (_cvt *)data;
+
+			/* read the most recent measurement
+			 * 3 Pressure and 2 temprtture
+			 */
+			uint8_t	b[3 + 2];
+			uint8_t reg = (uint8_t) offset;
+
+			ret = transfer(&reg, 1, &b[0], sizeof(b));
+
+			if (ret == PX4_OK) {
+				cvt->reading.pressure.q = ((uint32_t)b[0]) << 18 | ((uint32_t) b[1]) <<  10 | (((uint32_t)b[2]) & 0xc0) << 2 | ((
+								  b[2] & 0x30) >> 4);
+				cvt->reading.temperature.w = ((uint16_t) b[3]) << 8 | (b[4] >> 4);
+
+			}
+		}
+		break;
 	}
 
 	return ret;
@@ -142,8 +171,7 @@ MPL3115A2_I2C::ioctl(unsigned operation, unsigned &arg)
 
 	switch (operation) {
 	case IOCTL_RESET:
-		PX4_ERR("Not implemented");
-		ret = EINVAL;
+		ret = reset();
 		break;
 
 	case IOCTL_MEASURE:
@@ -163,7 +191,7 @@ MPL3115A2_I2C::probe()
 	_retries = 10;
 	uint8_t whoami = 0;
 
-	if ((reg_read(MPL3115A2_REG_WHO_AM_I, &whoami, 1) > 0) && (whoami == MPL3115A2_WHO_AM_I)) {
+	if ((reg_read(MPL3115A2_REG_WHO_AM_I, &whoami) > 0) && (whoami == MPL3115A2_WHO_AM_I)) {
 		/*
 		 * Disable retries; we may enable them selectively in some cases,
 		 * but the device gets confused if we retry some of the commands.
@@ -183,15 +211,20 @@ MPL3115A2_I2C::_measure(unsigned addr)
 	 * means the device did or did not see the command.
 	 */
 	_retries = 0;
-
-	uint8_t cmd = addr;
-	return transfer(&cmd, 1, nullptr, 0);
+	return reg_write((addr >> 8) & 0xff, addr & 0xff);
 }
 
 
-int MPL3115A2_I2C::reg_read(unsigned reg, void *data, unsigned count)
+int MPL3115A2_I2C::reg_read(uint8_t reg, void *data, unsigned count)
 {
 	uint8_t cmd = reg;
 	int ret = transfer(&cmd, 1, (uint8_t *)data, count);
 	return ret == PX4_OK ? count : ret;
 }
+
+int MPL3115A2_I2C::reg_write(uint8_t reg, uint8_t data)
+{
+	uint8_t buf[2] = { reg, data};
+	int ret = transfer(buf, sizeof(buf), NULL, 0);
+	return ret == PX4_OK ? 2 : ret;
+}