diff --git a/os/ex/devices/ST/lis302dl.c b/os/ex/devices/ST/lis302dl.c index 4ee84b4d7..2feb9cadd 100644 --- a/os/ex/devices/ST/lis302dl.c +++ b/os/ex/devices/ST/lis302dl.c @@ -441,8 +441,9 @@ void lis302dlObjectInit(LIS302DLDriver *devp) { * * @api */ -void lis302dlStart(LIS302DLDriver *devp, const LIS302DLConfig *config) { +msg_t lis302dlStart(LIS302DLDriver *devp, const LIS302DLConfig *config) { uint32_t i; + uint8_t devid; uint8_t cr[2] = {0, 0}; osalDbgCheck((devp != NULL) && (config != NULL)); @@ -451,6 +452,23 @@ void lis302dlStart(LIS302DLDriver *devp, const LIS302DLConfig *config) { devp->config = config; +#if LIS302DL_USE_SPI +#if LIS302DL_SHARED_SPI + spiAcquireBus((devp)->config->spip); +#endif /* LIS302DL_SHARED_SPI */ + spiStart((devp)->config->spip, (devp)->config->spicfg); + + /* Check WHO_I_AM */ + lis302dlSPIReadRegister(devp->config->spip, LIS302DL_AD_WHO_AM_I, + 1, &devid); + if (devid != 0x3b) + { +#if LIS302DL_SHARED_SPI + spiReleaseBus((devp)->config->spip); +#endif /* LIS302DL_SHARED_SPI */ + return MSG_RESET; + } + /* Control register 1 configuration block.*/ { cr[0] = LIS302DL_CTRL_REG1_XEN | LIS302DL_CTRL_REG1_YEN | @@ -467,12 +485,6 @@ void lis302dlStart(LIS302DLDriver *devp, const LIS302DLConfig *config) { #endif } -#if LIS302DL_USE_SPI -#if LIS302DL_SHARED_SPI - spiAcquireBus((devp)->config->spip); -#endif /* LIS302DL_SHARED_SPI */ - spiStart((devp)->config->spip, (devp)->config->spicfg); - lis302dlSPIWriteRegister(devp->config->spip, LIS302DL_AD_CTRL_REG1, 2, cr); @@ -516,6 +528,8 @@ void lis302dlStart(LIS302DLDriver *devp, const LIS302DLConfig *config) { osalThreadSleepMilliseconds(10); devp->state = LIS302DL_READY; + + return MSG_OK; } /** diff --git a/os/ex/devices/ST/lis302dl.h b/os/ex/devices/ST/lis302dl.h index 3c662f457..21b1894bd 100644 --- a/os/ex/devices/ST/lis302dl.h +++ b/os/ex/devices/ST/lis302dl.h @@ -554,7 +554,7 @@ struct LIS302DLDriver { extern "C" { #endif void lis302dlObjectInit(LIS302DLDriver *devp); - void lis302dlStart(LIS302DLDriver *devp, const LIS302DLConfig *config); + msg_t lis302dlStart(LIS302DLDriver *devp, const LIS302DLConfig *config); void lis302dlStop(LIS302DLDriver *devp); #ifdef __cplusplus }