X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_mpu6000.h;h=dc3a9fbfa0e9f664d15f04726641ffd306edc104;hp=f01e9e83b81ce9ca52ff0b2f3e32a9d07af7175d;hb=135abf0e7c5ceb5738a0b5f68fe2be4b7abdae5e;hpb=bf88c5f829ea5d32043431945e862a9f6c96740a

diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h
index f01e9e83..dc3a9fbf 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
 
@@ -133,13 +137,13 @@
 #define MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET	1
 #define MPU6000_SIGNAL_PATH_RESET_TEMP_RESET	0
 
-#define MPU6000_USER_CONTROL		0x6a
-#define MPU6000_USER_CONTROL_FIFO_EN		6
-#define MPU6000_USER_CONTROL_I2C_MST_EN		5
-#define MPU6000_USER_CONTROL_I2C_IF_DIS		4
-#define MPU6000_USER_CONTROL_FIFO_RESET		2
-#define MPU6000_USER_CONTROL_I2C_MST_RESET	1
-#define MPU6000_USER_CONTROL_SIG_COND_RESET	0
+#define MPU6000_USER_CTRL		0x6a
+#define MPU6000_USER_CTRL_FIFO_EN		6
+#define MPU6000_USER_CTRL_I2C_MST_EN		5
+#define MPU6000_USER_CTRL_I2C_IF_DIS		4
+#define MPU6000_USER_CTRL_FIFO_RESET		2
+#define MPU6000_USER_CTRL_I2C_MST_RESET		1
+#define MPU6000_USER_CTRL_SIG_COND_RESET	0
 
 #define MPU6000_PWR_MGMT_1	0x6b
 #define MPU6000_PWR_MGMT_1_DEVICE_RESET		7
@@ -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(float sensor) {
+	return 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;