From 08143a922fe27bc50a19924f46538f9476ab5fd1 Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Fri, 25 Oct 2013 04:05:09 -0700
Subject: altos: Add gyro-based orientation tracking

This tracks the angle-from-vertical as an additional input to the pyro
channels.

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/drivers/ao_mpu6000.c |  2 ++
 src/drivers/ao_mpu6000.h | 17 ++++++++++++++++-
 2 files changed, 18 insertions(+), 1 deletion(-)

(limited to 'src/drivers')

diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index 73057820..5e75b78a 100644
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -118,6 +118,7 @@ _ao_mpu6000_sample(struct ao_mpu6000_sample *sample)
 
 #define G	981	/* in cm/s² */
 
+#if 0
 static int16_t /* cm/s² */
 ao_mpu6000_accel(int16_t v)
 {
@@ -129,6 +130,7 @@ ao_mpu6000_gyro(int16_t v)
 {
 	return (int16_t) ((v * (int32_t) 20000) / 32767);
 }
+#endif
 
 static uint8_t
 ao_mpu6000_accel_check(int16_t normal, int16_t test, char *which)
diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h
index a42f69c4..2241bf80 100644
--- a/src/drivers/ao_mpu6000.h
+++ b/src/drivers/ao_mpu6000.h
@@ -18,6 +18,10 @@
 #ifndef _AO_MPU6000_H_
 #define _AO_MPU6000_H_
 
+#ifndef M_PI
+#define M_PI 3.1415926535897832384626433
+#endif
+
 #define MPU6000_ADDR_WRITE	0xd0
 #define MPU6000_ADDR_READ	0xd1
 
@@ -166,9 +170,20 @@
 /* Self test gyro is approximately 50°/s */
 #define MPU6000_ST_GYRO(full_scale)	((int16_t) (((int32_t) 32767 * (int32_t) 50) / (full_scale)))
 
-#define MPU6000_GYRO_FULLSCALE	2000
+#define MPU6000_GYRO_FULLSCALE	((float) 2000 * M_PI/180.0)
+
+static inline float
+ao_mpu6000_gyro(int16_t sensor) {
+	return (float) sensor * ((float) (MPU6000_GYRO_FULLSCALE / 32767.0));
+}
+
 #define MPU6000_ACCEL_FULLSCALE	16
 
+static inline float
+ao_mpu6000_accel(int16_t sensor) {
+	return (float) sensor * ((float) (MPU6000_ACCEL_FULLSCALE * GRAVITY / 32767.0));
+}
+
 struct ao_mpu6000_sample {
 	int16_t		accel_x;
 	int16_t		accel_y;
-- 
cgit v1.2.3