From 94bbc4d19ca17984db3262089ba7365fbf62c6de Mon Sep 17 00:00:00 2001 From: Rocco Marco Guglielmi Date: Wed, 16 Mar 2016 11:45:23 +0000 Subject: [PATCH] Improved Gyroscope methods. Updated L3GD20 files and demos. git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9126 35acf78f-673a-0410-8e92-d51de3d6d3f4 --- os/ex/ST/l3gd20.c | 45 ++++++++++--------- os/ex/ST/l3gd20.h | 2 +- .../lib/peripherals/sensors/hal_gyroscope.h | 43 +++++++++--------- testhal/STM32/STM32F3xx/SPI-L3GD20/.project | 38 ++++++++++++++++ testhal/STM32/STM32F3xx/SPI-L3GD20/main.c | 4 +- testhal/STM32/STM32L4xx/SPI-L3GD20/main.c | 4 +- 6 files changed, 90 insertions(+), 46 deletions(-) create mode 100644 testhal/STM32/STM32F3xx/SPI-L3GD20/.project diff --git a/os/ex/ST/l3gd20.c b/os/ex/ST/l3gd20.c index 4c6965875..63b902058 100644 --- a/os/ex/ST/l3gd20.c +++ b/os/ex/ST/l3gd20.c @@ -244,26 +244,12 @@ 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; + axes[i] = raw[i] * ((L3GD20Driver *)ip)->sensitivity[i]; } return msg; } -static msg_t reset_calibration(void *ip) { - uint32_t i; - - osalDbgCheck(ip != NULL); - - osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY) || - (((L3GD20Driver *)ip)->state == L3GD20_STOP), - "reset_calibration(), invalid state"); - - for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) - ((L3GD20Driver *)ip)->bias[i] = 0; - return MSG_OK; -} - -static msg_t calibrate(void *ip) { +static msg_t sample_bias(void *ip) { uint32_t i, j; int32_t raw[L3GD20_NUMBER_OF_AXES]; int32_t buff[L3GD20_NUMBER_OF_AXES] = {0, 0, 0}; @@ -287,13 +273,27 @@ static msg_t calibrate(void *ip) { return MSG_OK; } +static msg_t reset_bias(void *ip) { + uint32_t i; + + osalDbgCheck(ip != NULL); + + osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY) || + (((L3GD20Driver *)ip)->state == L3GD20_STOP), + "reset_calibration(), invalid state"); + + for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) + ((L3GD20Driver *)ip)->bias[i] = 0; + return MSG_OK; +} + static const struct BaseSensorVMT vmt_basesensor = { get_axes_number, read_raw, read_cooked }; static const struct BaseGyroscopeVMT vmt_basegyroscope = { get_axes_number, read_raw, read_cooked, - reset_calibration, calibrate + sample_bias, reset_bias }; @@ -329,7 +329,7 @@ void l3gd20ObjectInit(L3GD20Driver *devp) { * @api */ void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) { - + uint8_t i; osalDbgCheck((devp != NULL) && (config != NULL)); osalDbgAssert((devp->state == L3GD20_STOP) || (devp->state == L3GD20_READY), @@ -358,11 +358,14 @@ void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) { /* Storing sensitivity information according to full scale value */ if(devp->config->fullscale == L3GD20_FS_250DPS) - devp->sensitivity = L3GD20_SENS_250DPS; + for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) + devp->sensitivity[i] = L3GD20_SENS_250DPS; else if(devp->config->fullscale == L3GD20_FS_500DPS) - devp->sensitivity = L3GD20_SENS_500DPS; + for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) + devp->sensitivity[i] = L3GD20_SENS_500DPS; else if(devp->config->fullscale == L3GD20_FS_2000DPS) - devp->sensitivity = L3GD20_SENS_2000DPS; + for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) + devp->sensitivity[i] = L3GD20_SENS_2000DPS; else osalDbgAssert(FALSE, "l3gd20Start(), full scale issue"); /* This is the Gyroscope transient recovery time */ diff --git a/os/ex/ST/l3gd20.h b/os/ex/ST/l3gd20.h index 410e6e28a..e13dafba5 100644 --- a/os/ex/ST/l3gd20.h +++ b/os/ex/ST/l3gd20.h @@ -266,7 +266,7 @@ struct L3GD20VMT { /* Current configuration data.*/ \ const L3GD20Config *config; \ /* Current sensitivity.*/ \ - float sensitivity; \ + float sensitivity[L3GD20_NUMBER_OF_AXES]; \ /* Bias data.*/ \ int32_t bias[L3GD20_NUMBER_OF_AXES]; diff --git a/os/hal/lib/peripherals/sensors/hal_gyroscope.h b/os/hal/lib/peripherals/sensors/hal_gyroscope.h index ac2901fe4..e61d06b0f 100644 --- a/os/hal/lib/peripherals/sensors/hal_gyroscope.h +++ b/os/hal/lib/peripherals/sensors/hal_gyroscope.h @@ -47,10 +47,10 @@ * @brief BaseGyroscope specific methods. */ #define _base_gyroscope_methods_alone \ - /* Remove the calibration data.*/ \ - msg_t (*reset_calibration)(void *instance); \ - /* Invokes the calibration procedure.*/ \ - msg_t (*calibrate)(void *instance); + /* Invoke the sample bias procedure.*/ \ + msg_t (*sample_bias)(void *instance); \ + /* Remove bias stored data.*/ \ + msg_t (*reset_bias)(void *instance); /** * @brief BaseGyroscope specific methods with inherited ones. @@ -132,7 +132,24 @@ typedef struct { (ip)->vmt_basegyroscope->read_cooked(ip, dp) /** - * @brief Delete calibration data. + * @brief Gyroscope bias sampling procedure. + * @note During this procedure gyroscope must be kept hold in the rest + * position. Sampled bias will be automatically removed after calling + * this procedure. + * + * @param[in] ip pointer to a @p BaseGyroscope class. + * + * @return The operation status. + * @retval MSG_OK if the function succeeded. + * @retval MSG_RESET if one or more errors occurred. + * + * @api + */ +#define gyroscopeSampleBias(ip) \ + (ip)->vmt_basegyroscope->sample_bias(ip) + +/** + * @brief Reset bias data restoring it to zero. * * @param[in] ip pointer to a @p BaseGyroscope class. * @@ -143,21 +160,7 @@ typedef struct { * @api */ #define gyroscopeResetCalibration(ip) \ - (ip)->vmt_basegyroscope->reset_calibration(ip) - -/** - * @brief Gyroscope calibration procedure. - * - * @param[in] ip pointer to a @p BaseGyroscope class. - * - * @return The operation status. - * @retval MSG_OK if the function succeeded. - * @retval MSG_RESET if one or more errors occurred. - * - * @api - */ -#define gyroscopeCalibrate(ip) \ - (ip)->vmt_basegyroscope->calibrate(ip) + (ip)->vmt_basegyroscope->reset_bias(ip) /** @} */ /*===========================================================================*/ diff --git a/testhal/STM32/STM32F3xx/SPI-L3GD20/.project b/testhal/STM32/STM32F3xx/SPI-L3GD20/.project new file mode 100644 index 000000000..80babdba1 --- /dev/null +++ b/testhal/STM32/STM32F3xx/SPI-L3GD20/.project @@ -0,0 +1,38 @@ + + + STM32F3xx-SPI-L3GD20 + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + + + + board + 2 + C:/ChibiStudio/chibios_trunk/os/hal/boards/ST_STM32F3_DISCOVERY + + + os + 2 + CHIBIOS/os + + + diff --git a/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c b/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c index d9925b041..ec198ec7a 100644 --- a/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c +++ b/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c @@ -146,14 +146,14 @@ int main(void) { } palClearLine(LINE_LED10_RED); - chprintf(chp, "Calibrating Gyroscope...\r\n"); + chprintf(chp, "Calibrating Gyroscope sampling bias...\r\n"); chprintf(chp, "Keep it in the rest position while red LED is on\r\n"); chThdSleepMilliseconds(3000); palSetLine(LINE_LED10_RED); chThdSleepMilliseconds(1000); - gyroscopeCalibrate(&L3GD20D1); + gyroscopeSampleBias(&L3GD20D1); palClearLine(LINE_LED10_RED); #if CHPRINTF_USE_ANSI_CODE chprintf(chp, "\033[2J\033[1;1H"); diff --git a/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c b/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c index b7a837c80..464f5d550 100644 --- a/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c +++ b/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c @@ -119,14 +119,14 @@ int main(void) { } palClearLine(LINE_LED_RED); - chprintf(chp, "Calibrating Gyroscope...\r\n"); + chprintf(chp, "Calibrating Gyroscope sampling bias...\r\n"); chprintf(chp, "Keep it in the rest position while red LED is on\r\n"); chThdSleepMilliseconds(3000); palSetLine(LINE_LED_RED); chThdSleepMilliseconds(1000); - gyroscopeCalibrate(&L3GD20D1); + gyroscopeSampleBias(&L3GD20D1); palClearLine(LINE_LED_RED); #if CHPRINTF_USE_ANSI_CODE chprintf(chp, "\033[2J\033[1;1H");