Merge branch 'master' of github.com:PX4/Firmware

3 files changed, 35 insertions, 6 deletions

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index e6d765e13..970e8cf4b 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -377,9 +377,12 @@ out:
 int
 L3GD20::probe()
 {
+	irqstate_t flags = irqsave();
 	/* read dummy value to void to clear SPI statemachine on sensor */
 	(void)read_reg(ADDR_WHO_AM_I);
 
+	bool success = false;
+
 	/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
 	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
 
@@ -390,15 +393,21 @@ L3GD20::probe()
 		#else
 			#error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
 		#endif
-		return OK;
+
+		success = true;
 	}
 
 
 	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) {
 		_orientation = SENSOR_BOARD_ROTATION_180_DEG;
-		return OK;
+		success = true;
 	}
 
+	irqrestore(flags);
+
+	if (success)
+		return OK;
+
 	return -EIO;
 }
 
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 05d6f1881..35904cc4d 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -404,7 +404,7 @@ public:
 	LSM303D_mag(LSM303D *parent);
 	~LSM303D_mag();
 
-	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual ssize_t			read(struct file *filp, char *buffer, size_t buflen);
 	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
 
 protected:
@@ -498,8 +498,10 @@ LSM303D::init()
 	int mag_ret;
 
 	/* do SPI init (and probe) first */
-	if (SPI::init() != OK)
+	if (SPI::init() != OK) {
+		warnx("SPI init failed");
 		goto out;
+	}
 
 	/* allocate basic report buffers */
 	_num_accel_reports = 2;
@@ -541,6 +543,7 @@ out:
 void
 LSM303D::reset()
 {
+	irqstate_t flags = irqsave();
 	/* enable accel*/
 	write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
 
@@ -555,6 +558,7 @@ LSM303D::reset()
 
 	mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
 	mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
+	irqrestore(flags);
 
 	_accel_read = 0;
 	_mag_read = 0;
@@ -563,11 +567,16 @@ LSM303D::reset()
 int
 LSM303D::probe()
 {
+	irqstate_t flags = irqsave();
 	/* read dummy value to void to clear SPI statemachine on sensor */
 	(void)read_reg(ADDR_WHO_AM_I);
 
 	/* verify that the device is attached and functioning */
-	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM)
+	bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM);
+	
+	irqrestore(flags);
+
+	if (success)
 		return OK;
 
 	return -EIO;
@@ -1470,8 +1479,10 @@ start()
 	/* create the driver */
 	g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
 
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
+		warnx("failed instantiating LSM303D obj");
 		goto fail;
+	}
 
 	if (OK != g_dev->init())
 		goto fail;
diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp
index f6c624340..21caed2ff 100644
--- a/src/drivers/ms5611/ms5611_spi.cpp
+++ b/src/drivers/ms5611/ms5611_spi.cpp
@@ -134,6 +134,7 @@ int
 MS5611_SPI::init()
 {
 	int ret;
+	irqstate_t flags;
 
 	ret = SPI::init();
 	if (ret != OK) {
@@ -141,15 +142,23 @@ MS5611_SPI::init()
 		goto out;
 	}
 
+	/* disable interrupts, make this section atomic */
+	flags = irqsave();
 	/* send reset command */
 	ret = _reset();
+	/* re-enable interrupts */
+	irqrestore(flags);
 	if (ret != OK) {
 		debug("reset failed");
 		goto out;
 	}
 
+	/* disable interrupts, make this section atomic */
+	flags = irqsave();
 	/* read PROM */
 	ret = _read_prom();
+	/* re-enable interrupts */
+	irqrestore(flags);
 	if (ret != OK) {
 		debug("prom readout failed");
 		goto out;