1.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
1.2 +++ b/imu.h Mon Oct 14 13:02:19 2013 +0000
1.3 @@ -0,0 +1,260 @@
1.4 +/*
1.5 + * Ben NanoNote communication with the Pololu MinIMU-9 with the L3G4200D 3-axis
1.6 + * gyroscope and LSM303DLM accelerometer/magnetometer.
1.7 + *
1.8 + * http://www.pololu.com/catalog/product/1265
1.9 + *
1.10 + * Copyright (C) 2013 Paul Boddie
1.11 + *
1.12 + * This program is free software; you can redistribute it and/or modify
1.13 + * it under the terms of the GNU General Public License as published by
1.14 + * the Free Software Foundation; either version 2 of the License, or
1.15 + * (at your option) any later version.
1.16 + */
1.17 +
1.18 +#ifndef __IMU_H__
1.19 +#define __IMU_H__
1.20 +
1.21 +#include <ubb/ubb.h>
1.22 +#include "i2c.h"
1.23 +#include "geo.h"
1.24 +
1.25 +/* Pin assignments:
1.26 + *
1.27 + * Sniffer UBB MinIMU-9
1.28 + * ------- ---- --------
1.29 + * DAT2 DAT2
1.30 + * CD DAT3 SCL
1.31 + * CMD CMD SDA
1.32 + * VCC VDD VIN
1.33 + * CLK CLK (3V)
1.34 + * GND GND GND
1.35 + * DAT0 DAT0 (1V8)
1.36 + * DAT1 DAT1
1.37 + *
1.38 + * The 3V and 1V8 pins play no role in the communication, but a six pin header
1.39 + * exposing these pins on the IMU can be used to connect the IMU with the
1.40 + * Sniffer/UBB without any complications.
1.41 + */
1.42 +
1.43 +#define IMU_SCL UBB_DAT3
1.44 +#define IMU_SDA UBB_CMD
1.45 +#define IMU_3V UBB_CLK
1.46 +#define IMU_1V8 UBB_DAT0
1.47 +
1.48 +/* I2C addresses and operations. */
1.49 +
1.50 +#define IMU_GYRO_ADDRESS 0xd2 /* 1101001x */
1.51 +#define IMU_ACCEL_ADDRESS 0x30 /* 0011000x */
1.52 +#define IMU_MAGNET_ADDRESS 0x3c /* 0011110x */
1.53 +
1.54 +/* Gyroscope registers. */
1.55 +
1.56 +#define IMU_GYRO_WHO_AM_I 0x0f
1.57 +#define IMU_GYRO_CTRL_REG1 0x20
1.58 +#define IMU_GYRO_CTRL_REG4 0x23
1.59 +#define IMU_GYRO_CTRL_REG5 0x24
1.60 +#define IMU_GYRO_OUT_TEMP 0x26
1.61 +#define IMU_GYRO_STATUS_REG 0x27
1.62 +
1.63 +/* Gyroscope register values. */
1.64 +
1.65 +#define IMU_GYRO_CTRL_REG1_PD 0x08
1.66 +#define IMU_GYRO_CTRL_REG1_ZEN 0x04
1.67 +#define IMU_GYRO_CTRL_REG1_YEN 0x02
1.68 +#define IMU_GYRO_CTRL_REG1_XEN 0x01
1.69 +#define IMU_GYRO_CTRL_REG1_ALL 0x0f /* composite of power and all axes */
1.70 +#define IMU_GYRO_CTRL_REG4_BDU 0x80
1.71 +#define IMU_GYRO_CTRL_REG4_250DPS 0x00 /* FS1 = 0; FS0 = 0 */
1.72 +#define IMU_GYRO_CTRL_REG4_500DPS 0x10 /* FS1 = 0; FS0 = 1 */
1.73 +#define IMU_GYRO_CTRL_REG4_2000DPS 0x20 /* FS1 = 1; FS0 = 0 */
1.74 +#define IMU_GYRO_CTRL_REG5_BOOT 0x80
1.75 +#define IMU_GYRO_CTRL_REG5_HPEN 0x10
1.76 +#define IMU_GYRO_CTRL_REG5_OUTSEL1 0x02
1.77 +#define IMU_GYRO_CTRL_REG5_OUTSEL0 0x01
1.78 +#define IMU_GYRO_OUT_X_L 0x28
1.79 +#define IMU_GYRO_OUT_X_H 0x29
1.80 +#define IMU_GYRO_OUT_Y_L 0x2a
1.81 +#define IMU_GYRO_OUT_Y_H 0x2b
1.82 +#define IMU_GYRO_OUT_Z_L 0x2c
1.83 +#define IMU_GYRO_OUT_Z_H 0x2d
1.84 +#define IMU_GYRO_READ_MANY 0x80 /* gyroscope register increment */
1.85 +
1.86 +/* Accelerometer registers. */
1.87 +
1.88 +#define IMU_ACCEL_CTRL_REG1_A 0x20
1.89 +#define IMU_ACCEL_CTRL_REG2_A 0x21
1.90 +#define IMU_ACCEL_CTRL_REG4_A 0x23
1.91 +#define IMU_ACCEL_HP_FILTER_RESET_A 0x25
1.92 +#define IMU_ACCEL_OUT_X_L_A 0x28
1.93 +#define IMU_ACCEL_OUT_X_H_A 0x29
1.94 +#define IMU_ACCEL_OUT_Y_L_A 0x2a
1.95 +#define IMU_ACCEL_OUT_Y_H_A 0x2b
1.96 +#define IMU_ACCEL_OUT_Z_L_A 0x2c
1.97 +#define IMU_ACCEL_OUT_Z_H_A 0x2d
1.98 +#define IMU_ACCEL_READ_MANY 0x80 /* accelerometer register increment */
1.99 +
1.100 +/* Accelerometer register values. */
1.101 +
1.102 +#define IMU_ACCEL_CTRL_REG1_50HZ 0x20 /* normal power */
1.103 +#define IMU_ACCEL_CTRL_REG1_100HZ 0x28 /* normal power */
1.104 +#define IMU_ACCEL_CTRL_REG1_400HZ 0x30 /* normal power */
1.105 +#define IMU_ACCEL_CTRL_REG1_1000HZ 0x38 /* normal power */
1.106 +#define IMU_ACCEL_CTRL_REG1_05HZ 0x40 /* low power, 0.5Hz */
1.107 +#define IMU_ACCEL_CTRL_REG1_1HZ 0x60 /* low power, 1Hz */
1.108 +#define IMU_ACCEL_CTRL_REG1_2HZ 0x80 /* low power, 2Hz */
1.109 +#define IMU_ACCEL_CTRL_REG1_5HZ 0x90 /* low power, 5Hz */
1.110 +#define IMU_ACCEL_CTRL_REG1_10HZ 0xa0 /* low power, 10Hz */
1.111 +#define IMU_ACCEL_CTRL_REG1_ZEN 0x04
1.112 +#define IMU_ACCEL_CTRL_REG1_YEN 0x02
1.113 +#define IMU_ACCEL_CTRL_REG1_XEN 0x01
1.114 +#define IMU_ACCEL_CTRL_REG1_ALL 0x07 /* composite of all axes */
1.115 +#define IMU_ACCEL_CTRL_REG2_FDS 0x10
1.116 +#define IMU_ACCEL_CTRL_REG2_50 0x00 /* filter cut off is rate/50 */
1.117 +#define IMU_ACCEL_CTRL_REG2_100 0x01 /* filter cut off is rate/100 */
1.118 +#define IMU_ACCEL_CTRL_REG2_200 0x02 /* filter cut off is rate/200 */
1.119 +#define IMU_ACCEL_CTRL_REG2_400 0x03 /* filter cut off is rate/400 */
1.120 +#define IMU_ACCEL_CTRL_REG2_HPCF1 0x02
1.121 +#define IMU_ACCEL_CTRL_REG4_BDU 0x80
1.122 +#define IMU_ACCEL_CTRL_REG4_2G 0x00 /* FS1 = 0; FS0 = 0 */
1.123 +#define IMU_ACCEL_CTRL_REG4_4G 0x10 /* FS1 = 0; FS0 = 1 */
1.124 +#define IMU_ACCEL_CTRL_REG4_8G 0x30 /* FS1 = 1; FS0 = 1 */
1.125 +
1.126 +/* Magnetometer registers. */
1.127 +
1.128 +#define IMU_MAGNET_CRA_REG_M 0x00
1.129 +#define IMU_MAGNET_CRB_REG_M 0x01
1.130 +#define IMU_MAGNET_MR_REG_M 0x02
1.131 +#define IMU_MAGNET_OUT_X_H_M 0x03
1.132 +#define IMU_MAGNET_OUT_X_L_M 0x04
1.133 +#define IMU_MAGNET_OUT_Z_H_M 0x05
1.134 +#define IMU_MAGNET_OUT_Z_L_M 0x06
1.135 +#define IMU_MAGNET_OUT_Y_H_M 0x07
1.136 +#define IMU_MAGNET_OUT_Y_L_M 0x08
1.137 +#define IMU_MAGNET_SR_REG_M 0x09
1.138 +#define IMU_MAGNET_IRA_REG_M 0x0a
1.139 +#define IMU_MAGNET_IRB_REG_M 0x0b
1.140 +#define IMU_MAGNET_IRC_REG_M 0x0c
1.141 +#define IMU_MAGNET_WHO_AM_I_M 0x0f
1.142 +
1.143 +/* Magnetometer register values. */
1.144 +
1.145 +#define IMU_MAGNET_CRA_REG_0_75HZ 0x00 /* 0.75Hz */
1.146 +#define IMU_MAGNET_CRA_REG_1_5HZ 0x04 /* 1.5Hz */
1.147 +#define IMU_MAGNET_CRA_REG_3HZ 0x08 /* 3Hz */
1.148 +#define IMU_MAGNET_CRA_REG_7_5HZ 0x0c /* 7.5Hz */
1.149 +#define IMU_MAGNET_CRA_REG_15HZ 0x10 /* 15Hz */
1.150 +#define IMU_MAGNET_CRA_REG_30HZ 0x14 /* 30Hz */
1.151 +#define IMU_MAGNET_CRA_REG_75HZ 0x18 /* 75Hz */
1.152 +#define IMU_MAGNET_CRA_REG_220HZ 0x1c /* 220Hz */
1.153 +#define IMU_MAGNET_CRB_REG_1_3G 0x20 /* 1.3G */
1.154 +#define IMU_MAGNET_CRB_REG_1_9G 0x40 /* 1.9G */
1.155 +#define IMU_MAGNET_CRB_REG_2_5G 0x60 /* 2.5G */
1.156 +#define IMU_MAGNET_CRB_REG_4_0G 0x80 /* 4.0G */
1.157 +#define IMU_MAGNET_CRB_REG_4_7G 0xa0 /* 4.7G */
1.158 +#define IMU_MAGNET_CRB_REG_5_6G 0xc0 /* 5.6G */
1.159 +#define IMU_MAGNET_CRB_REG_8_1G 0xe0 /* 8.1G */
1.160 +#define IMU_MAGNET_MR_REG_CONT 0x00
1.161 +#define IMU_MAGNET_MR_REG_SINGLE 0x01
1.162 +#define IMU_MAGNET_MR_REG_SLEEP 0x02
1.163 +
1.164 +/* Common values. */
1.165 +
1.166 +#define IMU_250DPS_UNIT 8750 /* µdps -> 8.75mdps */
1.167 +#define IMU_500DPS_UNIT 17500 /* µdps -> 17.5mdps */
1.168 +#define IMU_2000DPS_UNIT 70000 /* µdps -> 70mdps */
1.169 +
1.170 +#define IMU_2G_UNIT 1000 /* µg -> 1mg */
1.171 +#define IMU_4G_UNIT 2000 /* µg -> 2mg */
1.172 +#define IMU_8G_UNIT 3900 /* µg -> 3.9mg */
1.173 +
1.174 +#define IMU_1_3G_UNIT 909 /* µG (1G / 1100) */
1.175 +#define IMU_1_9G_UNIT 1169 /* µG (1G / 855) */
1.176 +#define IMU_2_5G_UNIT 1492 /* µG (1G / 670) */
1.177 +#define IMU_4_0G_UNIT 2222 /* µG (1G / 450) */
1.178 +#define IMU_4_7G_UNIT 2500 /* µG (1G / 400) */
1.179 +#define IMU_5_6G_UNIT 3030 /* µG (1G / 330) */
1.180 +#define IMU_8_1G_UNIT 4347 /* µG (1G / 230) */
1.181 +
1.182 +#define IMU_MAGNET_Z_XY_RATIO (9.0 / 8.0) /* adjustments to the above for the smaller Z scale */
1.183 +
1.184 +/* Measurement parameters. */
1.185 +
1.186 +#define IMU_UPDATE_PERIOD 10000 /* µs -> 10ms == 0.01s (100Hz) */
1.187 +#define IMU_MAGNET_UPDATE_PERIOD 30000 /* µs -> 30ms == 0.03s (33Hz) */
1.188 +#define TEXT_UPDATE_PERIOD 500000 /* µs */
1.189 +#define GUI_UPDATE_PERIOD 100000 /* µs */
1.190 +
1.191 +#define IMU_CALIBRATION_WARMUP 100
1.192 +#define IMU_CALIBRATION_READINGS 100
1.193 +
1.194 +#define IMU_ACCEL_BUFFER_SIZE 10
1.195 +
1.196 +#define IMU_UDPS_FACTOR IMU_2000DPS_UNIT
1.197 +#define IMU_GYRO_DPS_SCALE IMU_GYRO_CTRL_REG4_2000DPS
1.198 +
1.199 +#define IMU_UG_FACTOR IMU_2G_UNIT
1.200 +#define IMU_ACCEL_SCALE IMU_ACCEL_CTRL_REG4_2G
1.201 +#define IMU_ACCEL_FREQ IMU_ACCEL_CTRL_REG1_50HZ
1.202 +#define IMU_ACCEL_FILTER_FREQ IMU_ACCEL_CTRL_REG2_50
1.203 +
1.204 +#define IMU_UGAUSS_FACTOR IMU_4_0G_UNIT
1.205 +#define IMU_MAGNET_SCALE IMU_MAGNET_CRB_REG_4_0G
1.206 +#define IMU_MAGNET_FREQ IMU_MAGNET_CRA_REG_30HZ
1.207 +
1.208 +#define ACCEL_G 9.81 /* ms**-2 */
1.209 +
1.210 +#define ACCEL_THRESHOLD 0.1 /* g */
1.211 +#define ACCEL_SUM_THRESHOLD 0.01 /* gs */
1.212 +#define VELOCITY_DECAY_SEVERE 0.5
1.213 +#define VELOCITY_DECAY_GRADUAL 0.9
1.214 +
1.215 +#define ACCEL_REST_MAG_LOWER 0.995
1.216 +#define ACCEL_REST_MAG_UPPER 1.005
1.217 +#define ROTATION_ADJUSTMENT_FACTOR 0.1
1.218 +
1.219 +/* Convert microdegrees * microseconds to degrees. */
1.220 +
1.221 +#define to_angle(x) ((double) (x) / ((uint64_t) 1000000000000))
1.222 +
1.223 +/* Convert microg to g. */
1.224 +
1.225 +#define to_accel(x) ((double) (x) / 1000000)
1.226 +
1.227 +/* Convert microgauss to gauss. */
1.228 +
1.229 +#define to_field(x) to_accel(x)
1.230 +
1.231 +/* Get a period in microseconds. */
1.232 +
1.233 +#define get_period(now, then) ((now.tv_sec - then.tv_sec) * 1000000 + (now.tv_usec - then.tv_usec))
1.234 +
1.235 +/* Function definitions. */
1.236 +
1.237 +bool imu_recv(uint8_t device, uint8_t reg, uint8_t *result, uint8_t len);
1.238 +bool imu_sendone(uint8_t device, uint8_t reg, uint8_t value);
1.239 +int16_t convert(uint8_t raw[]);
1.240 +int16_t convert12(uint8_t raw[]);
1.241 +int16_t convertBE12L(uint8_t raw[]);
1.242 +bool imu_read_vector(uint8_t device, uint8_t reg, vectorf *out, int16_t (*converter)(uint8_t *));
1.243 +bool imu_read_vector_xzy(uint8_t device, uint8_t reg, vectorf *out, int16_t (*converter)(uint8_t *));
1.244 +double scale(double value, double lower, double middle, double upper);
1.245 +void normalise(vectorf *in, vectorf *vmin, vectorf *vmax, vectorf *out);
1.246 +void queue(vectorf values[], int *oldest, int length, vectorf *value);
1.247 +void filter(vectorf values[], int oldest, int length, vectorf *result);
1.248 +void calibrate(vectorf *zerolevel, vectorf zerolevels[], int length, uint8_t address, uint8_t reg, uint16_t delay, int16_t (*converter)(uint8_t *));
1.249 +
1.250 +/* Accelerometer-specific functions. */
1.251 +
1.252 +void average_filter(vectorf *value, vectorf buffer[], int *index, int length);
1.253 +void noise_filter(vectorf *value, double threshold);
1.254 +void apply_acceleration(double acc, double acc_old, double *pos, double *neg, double seconds);
1.255 +void apply_decay(double acc, double* pos, double* neg);
1.256 +void update_velocity(vectorf *velocity, vectorf *acceleration, vectorf *acceleration_old, vectorf *apos, vectorf *aneg, double seconds);
1.257 +void update_displacement(vectorf *displacement, vectorf *velocity, vectorf *velocity_old, double seconds);
1.258 +
1.259 +/* Function aliases. */
1.260 +
1.261 +#define imu_init i2c_init
1.262 +
1.263 +#endif /* __IMU_H__ */