From 28add4f616dbaf06a1ca3234b81c68c4a299d056 Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Mon, 16 Sep 2019 13:10:09 -0700
Subject: [PATCH] altos: Integrate BMX160 into data code

Get conversion functions written and fix up a few warnings

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/drivers/ao_bmx160.c |  2 ++
 src/drivers/ao_bmx160.h | 16 ++++++++++++++++
 src/kernel/ao_data.h    | 23 +++++++++++++++++++++++
 3 files changed, 41 insertions(+)

diff --git a/src/drivers/ao_bmx160.c b/src/drivers/ao_bmx160.c
index 4c3c2a9f..e1af23a1 100644
--- a/src/drivers/ao_bmx160.c
+++ b/src/drivers/ao_bmx160.c
@@ -90,6 +90,7 @@ _ao_bmm150_reg_write(uint8_t addr, uint8_t data)
 	_ao_bmm150_wait_manual();
 }
 
+#if BMX160_TEST
 static uint8_t
 _ao_bmm150_reg_read(uint8_t addr)
 {
@@ -97,6 +98,7 @@ _ao_bmm150_reg_read(uint8_t addr)
 	_ao_bmm150_wait_manual();
 	return _ao_bmx160_reg_read(BMX160_DATA_0);
 }
+#endif
 
 static void
 _ao_bmx160_sample(struct ao_bmx160_sample *sample)
diff --git a/src/drivers/ao_bmx160.h b/src/drivers/ao_bmx160.h
index 265be995..2b7e7bd7 100644
--- a/src/drivers/ao_bmx160.h
+++ b/src/drivers/ao_bmx160.h
@@ -19,6 +19,8 @@
 #ifndef _AO_BMX160_H_
 #define _AO_BMX160_H_
 
+#include <math.h>
+
 struct ao_bmx160_sample {
 	int16_t		mag_x;
 	int16_t		mag_y;
@@ -250,4 +252,18 @@ ao_bmx160_init(void);
 #define BMM150_REPZ				0x52
 #define  BMM150_REPZ_VALUE(n)				((n) -1)
 
+#define BMX160_GYRO_FULLSCALE	((float) 2000 * M_PI/180.0)
+
+static inline float
+ao_bmx160_gyro(float sensor) {
+	return sensor * ((float) (BMX160_GYRO_FULLSCALE / 32767.0));
+}
+
+#define BMX160_ACCEL_FULLSCALE	16
+
+static inline float
+ao_bmx160_accel(int16_t sensor) {
+	return (float) sensor * ((float) (BMX160_ACCEL_FULLSCALE * GRAVITY / 32767.0));
+}
+
 #endif /* _BMX160_H_ */
diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h
index 55d82e48..a7e58762 100644
--- a/src/kernel/ao_data.h
+++ b/src/kernel/ao_data.h
@@ -441,6 +441,29 @@ static inline float ao_convert_accel(int16_t sensor)
 
 #endif
 
+#if !HAS_GYRO && HAS_BMX160
+
+#define HAS_GYRO	1
+
+typedef int16_t	gyro_t;		/* in raw sample units */
+typedef int16_t angle_t;	/* in degrees */
+
+/* Y axis is aligned with the direction of motion (along) */
+/* X axis is aligned in the other board axis (across) */
+/* Z axis is aligned perpendicular to the board (through) */
+
+static inline float ao_convert_gyro(float sensor)
+{
+	return ao_bmx160_gyro(sensor);
+}
+
+static inline float ao_convert_accel(int16_t sensor)
+{
+	return ao_bmx160_accel(sensor);
+}
+
+#endif
+
 #if !HAS_MAG && HAS_HMC5883
 
 #define HAS_MAG		1
-- 
2.30.2