From 6dc875e136b4ab418cf63154bdd2ffea46c6e67a Mon Sep 17 00:00:00 2001
From: Lars-Dominik Braun <lars@6xq.net>
Date: Tue, 29 Apr 2014 15:29:28 +0200
Subject: gyro: Replace (broken) angle with ticks

---
 gyro.c | 57 +++++++++++++++++++++++++++++++++++++--------------------
 gyro.h |  5 +++--
 main.c | 12 +++++++++---
 3 files changed, 49 insertions(+), 25 deletions(-)

diff --git a/gyro.c b/gyro.c
index 406ae2e..bc402e5 100644
--- a/gyro.c
+++ b/gyro.c
@@ -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;
+}
+
diff --git a/gyro.h b/gyro.h
index 3a8ad10..a3f62f7 100644
--- a/gyro.h
+++ b/gyro.h
@@ -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 */
 
diff --git a/main.c b/main.c
index 4eb461e..a581ea8 100644
--- a/main.c
+++ b/main.c
@@ -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 ();
 
-- 
cgit v1.2.3