gyro: Calculate angle

3 files changed, 59 insertions, 27 deletions

diff --git a/gyro.c b/gyro.c
index f4d7529..a373d29 100644
--- a/gyro.c
+++ b/gyro.c
@@ -1,4 +1,5 @@
 #include <stdio.h>
+#include <string.h>
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <avr/sleep.h>
@@ -6,6 +7,18 @@
 #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 */
@@ -22,6 +35,10 @@
 /* the first interrupt is lost */
 volatile bool drdy = true;
 volatile int16_t val[3] = {0, 0, 0};
+/* current (relative) angle, in millidegree */
+int16_t angle[3] = {0, 0, 0};
+/* currently reading from i2c */
+bool reading = false;
 
 /* data ready interrupt
  */
@@ -57,21 +74,45 @@ void gyroStart () {
 	printf ("final twi status was %i\n", twr.status);
 }
 
-bool gyroRead () {
-	if (drdy) {
-		drdy = false;
-
-		if (!twRequest (TWM_READ, L3GD20, 0x28, (uint8_t *) val, 6)) {
-			printf ("cannot start read\n");
+/*	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;
+			}
+			reading = false;
+			return true;
+		} else if (twr.status == TWST_ERR) {
+			printf ("gyro i2c error\n");
+			reading = false;
 		}
-
-		return true;
 	} else {
-		return false;
+		if (drdy && twr.status != TWST_WAIT) {
+			/* new data available in device buffer and bus is free */
+			if (!twRequest (TWM_READ, L3GD20, 0x28, (uint8_t *) val, 6)) {
+				printf ("cannot start read\n");
+			} else {
+				drdy = false;
+				reading = true;
+			}
+		}
 	}
+
+	return false;
+}
+
+const int16_t *gyroGetAngle () {
+	return angle;
+}
+
+void gyroResetAngle () {
+	memset (angle, 0, sizeof (angle));
 }
 
-volatile const int16_t *gyroGet () {
+volatile const int16_t *gyroGetRaw () {
 	return val;
 }
 
diff --git a/gyro.h b/gyro.h
index ac3550e..3a8ad10 100644
--- a/gyro.h
+++ b/gyro.h
@@ -6,8 +6,10 @@
 
 void gyroInit ();
 void gyroStart ();
-bool gyroRead ();
-volatile const int16_t *gyroGet ();
+bool gyroProcess ();
+void gyroResetAngle ();
+const int16_t *gyroGetAngle ();
+volatile const int16_t *gyroGetRaw ();
 
 #endif /* GYROSCOPE_H */
 
diff --git a/main.c b/main.c
index f04c725..2e894d3 100644
--- a/main.c
+++ b/main.c
@@ -57,23 +57,12 @@ int main () {
 		//sleepwhile (!timerHit ());
 		//printf ("running for %u seconds\n", seconds);
 
-		sleepwhile (!gyroRead ());
-		sleepwhile (twr.status == TWST_WAIT);
-		switch (twr.status) {
-			case TWST_OK: {
-				volatile const int16_t *val = gyroGet ();
-				printf ("%i/%i/%i\n", val[0], val[1], val[2]);
-				break;
-			}
-
-			case TWST_ERR:
-				goto fail;
-				break;
-		}
+		sleepwhile (!gyroProcess());
+		volatile const int16_t *val = gyroGetAngle ();
+		printf ("%i/%i/%i\n", val[0], val[1], val[2]);
+		gyroResetAngle ();
 	}
 	//timerStop ();
-fail:
-	printf ("fail\n");
 
 	/* global interrupt disable */
 	cli ();