From ce4ef6d59d063265d878f1f44da83c88b56eab33 Mon Sep 17 00:00:00 2001 From: Andrey Gusakov Date: Thu, 14 Dec 2023 02:17:58 +0300 Subject: [PATCH] lsm303agr: check WHO_AM_I register on lsm303agrStart() --- os/ex/devices/ST/lsm303agr.c | 13 ++++++++++++- os/ex/devices/ST/lsm303agr.h | 2 +- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/os/ex/devices/ST/lsm303agr.c b/os/ex/devices/ST/lsm303agr.c index 5a013b69a..b563da5ce 100644 --- a/os/ex/devices/ST/lsm303agr.c +++ b/os/ex/devices/ST/lsm303agr.c @@ -775,8 +775,9 @@ void lsm303agrObjectInit(LSM303AGRDriver *devp) { * * @api */ -void lsm303agrStart(LSM303AGRDriver *devp, const LSM303AGRConfig *config) { +msg_t lsm303agrStart(LSM303AGRDriver *devp, const LSM303AGRConfig *config) { uint32_t i; + uint8_t devid; uint8_t cr[4]; osalDbgCheck((devp != NULL) && (config != NULL)); @@ -871,6 +872,14 @@ void lsm303agrStart(LSM303AGRDriver *devp, const LSM303AGRConfig *config) { #endif /* LSM303AGR_SHARED_I2C || LSM303AGR_SHARED_SPI */ lsm303agrStartBus(devp->config); + /* Check WHO_I_AM */ + lsm303agrReadRegister(devp->config, LSM303AGR_SAD_ACC, + LSM303AGR_AD_WHO_AM_I_A, &devid, 1); + if (devid != 0x33) + { + return MSG_RESET; + } + lsm303agrWriteRegister(devp->config, LSM303AGR_SAD_ACC, LSM303AGR_AD_CTRL_REG1_A, cr, 4); @@ -915,6 +924,8 @@ void lsm303agrStart(LSM303AGRDriver *devp, const LSM303AGRConfig *config) { osalThreadSleepMilliseconds(5); devp->state = LSM303AGR_READY; + + return MSG_OK; } /** diff --git a/os/ex/devices/ST/lsm303agr.h b/os/ex/devices/ST/lsm303agr.h index f13ed1f22..c1e1fde5d 100644 --- a/os/ex/devices/ST/lsm303agr.h +++ b/os/ex/devices/ST/lsm303agr.h @@ -919,7 +919,7 @@ struct LSM303AGRDriver { extern "C" { #endif void lsm303agrObjectInit(LSM303AGRDriver *devp); - void lsm303agrStart(LSM303AGRDriver *devp, const LSM303AGRConfig *config); + msg_t lsm303agrStart(LSM303AGRDriver *devp, const LSM303AGRConfig *config); void lsm303agrStop(LSM303AGRDriver *devp); #ifdef __cplusplus }