From 4061297c2a41081c374585a6f256a51f1b799058 Mon Sep 17 00:00:00 2001 From: Rocco Marco Guglielmi Date: Fri, 22 Apr 2016 21:32:32 +0000 Subject: EX: added get temperature for L3GD20, improved related demos git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9341 35acf78f-673a-0410-8e92-d51de3d6d3f4 --- os/ex/ST/l3gd20.c | 26 ++++++++++---------- os/ex/ST/l3gd20.h | 30 +++++++++++++++++++---- os/hal/lib/peripherals/sensors/hal_gyroscope.h | 34 +------------------------- testhal/STM32/STM32F3xx/SPI-L3GD20/main.c | 11 +++++++-- testhal/STM32/STM32L4xx/SPI-L3GD20/main.c | 11 +++++++-- 5 files changed, 57 insertions(+), 55 deletions(-) diff --git a/os/ex/ST/l3gd20.c b/os/ex/ST/l3gd20.c index 2fa329a76..2b4e2cc22 100644 --- a/os/ex/ST/l3gd20.c +++ b/os/ex/ST/l3gd20.c @@ -336,16 +336,12 @@ static msg_t reset_sensivity(void *ip) { return MSG_OK; } -static msg_t enable_temperature_compensation(void *ip) { - (void) ip; - /* TODO complete this function */ - return 0; -} +static msg_t get_temperature(void *ip, float* tempp) { + + *tempp = (int8_t)l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip, + L3GD20_AD_OUT_TEMP); -static msg_t disable_temperature_compensation(void *ip) { - (void) ip; - /* TODO complete this function */ - return 0; + return MSG_OK; } static const struct BaseSensorVMT vmt_basesensor = { @@ -355,9 +351,13 @@ static const struct BaseSensorVMT vmt_basesensor = { static const struct BaseGyroscopeVMT vmt_basegyroscope = { get_axes_number, read_raw, read_cooked, sample_bias, set_bias, reset_bias, - set_sensivity, reset_sensivity, - enable_temperature_compensation, - disable_temperature_compensation + set_sensivity, reset_sensivity +}; + +static const struct L3GD20VMT vmt_l3gd20 = { + get_axes_number, read_raw, read_cooked, + sample_bias, set_bias, reset_bias, + set_sensivity, reset_sensivity, get_temperature }; /*===========================================================================*/ @@ -376,7 +376,7 @@ void l3gd20ObjectInit(L3GD20Driver *devp) { devp->vmt_basesensor = &vmt_basesensor; devp->vmt_basegyroscope = &vmt_basegyroscope; - devp->vmt = (struct L3GD20VMT*) &vmt_basegyroscope; + devp->vmt_l3gd20 = &vmt_l3gd20; devp->state = L3GD20_STOP; devp->config = NULL; for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) diff --git a/os/ex/ST/l3gd20.h b/os/ex/ST/l3gd20.h index e13dafba5..ddd21a543 100644 --- a/os/ex/ST/l3gd20.h +++ b/os/ex/ST/l3gd20.h @@ -245,7 +245,10 @@ typedef struct L3GD20Driver L3GD20Driver; * @brief @p L3GD20 specific methods. */ #define _l3gd20_methods \ - _base_gyroscope_methods + _base_gyroscope_methods \ + /* Retrieve the temperature of L3GD20 chip.*/ \ + msg_t (*get_temperature)(void *instance, float* temperature); + /** * @extends BaseGyroscopeVMT @@ -278,12 +281,14 @@ struct L3GD20VMT { * driver implementation. */ struct L3GD20Driver { - /** @brief Virtual Methods Table.*/ + /** @brief Base Sensor Virtual Methods Table.*/ const struct BaseSensorVMT *vmt_basesensor; - /** @brief Virtual Methods Table.*/ + /** @brief Base Gyroscope Virtual Methods Table. + * @note Extend BaseSensor VMT.*/ const struct BaseGyroscopeVMT *vmt_basegyroscope; - /** @brief Virtual Methods Table.*/ - const struct L3GD20VMT *vmt; + /** @brief L3GD20 Virtual Methods Table. + * @note Extend BaseGyroscope VMT.*/ + const struct L3GD20VMT *vmt_l3gd20; _l3gd20_data }; /** @} */ @@ -292,6 +297,21 @@ struct L3GD20Driver { /* Driver macros. */ /*===========================================================================*/ +/** + * @brief Get current MEMS temperature. + * @detail This information is very useful especially for high accuracy IMU + * + * @param[in] ip pointer to a @p BaseGyroscope class. + * @param[out] temp the MEMS temperature as single precision floating. + * + * @return The operation status. + * @retval MSG_OK if the function succeeded. + * @retval MSG_RESET if one or more errors occurred. + * @api + */ +#define gyroscopeGetTemp(ip, tpp) \ + (ip)->vmt_l3gd20->get_temperature(ip, tpp) + /*===========================================================================*/ /* External declarations. */ /*===========================================================================*/ diff --git a/os/hal/lib/peripherals/sensors/hal_gyroscope.h b/os/hal/lib/peripherals/sensors/hal_gyroscope.h index 9ffb1b9f5..1fb36e8d8 100644 --- a/os/hal/lib/peripherals/sensors/hal_gyroscope.h +++ b/os/hal/lib/peripherals/sensors/hal_gyroscope.h @@ -56,11 +56,7 @@ /* Invoke the set sensitivity procedure.*/ \ msg_t (*set_sensitivity)(void *instance, float sensitivities[]); \ /* Restore sensitivity stored data to default.*/ \ - msg_t (*reset_sensitivity)(void *instance); \ - /* Enable temperature drift effect compensation.*/ \ - msg_t (*enable_temperature_compensation)(void *instance); \ - /* Disable temperature drift effect compensation.*/ \ - msg_t (*disable_temperature_compensation)(void *instance); + msg_t (*reset_sensitivity)(void *instance); /** @@ -221,34 +217,6 @@ typedef struct { */ #define gyroscopeResetSensitivity(ip) \ (ip)->vmt_basegyroscope->reset_sensitivity(ip) - -/** - * @brief Enables data compensation removing temperature drift. - * - * @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 gyroscopeEnableTempCompensation(ip) \ - (ip)->vmt_basegyroscope->enable_temperature_compensation(ip) - -/** - * @brief Disable data compensation. - * - * @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 gyroscopeDisableTempCompensation(ip) \ - (ip)->vmt_basegyroscope->disable_temperature_compensation(ip) /** @} */ /*===========================================================================*/ diff --git a/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c b/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c index 31ec3004f..a845f92a7 100644 --- a/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c +++ b/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c @@ -38,6 +38,7 @@ static L3GD20Driver L3GD20D1; static int32_t rawdata[L3GD20_NUMBER_OF_AXES]; static float cookeddata[L3GD20_NUMBER_OF_AXES]; +static float temperature; static char axesID[L3GD20_NUMBER_OF_AXES] = {'X', 'Y', 'Z'}; static uint32_t i; @@ -136,7 +137,10 @@ int main(void) { gyroscopeReadCooked(&L3GD20D1, cookeddata); for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) - chprintf(chp, "COOKED-%c:%f\r\n", axesID[i], cookeddata[i]); + chprintf(chp, "COOKED-%c:%.3f\r\n", axesID[i], cookeddata[i]); + + gyroscopeGetTemp(&L3GD20D1, &temperature); + chprintf(chp, "TEMP:%.1f C°\r\n", temperature); chprintf(chp, "Press Button to continue...\r\n"); chThdSleepMilliseconds(150); @@ -167,7 +171,10 @@ int main(void) { gyroscopeReadCooked(&L3GD20D1, cookeddata); for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) - chprintf(chp, "COOKED-%c:%f\r\n", axesID[i], cookeddata[i]); + chprintf(chp, "COOKED-%c:%.3f\r\n", axesID[i], cookeddata[i]); + + gyroscopeGetTemp(&L3GD20D1, &temperature); + chprintf(chp, "TEMP:%.1f C°\r\n", temperature); chThdSleepMilliseconds(150); #if CHPRINTF_USE_ANSI_CODE diff --git a/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c b/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c index 7bd21d8cb..94850da3b 100644 --- a/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c +++ b/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c @@ -30,6 +30,7 @@ static L3GD20Driver L3GD20D1; static int32_t rawdata[L3GD20_NUMBER_OF_AXES]; static float cookeddata[L3GD20_NUMBER_OF_AXES]; +static float temperature; static char axesID[L3GD20_NUMBER_OF_AXES] = {'X', 'Y', 'Z'}; static uint32_t i; @@ -109,7 +110,10 @@ int main(void) { gyroscopeReadCooked(&L3GD20D1, cookeddata); for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) - chprintf(chp, "COOKED-%c:%f\r\n", axesID[i], cookeddata[i]); + chprintf(chp, "COOKED-%c:%.3f\r\n", axesID[i], cookeddata[i]); + + gyroscopeGetTemp(&L3GD20D1, &temperature); + chprintf(chp, "TEMP:%.1f C°\r\n", temperature); chprintf(chp, "Press JOY to continue...\r\n"); chThdSleepMilliseconds(150); @@ -140,7 +144,10 @@ int main(void) { gyroscopeReadCooked(&L3GD20D1, cookeddata); for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) - chprintf(chp, "COOKED-%c:%f\r\n", axesID[i], cookeddata[i]); + chprintf(chp, "COOKED-%c:%.3f\r\n", axesID[i], cookeddata[i]); + + gyroscopeGetTemp(&L3GD20D1, &temperature); + chprintf(chp, "TEMP:%.1f C°\r\n", temperature); chThdSleepMilliseconds(150); #if CHPRINTF_USE_ANSI_CODE -- cgit v1.2.3