*
* 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
#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
#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
/* 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;