gyro: Calculate angle

1 files changed, 51 insertions, 10 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;
 }