aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
Diffstat
-rw-r--r--os/ex/ST/l3gd20.c127
-rw-r--r--os/ex/ST/l3gd20.h10
-rw-r--r--testhal/STM32/STM32F3xx/SPI-L3GD20/Makefile2
-rw-r--r--testhal/STM32/STM32F3xx/SPI-L3GD20/main.c4
-rw-r--r--testhal/STM32/STM32F4xx/SPI-L3GD20/Makefile2
-rw-r--r--testhal/STM32/STM32F4xx/SPI-L3GD20/main.c4
-rw-r--r--testhal/STM32/STM32L4xx/SPI-L3GD20/Makefile4
-rw-r--r--testhal/STM32/STM32L4xx/SPI-L3GD20/main.c4
8 files changed, 100 insertions, 57 deletions
diff --git a/os/ex/ST/l3gd20.c b/os/ex/ST/l3gd20.c
index a6551b598..58e3c5f54 100644
--- a/os/ex/ST/l3gd20.c
+++ b/os/ex/ST/l3gd20.c
@@ -52,16 +52,16 @@
*
* @param[in] spip pointer to the SPI interface
* @param[in] reg starting register address
- * @param[in] n number of adjacent registers to write
- * @param[in] b pointer to a buffer.
+ * @param[in] n number of consecutive registers to read
+ * @param[in] b pointer to an output buffer.
*/
static void l3gd20SPIReadRegister(SPIDriver *spip, uint8_t reg, size_t n,
- uint8_t* b) {
+ uint8_t* rxbuf) {
uint8_t cmd;
(n == 1) ? (cmd = reg | L3GD20_RW) : (cmd = reg | L3GD20_RW | L3GD20_MS);
spiSelect(spip);
spiSend(spip, 1, &cmd);
- spiReceive(spip, n, b);
+ spiReceive(spip, n, rxbuf);
spiUnselect(spip);
}
@@ -94,33 +94,38 @@ static size_t get_axes_number(void *ip) {
return L3GD20_NUMBER_OF_AXES;
}
-static msg_t read_raw(void *ip, int32_t axes[L3GD20_NUMBER_OF_AXES]) {
- uint8_t buff [L3GD20_NUMBER_OF_AXES * 2], i;
+static msg_t read_raw(void *ip, int32_t axes[L3GD20_NUMBER_OF_AXES]) {
int16_t tmp;
+ uint8_t i, buff [2 * L3GD20_NUMBER_OF_AXES];
+ msg_t msg = MSG_OK;
+
osalDbgCheck((ip != NULL) && (axes != NULL));
-
osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
"read_raw(), invalid state");
#if L3GD20_USE_SPI
osalDbgAssert((((L3GD20Driver *)ip)->config->spip->state == SPI_READY),
- "read_raw(), channel not ready");
+ "read_raw(), channel not ready");
+
#if L3GD20_SHARED_SPI
spiAcquireBus(((L3GD20Driver *)ip)->config->spip);
spiStart(((L3GD20Driver *)ip)->config->spip,
((L3GD20Driver *)ip)->config->spicfg);
-#endif /* L3GD20_SHARED_SPI */
+#endif /* L3GD20_SHARED_SPI */
+
l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip, L3GD20_AD_OUT_X_L,
- L3GD20_NUMBER_OF_AXES * 2, buff);
+ L3GD20_NUMBER_OF_AXES * 2, buff);
+
+#if L3GD20_SHARED_SPI
+ spiReleaseBus(((L3GD20Driver *)ip)->config->spip);
+#endif /* L3GD20_SHARED_SPI */
+#endif /* L3GD20_USE_SPI */
+
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
tmp = buff[2*i] + (buff[2*i+1] << 8);
axes[i] = (int32_t)tmp;
}
-#if L3GD20_SHARED_SPI
- spiReleaseBus(((L3GD20Driver *)ip)->config->spip);
-#endif /* L3GD20_SHARED_SPI */
-#endif /* L3GD20_USE_SPI */
- return MSG_OK;
+ return msg;
}
static msg_t read_cooked(void *ip, float axes[]) {
@@ -135,7 +140,7 @@ static msg_t read_cooked(void *ip, float axes[]) {
msg = read_raw(ip, raw);
for(i = 0; i < L3GD20_NUMBER_OF_AXES ; i++){
- axes[i] = (raw[i] * ((L3GD20Driver *)ip)->sensitivity[i]);
+ axes[i] = raw[i] * ((L3GD20Driver *)ip)->sensitivity[i];
axes[i] -= ((L3GD20Driver *)ip)->bias[i];
}
return msg;
@@ -257,6 +262,7 @@ static msg_t set_full_scale(void *ip, l3gd20_fs_t fs) {
scale = newfs / ((L3GD20Driver *)ip)->fullscale;
((L3GD20Driver *)ip)->fullscale = newfs;
+#if L3GD20_USE_SPI
#if L3GD20_SHARED_SPI
spiAcquireBus(((L3GD20Driver *)ip)->config->spip);
spiStart(((L3GD20Driver *)ip)->config->spip,
@@ -266,13 +272,27 @@ static msg_t set_full_scale(void *ip, l3gd20_fs_t fs) {
/* Updating register.*/
l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
L3GD20_AD_CTRL_REG4, 1, &cr);
+
+#if L3GD20_SHARED_SPI
+ spiReleaseBus(((L3GD20Driver *)ip)->config->spip);
+#endif /* L3GD20_SHARED_SPI */
+#endif /* L3GD20_USE_SPI */
cr &= ~(L3GD20_CTRL_REG4_FS_MASK);
cr |= fs;
+
+#if L3GD20_USE_SPI
+#if L3GD20_SHARED_SPI
+ spiAcquireBus(((L3GD20Driver *)ip)->config->spip);
+ spiStart(((L3GD20Driver *)ip)->config->spip,
+ ((L3GD20Driver *)ip)->config->spicfg);
+#endif /* L3GD20_SHARED_SPI */
+
l3gd20SPIWriteRegister(((L3GD20Driver *)ip)->config->spip,
L3GD20_AD_CTRL_REG4, 1, &cr);
#if L3GD20_SHARED_SPI
spiReleaseBus(((L3GD20Driver *)ip)->config->spip);
#endif /* L3GD20_SHARED_SPI */
+#endif /* L3GD20_USE_SPI */
/* Scaling sensitivity and bias. Re-calibration is suggested anyway. */
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
@@ -318,7 +338,7 @@ void l3gd20ObjectInit(L3GD20Driver *devp) {
devp->vmt_l3gd20 = &vmt_l3gd20;
devp->config = NULL;
for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
- devp->bias[i] = 0;
+ devp->bias[i] = 0.0f;
devp->state = L3GD20_STOP;
}
@@ -339,14 +359,7 @@ void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {
"l3gd20Start(), invalid state");
devp->config = config;
-
-#if L3GD20_USE_SPI
-#if L3GD20_SHARED_SPI
- spiAcquireBus((devp)->config->spip);
-#endif /* L3GD20_SHARED_SPI */
- spiStart((devp)->config->spip,
- (devp)->config->spicfg);
-
+
/* Control register 1 configuration block.*/
{
cr[0] = L3GD20_CTRL_REG1_XEN | L3GD20_CTRL_REG1_YEN |
@@ -390,7 +403,15 @@ void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {
}
}
#endif
- }
+ }
+
+#if L3GD20_USE_SPI
+#if L3GD20_SHARED_SPI
+ spiAcquireBus((devp)->config->spip);
+#endif /* L3GD20_SHARED_SPI */
+ spiStart((devp)->config->spip,
+ (devp)->config->spicfg);
+
l3gd20SPIWriteRegister(devp->config->spip, L3GD20_AD_CTRL_REG1,
5, cr);
#if L3GD20_SHARED_SPI
@@ -401,21 +422,40 @@ void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {
/* Storing sensitivity information according to full scale.*/
if(devp->config->fullscale == L3GD20_FS_250DPS) {
devp->fullscale = L3GD20_250DPS;
- for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
- devp->sensitivity[i] = L3GD20_SENS_250DPS;
+ for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
+ if (devp->config->sensitivity == NULL)
+ devp->sensitivity[i] = L3GD20_SENS_250DPS;
+ else
+ devp->sensitivity[i] = devp->config->sensitivity[i];
+ }
}
else if(devp->config->fullscale == L3GD20_FS_500DPS) {
devp->fullscale = L3GD20_500DPS;
- for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
- devp->sensitivity[i] = L3GD20_SENS_500DPS;
+ for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
+ if (devp->config->sensitivity == NULL)
+ devp->sensitivity[i] = L3GD20_SENS_500DPS;
+ else
+ devp->sensitivity[i] = devp->config->sensitivity[i];
+ }
}
else if(devp->config->fullscale == L3GD20_FS_2000DPS) {
devp->fullscale = L3GD20_2000DPS;
- for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
- devp->sensitivity[i] = L3GD20_SENS_2000DPS;
+ for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
+ if (devp->config->sensitivity == NULL)
+ devp->sensitivity[i] = L3GD20_SENS_2000DPS;
+ else
+ devp->sensitivity[i] = devp->config->sensitivity[i];
+ }
}
else
- osalDbgAssert(FALSE, "l3gd20Start(), full scale issue");
+ osalDbgAssert(FALSE, "l3gd20Start(), full scale issue");
+
+ if(devp->config->bias != NULL) {
+ for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
+ devp->bias[i] = devp->config->bias[i];
+ }
+ }
+
/* This is the Gyroscope transient recovery time.*/
osalThreadSleepMilliseconds(10);
@@ -435,24 +475,27 @@ void l3gd20Stop(L3GD20Driver *devp) {
osalDbgAssert((devp->state == L3GD20_STOP) || (devp->state == L3GD20_READY),
"l3gd20Stop(), invalid state");
-
-#if (L3GD20_USE_SPI)
- if (devp->state == L3GD20_STOP) {
+
+ if (devp->state == L3GD20_READY) {
+ /* Disabling all axes and enabling power down mode.*/
+ cr1 = 0;
+
+#if L3GD20_USE_SPI
#if L3GD20_SHARED_SPI
spiAcquireBus((devp)->config->spip);
spiStart((devp)->config->spip,
(devp)->config->spicfg);
#endif /* L3GD20_SHARED_SPI */
- /* Disabling all axes and enabling power down mode.*/
- cr1 = 0;
+
l3gd20SPIWriteRegister(devp->config->spip, L3GD20_AD_CTRL_REG1,
1, &cr1);
- spiStop((devp)->config->spip);
+ spiStop((devp)->config->spip);
+
#if L3GD20_SHARED_SPI
spiReleaseBus((devp)->config->spip);
-#endif /* L3GD20_SHARED_SPI */
- }
-#endif /* L3GD20_USE_SPI */
+#endif /* L3GD20_SHARED_SPI */
+#endif /* L3GD20_USE_SPI */
+ }
devp->state = L3GD20_STOP;
}
/** @} */
diff --git a/os/ex/ST/l3gd20.h b/os/ex/ST/l3gd20.h
index 7dfce1f82..d658aef0c 100644
--- a/os/ex/ST/l3gd20.h
+++ b/os/ex/ST/l3gd20.h
@@ -40,7 +40,7 @@
/**
* @brief L3GD20 driver version string.
*/
-#define EX_L3GD20_VERSION "1.0.3"
+#define EX_L3GD20_VERSION "1.0.4"
/**
* @brief L3GD20 driver version major number.
@@ -55,7 +55,7 @@
/**
* @brief L3GD20 driver version patch number.
*/
-#define EX_L3GD20_PATCH 3
+#define EX_L3GD20_PATCH 4
/** @} */
/**
@@ -429,12 +429,12 @@ typedef struct {
/**
* @brief L3GD20 initial sensitivity.
*/
- float sensitivity[L3GD20_NUMBER_OF_AXES];
+ float *sensitivity;
/**
* @brief L3GD20 initial bias.
*/
- float bias[L3GD20_NUMBER_OF_AXES];
- /**
+ float *bias;
+ /**
* @brief L3GD20 initial full scale value.
*/
l3gd20_fs_t fullscale;
diff --git a/testhal/STM32/STM32F3xx/SPI-L3GD20/Makefile b/testhal/STM32/STM32F3xx/SPI-L3GD20/Makefile
index 3679b008a..ba5a5012e 100644
--- a/testhal/STM32/STM32F3xx/SPI-L3GD20/Makefile
+++ b/testhal/STM32/STM32F3xx/SPI-L3GD20/Makefile
@@ -201,7 +201,7 @@ CPPWARN = -Wall -Wextra -Wundef
# List all user C define here, like -D_DEBUG=1
UDEFS = -DCHPRINTF_USE_FLOAT=1 -DSHELL_CMD_TEST_ENABLED=0 \
- -DL3GD20_USE_ADVANCED=0
+ -DL3GD20_USE_ADVANCED=0 -DL3GD20_SHARED_SPI=0
# Define ASM defines here
UADEFS =
diff --git a/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c b/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c
index fb21b0af8..2df0437b1 100644
--- a/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c
+++ b/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c
@@ -48,8 +48,8 @@ static const SPIConfig spicfg = {
static L3GD20Config l3gd20cfg = {
&SPID1, /* Pointer to SPI Driver.*/
&spicfg, /* Pointer to SPI Configuration.*/
- {0, 0, 0}, /* Use default sensitivity.*/
- {0, 0, 0}, /* Use default bias.*/
+ NULL, /* Use default sensitivity.*/
+ NULL, /* Use default bias.*/
L3GD20_FS_250DPS, /* Full scale value.*/
L3GD20_ODR_760HZ, /* Output data rate.*/
#if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
diff --git a/testhal/STM32/STM32F4xx/SPI-L3GD20/Makefile b/testhal/STM32/STM32F4xx/SPI-L3GD20/Makefile
index db92c1e2c..655a35ddb 100644
--- a/testhal/STM32/STM32F4xx/SPI-L3GD20/Makefile
+++ b/testhal/STM32/STM32F4xx/SPI-L3GD20/Makefile
@@ -201,7 +201,7 @@ CPPWARN = -Wall -Wextra -Wundef
# List all user C define here, like -D_DEBUG=1
UDEFS = -DCHPRINTF_USE_FLOAT=1 -DSHELL_CMD_TEST_ENABLED=0 \
- -DL3GD20_USE_ADVANCED=0
+ -DL3GD20_USE_ADVANCED=0 -DL3GD20_SHARED_SPI=0
# Define ASM defines here
UADEFS =
diff --git a/testhal/STM32/STM32F4xx/SPI-L3GD20/main.c b/testhal/STM32/STM32F4xx/SPI-L3GD20/main.c
index ae30c6c98..2019b99a3 100644
--- a/testhal/STM32/STM32F4xx/SPI-L3GD20/main.c
+++ b/testhal/STM32/STM32F4xx/SPI-L3GD20/main.c
@@ -48,8 +48,8 @@ static const SPIConfig spicfg = {
static L3GD20Config l3gd20cfg = {
&SPID1, /* Pointer to SPI Driver.*/
&spicfg, /* Pointer to SPI Configuration.*/
- {0, 0, 0}, /* Use default sensitivity.*/
- {0, 0, 0}, /* Use default bias.*/
+ NULL, /* Use default sensitivity.*/
+ NULL, /* Use default bias.*/
L3GD20_FS_250DPS, /* Full scale value.*/
L3GD20_ODR_760HZ, /* Output data rate.*/
#if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
diff --git a/testhal/STM32/STM32L4xx/SPI-L3GD20/Makefile b/testhal/STM32/STM32L4xx/SPI-L3GD20/Makefile
index 4f6779d7b..aef87d496 100644
--- a/testhal/STM32/STM32L4xx/SPI-L3GD20/Makefile
+++ b/testhal/STM32/STM32L4xx/SPI-L3GD20/Makefile
@@ -5,7 +5,7 @@
# Compiler options here.
ifeq ($(USE_OPT),)
- USE_OPT = -O2 -ggdb -fomit-frame-pointer -falign-functions=16
+ USE_OPT = -O0 -ggdb -fomit-frame-pointer -falign-functions=16
endif
# C specific options here (added to USE_OPT).
@@ -201,7 +201,7 @@ CPPWARN = -Wall -Wextra -Wundef
# List all user C define here, like -D_DEBUG=1
UDEFS = -DCHPRINTF_USE_FLOAT=1 -DSHELL_CMD_TEST_ENABLED=0 \
- -DL3GD20_USE_ADVANCED=0
+ -DL3GD20_USE_ADVANCED=0 -DL3GD20_SHARED_SPI=0
# Define ASM defines here
UADEFS =
diff --git a/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c b/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c
index 644a155f6..ef9367137 100644
--- a/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c
+++ b/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c
@@ -47,8 +47,8 @@ static const SPIConfig spicfg = {
static L3GD20Config l3gd20cfg = {
&SPID2, /* Pointer to SPI Driver.*/
&spicfg, /* Pointer to SPI Configuration.*/
- {0, 0, 0}, /* Use default sensitivity.*/
- {0, 0, 0}, /* Use default bias.*/
+ NULL, /* Use default sensitivity.*/
+ NULL, /* Use default bias.*/
L3GD20_FS_250DPS, /* Full scale value.*/
L3GD20_ODR_760HZ, /* Output data rate.*/
#if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-01 19:09:57 +0000