diff options
| author | Keith Packard <keithp@keithp.com> | 2013-10-25 04:05:09 -0700 |
|---|---|---|
| committer | Keith Packard <keithp@keithp.com> | 2013-10-25 04:05:09 -0700 |
| commit | 08143a922fe27bc50a19924f46538f9476ab5fd1 (patch) | |
| tree | cdd2be15a286183eade9d6b665d5bc5a99d5abff /src/drivers | |
| parent | ba99630f33440b993c69830856d2a7741ffdef71 (diff) | |
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>
Diffstat (limited to 'src/drivers')
| -rw-r--r-- | src/drivers/ao_mpu6000.c | 2 | ||||
| -rw-r--r-- | src/drivers/ao_mpu6000.h | 17 |
2 files changed, 18 insertions, 1 deletions
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; |