From d664b474657eff9988c14b40e5d050dc5571f778 Mon Sep 17 00:00:00 2001 From: Lars-Dominik Braun Date: Tue, 17 Jun 2014 15:29:50 +0200 Subject: ui: Added Seems to work mostly now. --- gyro.c | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) (limited to 'gyro.c') diff --git a/gyro.c b/gyro.c index b9cba41..6a32ef3 100644 --- a/gyro.c +++ b/gyro.c @@ -24,7 +24,7 @@ static volatile int16_t zval = 0; /* accumulated z value */ static int32_t zaccum = 0; /* calculated zticks */ -static int8_t zticks = 0; +static int16_t zticks = 0; /* currently reading from i2c */ static bool reading = false; @@ -66,16 +66,18 @@ void gyroStart () { */ static void gyroProcessTicks () { const uint8_t shift = 14; + const uint32_t max = (1 << shift); + const uint32_t mask = ~(max-1); - if (zaccum > (1 << shift)) { + if (zaccum > (int32_t) max) { const uint32_t a = abs (zaccum); zticks += a >> shift; /* mask shift bits */ - zaccum -= a & (~0x3ff); - } else if (zaccum < -(1 << shift)) { + zaccum -= a & mask; + } else if (zaccum < -((int32_t) max)) { const uint32_t a = abs (zaccum); zticks -= a >> shift; - zaccum += a & (~0x3ff); + zaccum += a & mask; } } @@ -123,7 +125,11 @@ int16_t gyroGetZRaw () { return zval; } -int8_t gyroGetZTicks () { +int16_t gyroGetZTicks () { return zticks; } +void gyroResetZTicks () { + zticks = 0; +} + -- cgit v1.2.3