From 8d094a2c6c9f85cda6649f59d19ce4c902338fa4 Mon Sep 17 00:00:00 2001
From: Lars-Dominik Braun <lars@6xq.net>
Date: Tue, 20 May 2014 15:32:42 +0200
Subject: gyro/accel: disable x/y axis

Currently unused.
---
 gyro.c | 49 ++++++++++++++++++++++++-------------------------
 1 file changed, 24 insertions(+), 25 deletions(-)

(limited to 'gyro.c')

diff --git a/gyro.c b/gyro.c
index bc402e5..0333e86 100644
--- a/gyro.c
+++ b/gyro.c
@@ -17,11 +17,12 @@
 #define L3GD20_CTRLREG1 0x20
 #define L3GD20_CTRLREG3 0x22
 #define L3GD20_CTRLREG4 0x23
+#define L3GD20_OUTZ 0x2c
 
-/* raw values */
-static volatile int16_t val[3] = {0, 0, 0};
-/* accumulated values */
-static int32_t accum[3] = {0, 0, 0};
+/* raw z value */
+static volatile int16_t zval = 0;
+/* accumulated z value */
+static int32_t zaccum = 0;
 /* calculated zticks */
 static int8_t zticks = 0;
 /* currently reading from i2c */
@@ -46,12 +47,12 @@ void gyroInit () {
 /* XXX: make nonblocking */
 void gyroStart () {
 	/* configuration:
-	 * disable power-down-mode
+	 * disable power-down-mode, enable z
 	 * defaults
 	 * low-active (does not work?), push-pull, drdy on int2
 	 * select 500dps
 	 */
-	uint8_t data[] = {0b00001111, 0b0, 0b00101000, 0b00010000};
+	uint8_t data[] = {0b00001100, 0b0, 0b00101000, 0b00010000};
 
 	if (!twRequest (TWM_WRITE, L3GD20, L3GD20_CTRLREG1, data,
 			sizeof (data)/sizeof (*data))) {
@@ -65,17 +66,16 @@ void gyroStart () {
  */
 static void gyroProcessTicks () {
 	const uint8_t shift = 14;
-	const int32_t zval = accum[2];
 
-	if (zval > (1 << shift)) {
-		const uint32_t a = abs (zval);
+	if (zaccum > (1 << shift)) {
+		const uint32_t a = abs (zaccum);
 		zticks += a >> shift;
 		/* mask shift bits */
-		accum[2] -= a & (~0x3ff);
-	} else if (zval < -(1 << shift)) {
-		const uint32_t a = abs (zval);
+		zaccum -= a & (~0x3ff);
+	} else if (zaccum < -(1 << shift)) {
+		const uint32_t a = abs (zaccum);
 		zticks -= a >> shift;
-		accum[2] += a & (~0x3ff);
+		zaccum += a & (~0x3ff);
 	}
 }
 
@@ -85,11 +85,9 @@ bool gyroProcess () {
 	if (reading) {
 		if (twr.status == TWST_OK) {
 			/* new data transfered, process it */
-			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];
-				}
+			/* poor man's noise filter */
+			if (abs (zval) > 64) {
+				zaccum += zval;
 			}
 			gyroProcessTicks ();
 			reading = false;
@@ -101,7 +99,8 @@ bool gyroProcess () {
 	} else {
 		if (((PINB >> PINB1) & 0x1) && twr.status != TWST_WAIT) {
 			/* new data available in device buffer and bus is free */
-			if (!twRequest (TWM_READ, L3GD20, 0x28, (uint8_t *) val, 6)) {
+			if (!twRequest (TWM_READ, L3GD20, L3GD20_OUTZ,
+					(uint8_t *) &zval, sizeof (zval))) {
 				printf ("cannot start read\n");
 			} else {
 				reading = true;
@@ -112,16 +111,16 @@ bool gyroProcess () {
 	return false;
 }
 
-const int32_t *gyroGetAccum () {
-	return accum;
+const int32_t gyroGetZAccum () {
+	return zaccum;
 }
 
-void gyroResetAccum () {
-	memset (accum, 0, sizeof (accum));
+void gyroResetZAccum () {
+	zaccum = 0;
 }
 
-volatile const int16_t *gyroGetRaw () {
-	return val;
+volatile const int16_t gyroGetZRaw () {
+	return zval;
 }
 
 const int8_t gyroGetZTicks () {
-- 
cgit v1.2.3