first cut at turnon scripts for EasyTimer v2

[fw/altos] / src / drivers / ao_mpu6000.h

diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h
index a42f69c411fd4ab416cbea4bda0bd2400523c203..ae42d4b4eed18ad601ccb9f31ea68e019a2aebd6 100644 (file)
--- a/src/drivers/ao_mpu6000.h
+++ b/src/drivers/ao_mpu6000.h
@@ -3,7 +3,8 @@
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; version 2 of the License.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
  *
  * This program is distributed in the hope that it will be useful, but
  * WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -18,6 +19,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 +171,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;