dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Guard probe / reset against other SPI drivers

This commit is contained in:
Lorenz Meier 2013-09-11 22:14:56 +02:00
parent 8755d76d1b
commit 3a326cb467
2 changed files with 26 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/drivers/l3gd20/l3gd20.cpp
View File

@ -377,9 +377,12 @@ out:
int int
L3GD20::probe() L3GD20::probe()
{ {
irqstate_t flags = irqsave();
/* read dummy value to void to clear SPI statemachine on sensor */ /* read dummy value to void to clear SPI statemachine on sensor */
(void)read_reg(ADDR_WHO_AM_I); (void)read_reg(ADDR_WHO_AM_I);
bool success = false;
/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
@ -390,15 +393,21 @@ L3GD20::probe()
#else #else
#error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
#endif #endif
return OK;
success = true;
} }
if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) { if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) {
_orientation = SENSOR_BOARD_ROTATION_180_DEG; _orientation = SENSOR_BOARD_ROTATION_180_DEG;
return OK; success = true;
} }
irqrestore(flags);
if (success)
return OK;
return -EIO; return -EIO;
} }

19
src/drivers/lsm303d/lsm303d.cpp
View File

@ -404,7 +404,7 @@ public:
LSM303D_mag(LSM303D *parent); LSM303D_mag(LSM303D *parent);
~LSM303D_mag(); ~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); virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
protected: protected:
@ -498,8 +498,10 @@ LSM303D::init()
int mag_ret; int mag_ret;
/* do SPI init (and probe) first */ /* do SPI init (and probe) first */
if (SPI::init() != OK) if (SPI::init() != OK) {
warnx("SPI init failed");
goto out; goto out;
}
/* allocate basic report buffers */ /* allocate basic report buffers */
_num_accel_reports = 2; _num_accel_reports = 2;
@ -541,6 +543,7 @@ out:
void void
LSM303D::reset() LSM303D::reset()
{ {
irqstate_t flags = irqsave();
/* enable accel*/ /* enable accel*/
write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); 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_range(LSM303D_MAG_DEFAULT_RANGE_GA);
mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
irqrestore(flags);
_accel_read = 0; _accel_read = 0;
_mag_read = 0; _mag_read = 0;
@ -563,11 +567,16 @@ LSM303D::reset()
int int
LSM303D::probe() LSM303D::probe()
{ {
irqstate_t flags = irqsave();
/* read dummy value to void to clear SPI statemachine on sensor */ /* read dummy value to void to clear SPI statemachine on sensor */
(void)read_reg(ADDR_WHO_AM_I); (void)read_reg(ADDR_WHO_AM_I);
/* verify that the device is attached and functioning */ /* 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 OK;
return -EIO; return -EIO;
@ -1470,8 +1479,10 @@ start()
/* create the driver */ /* create the driver */
g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); 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; goto fail;
}
if (OK != g_dev->init()) if (OK != g_dev->init())
goto fail; goto fail;