diff --git a/examples/zolertia/zoul/test-grove-gyro.c b/examples/zolertia/zoul/test-grove-gyro.c index 012188e31..24baa360f 100644 --- a/examples/zolertia/zoul/test-grove-gyro.c +++ b/examples/zolertia/zoul/test-grove-gyro.c @@ -108,6 +108,11 @@ PROCESS_THREAD(remote_grove_gyro_process, ev, data) */ grove_gyro.configure(GROVE_GYRO_POWER_ON, GROVE_GYRO_XYZ); + /* Calibrate the sensor taking 200 samples every 5ms for the zero-point offset + * value, this has to be done manually and is up to the user + */ + grove_gyro.configure(GROVE_GYRO_CALIBRATE_ZERO, 1); + /* Enabling the data interrupt will feed data directly to the extern data * structure and notify us about it, depending on the sampling rate and * divisor this could generate many interrupts/second. A zero argument would diff --git a/platform/zoul/dev/grove-gyro.c b/platform/zoul/dev/grove-gyro.c index 7f6723d49..56f79d9b3 100644 --- a/platform/zoul/dev/grove-gyro.c +++ b/platform/zoul/dev/grove-gyro.c @@ -45,6 +45,7 @@ #include "dev/i2c.h" #include "dev/grove-gyro.h" #include "lib/sensors.h" +#include "dev/watchdog.h" /*---------------------------------------------------------------------------*/ #define DEBUG 0 #if DEBUG @@ -58,6 +59,7 @@ /*---------------------------------------------------------------------------*/ static uint8_t enabled; static uint8_t power_mgmt; +static uint8_t int_en; /*---------------------------------------------------------------------------*/ grove_gyro_values_t gyro_values; /*---------------------------------------------------------------------------*/ @@ -347,6 +349,89 @@ grove_gyro_read(int type) return GROVE_GYRO_ERROR; } /*---------------------------------------------------------------------------*/ +static int +grove_gyro_calibrate(void) +{ + uint8_t i; + uint8_t buf[GROVE_GYRO_MAX_DATA]; + uint8_t power_mgmt_backup; + uint32_t x, y, z; + + /* Disable interrupts */ + if(int_en) { + if(grove_gyro_interrupt(GROVE_GYRO_INT_CFG_DISABLE) == GROVE_GYRO_ERROR) { + PRINTF("Gyro: failed to disable the interrupts\n"); + return GROVE_GYRO_ERROR; + } + GPIO_DISABLE_INTERRUPT(GROVE_GYRO_INT_PORT_BASE, GROVE_GYRO_INT_PIN_MASK); + } + + /* Turn on the 3-axis, save the current config */ + if(grove_gyro_read_reg(GROVE_GYRO_PWR_MGMT, &power_mgmt_backup, 1) == + GROVE_GYRO_ERROR) { + PRINTF("Gyro: failed to read power mgmt config\n"); + return GROVE_GYRO_ERROR; + } + + if(grove_gyro_power_mgmt(GROVE_GYRO_ALL, GROVE_GYRO_POWER_ON) == + GROVE_GYRO_ERROR) { + PRINTF("Gyro: failed to bring sensor up\n"); + return GROVE_GYRO_ERROR; + } + + x = 0; + y = 0; + z = 0; + + for (i = 0; i < GROVE_GYRO_CALIB_SAMPLES; i++){ + clock_delay_usec(GROVE_GYRO_CALIB_TIME_US); + watchdog_periodic(); + if(grove_gyro_read_reg(GROVE_GYRO_XOUT_H, buf, GROVE_GYRO_MAX_DATA) == + GROVE_GYRO_SUCCESS) { + x += (buf[0] << 8) + buf[1]; + y += (buf[2] << 8) + buf[3]; + z += (buf[4] << 8) + buf[5]; + } + } + + gyro_values.x_offset = ABS(x)/GROVE_GYRO_CALIB_SAMPLES; + gyro_values.y_offset = ABS(y)/GROVE_GYRO_CALIB_SAMPLES; + gyro_values.z_offset = ABS(z)/GROVE_GYRO_CALIB_SAMPLES; + + PRINTF("Gyro: x_offset (RAW) 0x%02X\n", gyro_values.x_offset); + PRINTF("Gyro: y_offset (RAW) 0x%02X\n", gyro_values.y_offset); + PRINTF("Gyro: z_offset (RAW) 0x%02X\n", gyro_values.z_offset); + + gyro_values.x_offset = grove_gyro_convert_to_value(gyro_values.x_offset); + gyro_values.y_offset = grove_gyro_convert_to_value(gyro_values.y_offset); + gyro_values.z_offset = grove_gyro_convert_to_value(gyro_values.z_offset); + + PRINTF("Gyro: x_offset (converted) %d\n", gyro_values.x_offset); + PRINTF("Gyro: y_offset (converted) %d\n", gyro_values.y_offset); + PRINTF("Gyro: z_offset (converted) %d\n", gyro_values.z_offset); + + /* Cleaning up */ + buf[0] = GROVE_GYRO_PWR_MGMT; + buf[1] = power_mgmt_backup; + + if(grove_gyro_write_reg(&buf[0], 2) != GROVE_GYRO_SUCCESS) { + PRINTF("Gyro: failed restoring power mgmt (0x%02X)\n", power_mgmt_backup); + return GROVE_GYRO_ERROR; + } + + if(int_en) { + if(grove_gyro_interrupt(GROVE_GYRO_INT_CFG_RAW_READY_EN + + GROVE_GYRO_INT_CFG_LATCH_EN) == GROVE_GYRO_ERROR) { + PRINTF("Gyro: failed to enable the interrupt\n"); + return GROVE_GYRO_ERROR; + } + + GPIO_ENABLE_INTERRUPT(GROVE_GYRO_INT_PORT_BASE, GROVE_GYRO_INT_PIN_MASK); + } + + return GROVE_GYRO_SUCCESS; +} +/*---------------------------------------------------------------------------*/ PROCESS(grove_gyro_int_process, "Grove gyroscope interrupt process handler"); /*---------------------------------------------------------------------------*/ PROCESS_THREAD(grove_gyro_int_process, ev, data) @@ -407,7 +492,8 @@ configure(int type, int value) { if((type != GROVE_GYRO_ACTIVE) && (type != GROVE_GYRO_SAMPLE_RATE) && (type != GROVE_GYRO_SAMPLE_RATE_DIVIDER) && (type != GROVE_GYRO_POWER_ON) && - (type != GROVE_GYRO_POWER_OFF) && (type != GROVE_GYRO_DATA_INTERRUPT)) { + (type != GROVE_GYRO_POWER_OFF) && (type != GROVE_GYRO_DATA_INTERRUPT) && + (type != GROVE_GYRO_CALIBRATE_ZERO)) { PRINTF("Gyro: option not supported\n"); return GROVE_GYRO_ERROR; } @@ -418,6 +504,15 @@ configure(int type, int value) i2c_init(I2C_SDA_PORT, I2C_SDA_PIN, I2C_SCL_PORT, I2C_SCL_PIN, I2C_SCL_FAST_BUS_SPEED); + /* Initialize the data structure values */ + gyro_values.x = 0; + gyro_values.y = 0; + gyro_values.z = 0; + gyro_values.temp = 0; + gyro_values.x_offset = 0; + gyro_values.y_offset = 0; + gyro_values.z_offset = 0; + /* Make sure the sensor is on */ if(grove_gyro_power_mgmt(GROVE_GYRO_ALL, GROVE_GYRO_POWER_ON) != GROVE_GYRO_SUCCESS) { @@ -456,6 +551,7 @@ configure(int type, int value) } else { enabled = 0; + int_en = 0; GPIO_DISABLE_INTERRUPT(GROVE_GYRO_INT_PORT_BASE, GROVE_GYRO_INT_PIN_MASK); grove_gyro_int_callback = NULL; if(grove_gyro_interrupt(GROVE_GYRO_INT_CFG_DISABLE) == @@ -479,6 +575,7 @@ configure(int type, int value) * I2C digital sensors using the bus and sharing this pin, so an user may * comment the line below */ + int_en = 0; GPIO_DISABLE_INTERRUPT(GROVE_GYRO_INT_PORT_BASE, GROVE_GYRO_INT_PIN_MASK); return grove_gyro_interrupt(GROVE_GYRO_INT_CFG_DISABLE); } @@ -503,6 +600,7 @@ configure(int type, int value) process_start(&grove_gyro_int_process, NULL); /* Enable interrupts */ + int_en = 1; GPIO_ENABLE_INTERRUPT(GROVE_GYRO_INT_PORT_BASE, GROVE_GYRO_INT_PIN_MASK); ioc_set_over(I2C_INT_PORT, I2C_INT_PIN, IOC_OVERRIDE_PUE); nvic_interrupt_enable(I2C_INT_VECTOR); @@ -534,6 +632,9 @@ configure(int type, int value) } return grove_gyro_power_mgmt(value, type); + case GROVE_GYRO_CALIBRATE_ZERO: + return grove_gyro_calibrate(); + default: return GROVE_GYRO_ERROR; } diff --git a/platform/zoul/dev/grove-gyro.h b/platform/zoul/dev/grove-gyro.h index 56649a2f1..8ab84ba7e 100644 --- a/platform/zoul/dev/grove-gyro.h +++ b/platform/zoul/dev/grove-gyro.h @@ -64,6 +64,9 @@ typedef struct { uint16_t x; uint16_t y; uint16_t z; + uint16_t x_offset; + uint16_t y_offset; + uint16_t z_offset; int16_t temp; } grove_gyro_values_t; @@ -149,6 +152,7 @@ extern grove_gyro_values_t gyro_values; #define GROVE_GYRO_SAMPLE_RATE_DIVIDER 0x03 #define GROVE_GYRO_POWER_ON 0x04 #define GROVE_GYRO_POWER_OFF 0x05 +#define GROVE_GYRO_CALIBRATE_ZERO 0x06 /* Sensor value request type, match to the stand-by mask to check if enabled */ #define GROVE_GYRO_X GROVE_GYRO_PWR_MGMT_STBY_XG @@ -163,6 +167,10 @@ extern grove_gyro_values_t gyro_values; /* Return types */ #define GROVE_GYRO_ERROR (-1) #define GROVE_GYRO_SUCCESS 0x00 + +/* Calibration constants */ +#define GROVE_GYRO_CALIB_SAMPLES 200 +#define GROVE_GYRO_CALIB_TIME_US 5000 /** @} */ /* -------------------------------------------------------------------------- */ #define GROVE_GYRO_STRING "Grove 3-axis gyroscope Sensor"