diff options
author | Lars-Dominik Braun <lars@6xq.net> | 2014-04-29 15:29:28 +0200 |
---|---|---|
committer | Lars-Dominik Braun <lars@6xq.net> | 2014-04-29 15:29:28 +0200 |
commit | 6dc875e136b4ab418cf63154bdd2ffea46c6e67a (patch) | |
tree | 12604ab652ca22a7d1f8f864b8cfb50a05443f58 | |
parent | b05df9df58bc6fdd122abfec3012bc2537fc069c (diff) | |
download | hourglass-6dc875e136b4ab418cf63154bdd2ffea46c6e67a.tar.gz hourglass-6dc875e136b4ab418cf63154bdd2ffea46c6e67a.tar.bz2 hourglass-6dc875e136b4ab418cf63154bdd2ffea46c6e67a.zip |
gyro: Replace (broken) angle with ticks
-rw-r--r-- | gyro.c | 57 | ||||
-rw-r--r-- | gyro.h | 5 | ||||
-rw-r--r-- | main.c | 12 |
3 files changed, 49 insertions, 25 deletions
@@ -1,6 +1,7 @@ #include "common.h" #include <stdio.h> +#include <stdlib.h> #include <string.h> #include <avr/io.h> #include <avr/interrupt.h> @@ -9,18 +10,6 @@ #include "i2c.h" #include "gyro.h" -/* max degree +/- per second */ -#define GYRO_DPS 500 -/* output data rate, hertz */ -#define GYRO_FREQ 95 -/* millidegree per second and digit, (2*GYRO_DPS+1)/(2^16) ~= 15.274, manual - * says 17.5, so choose something in between */ -#define GYRO_MDPS_PER_DIGIT 16 -/* milliangle (in degree) per 32 units, 32 is a relatively small number (not - * much information lost) and the angle is very to a full integer number, - * GYRO_MDPS_PER_DIGIT/GYRO_FREQ*32 */ -#define GYRO_MANGLE_PER_32 5 - /* device address */ #define L3GD20 0b11010100 /* registers */ @@ -31,8 +20,10 @@ /* raw values */ static volatile int16_t val[3] = {0, 0, 0}; -/* current (relative) angle, in millidegree */ -static int16_t angle[3] = {0, 0, 0}; +/* accumulated values */ +static int32_t accum[3] = {0, 0, 0}; +/* calculated zticks */ +static int8_t zticks = 0; /* currently reading from i2c */ static bool reading = false; @@ -70,15 +61,37 @@ void gyroStart () { printf ("final twi status was %i\n", twr.status); } +/* calculate ticks for z rotation + */ +static void gyroProcessTicks () { + const uint8_t shift = 14; + const int32_t zval = accum[2]; + + if (zval > (1 << shift)) { + const uint32_t a = abs (zval); + zticks += a >> shift; + /* mask shift bits */ + accum[2] -= a & (~0x3ff); + } else if (zval < -(1 << shift)) { + const uint32_t a = abs (zval); + zticks -= a >> shift; + accum[2] += a & (~0x3ff); + } +} + /* process gyro sensor data, returns true if new data is available */ bool gyroProcess () { if (reading) { if (twr.status == TWST_OK) { /* new data transfered, process it */ - for (uint8_t i = 0; i < sizeof (angle)/sizeof (*angle); i++) { - angle[i] += (val[i] >> 5) * GYRO_MANGLE_PER_32; + 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]; + } } + gyroProcessTicks (); reading = false; return true; } else if (twr.status == TWST_ERR) { @@ -99,15 +112,19 @@ bool gyroProcess () { return false; } -const int16_t *gyroGetAngle () { - return angle; +const int32_t *gyroGetAccum () { + return accum; } -void gyroResetAngle () { - memset (angle, 0, sizeof (angle)); +void gyroResetAccum () { + memset (accum, 0, sizeof (accum)); } volatile const int16_t *gyroGetRaw () { return val; } +const int8_t gyroGetZTicks () { + return zticks; +} + @@ -7,9 +7,10 @@ void gyroInit (); void gyroStart (); bool gyroProcess (); -void gyroResetAngle (); -const int16_t *gyroGetAngle (); +void gyroResetAccum (); +const int32_t *gyroGetAccum (); volatile const int16_t *gyroGetRaw (); +const int8_t gyroGetZTicks (); #endif /* GYROSCOPE_H */ @@ -6,6 +6,7 @@ #include <util/delay.h> #include <stdio.h> #include <stdbool.h> +#include <stdlib.h> #include "i2c.h" #include "uart.h" @@ -74,11 +75,16 @@ int main () { sleep_cpu (); sleep_disable (); } - volatile const int16_t *gyroval = gyroGetAngle (); + printf ("ticks=%i\n", gyroGetZTicks ()); +#if 0 + volatile const int32_t *gyroval = gyroGetAccum (); + volatile const int16_t *gyroraw = gyroGetRaw (); volatile const int8_t *accelval = accelGet (); - printf ("%i/%i/%i - %i/%i/%i\n", gyroval[0], gyroval[1], gyroval[2], + printf ("%li/%li/%li - %i/%i/%i - %i/%i/%i\n", + gyroval[0], gyroval[1], gyroval[2], + gyroraw[0], gyroraw[1], gyroraw[2], accelval[1], accelval[3], accelval[5]); - gyroResetAngle (); +#endif } timerStop (); |