From 8d094a2c6c9f85cda6649f59d19ce4c902338fa4 Mon Sep 17 00:00:00 2001 From: Lars-Dominik Braun Date: Tue, 20 May 2014 15:32:42 +0200 Subject: gyro/accel: disable x/y axis Currently unused. --- accel.c | 22 ++++++++++------------ accel.h | 2 +- gyro.c | 49 ++++++++++++++++++++++++------------------------- gyro.h | 4 ++-- 4 files changed, 37 insertions(+), 40 deletions(-) diff --git a/accel.c b/accel.c index 9e3bda4..2a73b40 100644 --- a/accel.c +++ b/accel.c @@ -16,6 +16,7 @@ #define LIS302DL_WHOAMI 0xf #define LIS302DL_CTRLREG1 0x20 #define LIS302DL_UNUSED1 0x28 +#define LIS302DL_OUTZ 0x2D /* value for 1g with +-2g max; measured */ #define ACCEL_1G_POS (55) @@ -28,8 +29,8 @@ /* 250ms for 100Hz data rate */ #define ACCEL_SHAKE_TIMEOUT (25) -/* 0, 2 and 4 are zero, as they contain the dummy register’s content */ -static volatile int8_t val[6] = {0, 0, 0, 0, 0, 0}; +/* z value */ +static volatile int8_t zval = 0; /* number of times shaken (i.e. peak level measured) */ static uint8_t shakeCount = 0; /* if max in one direction direction has been detected give it some time to @@ -66,11 +67,11 @@ void accelInit () { /* XXX: make nonblocking */ void accelStart () { /* configuration: - * disable power-down-mode + * disable power-down-mode, enable z-axis * defaults * low active, data ready interrupt on int2 */ - uint8_t data[] = {0b01000111, 0b0, 0b10100000}; + uint8_t data[] = {0b01000100, 0b0, 0b10100000}; if (!twRequest (TWM_WRITE, LIS302DL, LIS302DL_CTRLREG1, data, sizeof (data)/sizeof (*data))) { @@ -86,7 +87,6 @@ void accelStart () { * direction. called for every data set pulled. */ static void accelProcessShake () { - const int8_t zval = val[5]; /* detect shake if: * a) horizon is positive and accel z value is >= ACCEL_SHAKE_POS * b) horizon is negative and accel z value is >= ACCEL_SHAKE_POS offset by @@ -133,7 +133,6 @@ static void accelProcessShake () { * i.e. have we been turned upside down? */ static void accelProcessHorizon () { - const int8_t zval = val[5]; /* measuring approximately 1g */ if (zval > (ACCEL_1G_POS - ACCEL_1G_OFF) && zval < (ACCEL_1G_POS + ACCEL_1G_OFF) && @@ -168,10 +167,9 @@ bool accelProcess () { } } else { if (!((PINC >> PINC1) & 0x1) && twr.status != TWST_WAIT) { - /* new data available in device buffer and bus is free, we are - * reading the registers inbetween out_x/y/z and ignore them */ - if (!twRequest (TWM_READ, LIS302DL, LIS302DL_UNUSED1, - (uint8_t *) val, 6)) { + /* new data available in device buffer and bus is free */ + if (!twRequest (TWM_READ, LIS302DL, LIS302DL_OUTZ, + (uint8_t *) &zval, sizeof (zval))) { printf ("cannot start read\n"); } else { reading = true; @@ -182,8 +180,8 @@ bool accelProcess () { return false; } -volatile const int8_t *accelGet () { - return val; +const int8_t accelGetZ () { + return zval; } const uint8_t accelGetShakeCount () { diff --git a/accel.h b/accel.h index 6c1022d..3aba602 100644 --- a/accel.h +++ b/accel.h @@ -9,7 +9,7 @@ typedef enum {HORIZON_NONE, HORIZON_POS, HORIZON_NEG} horizon; void accelInit (); void accelStart (); bool accelProcess (); -volatile const int8_t *accelGet (); +const int8_t accelGetZ (); const uint8_t accelGetShakeCount (); const horizon accelGetHorizon (); diff --git a/gyro.c b/gyro.c index bc402e5..0333e86 100644 --- a/gyro.c +++ b/gyro.c @@ -17,11 +17,12 @@ #define L3GD20_CTRLREG1 0x20 #define L3GD20_CTRLREG3 0x22 #define L3GD20_CTRLREG4 0x23 +#define L3GD20_OUTZ 0x2c -/* raw values */ -static volatile int16_t val[3] = {0, 0, 0}; -/* accumulated values */ -static int32_t accum[3] = {0, 0, 0}; +/* raw z value */ +static volatile int16_t zval = 0; +/* accumulated z value */ +static int32_t zaccum = 0; /* calculated zticks */ static int8_t zticks = 0; /* currently reading from i2c */ @@ -46,12 +47,12 @@ void gyroInit () { /* XXX: make nonblocking */ void gyroStart () { /* configuration: - * disable power-down-mode + * disable power-down-mode, enable z * defaults * low-active (does not work?), push-pull, drdy on int2 * select 500dps */ - uint8_t data[] = {0b00001111, 0b0, 0b00101000, 0b00010000}; + uint8_t data[] = {0b00001100, 0b0, 0b00101000, 0b00010000}; if (!twRequest (TWM_WRITE, L3GD20, L3GD20_CTRLREG1, data, sizeof (data)/sizeof (*data))) { @@ -65,17 +66,16 @@ void gyroStart () { */ static void gyroProcessTicks () { const uint8_t shift = 14; - const int32_t zval = accum[2]; - if (zval > (1 << shift)) { - const uint32_t a = abs (zval); + if (zaccum > (1 << shift)) { + const uint32_t a = abs (zaccum); zticks += a >> shift; /* mask shift bits */ - accum[2] -= a & (~0x3ff); - } else if (zval < -(1 << shift)) { - const uint32_t a = abs (zval); + zaccum -= a & (~0x3ff); + } else if (zaccum < -(1 << shift)) { + const uint32_t a = abs (zaccum); zticks -= a >> shift; - accum[2] += a & (~0x3ff); + zaccum += a & (~0x3ff); } } @@ -85,11 +85,9 @@ bool gyroProcess () { if (reading) { if (twr.status == TWST_OK) { /* new data transfered, process it */ - for (uint8_t i = 0; i < sizeof (accum)/sizeof (*accum); i++) { - /* poor man's noise filter */ - if (abs (val[i]) > 64) { - accum[i] += val[i]; - } + /* poor man's noise filter */ + if (abs (zval) > 64) { + zaccum += zval; } gyroProcessTicks (); reading = false; @@ -101,7 +99,8 @@ bool gyroProcess () { } else { if (((PINB >> PINB1) & 0x1) && twr.status != TWST_WAIT) { /* new data available in device buffer and bus is free */ - if (!twRequest (TWM_READ, L3GD20, 0x28, (uint8_t *) val, 6)) { + if (!twRequest (TWM_READ, L3GD20, L3GD20_OUTZ, + (uint8_t *) &zval, sizeof (zval))) { printf ("cannot start read\n"); } else { reading = true; @@ -112,16 +111,16 @@ bool gyroProcess () { return false; } -const int32_t *gyroGetAccum () { - return accum; +const int32_t gyroGetZAccum () { + return zaccum; } -void gyroResetAccum () { - memset (accum, 0, sizeof (accum)); +void gyroResetZAccum () { + zaccum = 0; } -volatile const int16_t *gyroGetRaw () { - return val; +volatile const int16_t gyroGetZRaw () { + return zval; } const int8_t gyroGetZTicks () { diff --git a/gyro.h b/gyro.h index a3f62f7..c55d8dd 100644 --- a/gyro.h +++ b/gyro.h @@ -8,8 +8,8 @@ void gyroInit (); void gyroStart (); bool gyroProcess (); void gyroResetAccum (); -const int32_t *gyroGetAccum (); -volatile const int16_t *gyroGetRaw (); +const int32_t gyroGetZAccum (); +volatile const int16_t gyroGetZRaw (); const int8_t gyroGetZTicks (); #endif /* GYROSCOPE_H */ -- cgit v1.2.3