1 /* 2 * Ben NanoNote communication with the Pololu MinIMU-9 with the L3G4200D 3-axis 3 * gyroscope and LSM303DLM accelerometer/magnetometer. 4 * 5 * http://www.pololu.com/catalog/product/1265 6 * 7 * Copyright (C) 2013 Paul Boddie 8 * 9 * This program is free software; you can redistribute it and/or modify 10 * it under the terms of the GNU General Public License as published by 11 * the Free Software Foundation; either version 2 of the License, or 12 * (at your option) any later version. 13 */ 14 15 #ifndef __IMU_H__ 16 #define __IMU_H__ 17 18 #include <ubb/ubb.h> 19 #include "i2c.h" 20 #include "geo.h" 21 22 /* Pin assignments: 23 * 24 * Sniffer UBB MinIMU-9 25 * ------- ---- -------- 26 * DAT2 DAT2 27 * CD DAT3 SCL 28 * CMD CMD SDA 29 * VCC VDD VIN 30 * CLK CLK (3V) 31 * GND GND GND 32 * DAT0 DAT0 (1V8) 33 * DAT1 DAT1 34 * 35 * The 3V and 1V8 pins play no role in the communication, but a six pin header 36 * exposing these pins on the IMU can be used to connect the IMU with the 37 * Sniffer/UBB without any complications. 38 */ 39 40 #define IMU_SCL UBB_DAT3 41 #define IMU_SDA UBB_CMD 42 #define IMU_3V UBB_CLK 43 #define IMU_1V8 UBB_DAT0 44 45 /* I2C addresses and operations. */ 46 47 #define IMU_GYRO_ADDRESS 0xd2 /* 1101001x */ 48 #define IMU_ACCEL_ADDRESS 0x30 /* 0011000x */ 49 #define IMU_MAGNET_ADDRESS 0x3c /* 0011110x */ 50 51 /* Gyroscope registers. */ 52 53 #define IMU_GYRO_WHO_AM_I 0x0f 54 #define IMU_GYRO_CTRL_REG1 0x20 55 #define IMU_GYRO_CTRL_REG4 0x23 56 #define IMU_GYRO_CTRL_REG5 0x24 57 #define IMU_GYRO_OUT_TEMP 0x26 58 #define IMU_GYRO_STATUS_REG 0x27 59 60 /* Gyroscope register values. */ 61 62 #define IMU_GYRO_CTRL_REG1_PD 0x08 63 #define IMU_GYRO_CTRL_REG1_ZEN 0x04 64 #define IMU_GYRO_CTRL_REG1_YEN 0x02 65 #define IMU_GYRO_CTRL_REG1_XEN 0x01 66 #define IMU_GYRO_CTRL_REG1_ALL 0x0f /* composite of power and all axes */ 67 #define IMU_GYRO_CTRL_REG4_BDU 0x80 68 #define IMU_GYRO_CTRL_REG4_250DPS 0x00 /* FS1 = 0; FS0 = 0 */ 69 #define IMU_GYRO_CTRL_REG4_500DPS 0x10 /* FS1 = 0; FS0 = 1 */ 70 #define IMU_GYRO_CTRL_REG4_2000DPS 0x20 /* FS1 = 1; FS0 = 0 */ 71 #define IMU_GYRO_CTRL_REG5_BOOT 0x80 72 #define IMU_GYRO_CTRL_REG5_HPEN 0x10 73 #define IMU_GYRO_CTRL_REG5_OUTSEL1 0x02 74 #define IMU_GYRO_CTRL_REG5_OUTSEL0 0x01 75 #define IMU_GYRO_OUT_X_L 0x28 76 #define IMU_GYRO_OUT_X_H 0x29 77 #define IMU_GYRO_OUT_Y_L 0x2a 78 #define IMU_GYRO_OUT_Y_H 0x2b 79 #define IMU_GYRO_OUT_Z_L 0x2c 80 #define IMU_GYRO_OUT_Z_H 0x2d 81 #define IMU_GYRO_READ_MANY 0x80 /* gyroscope register increment */ 82 83 /* Accelerometer registers. */ 84 85 #define IMU_ACCEL_CTRL_REG1_A 0x20 86 #define IMU_ACCEL_CTRL_REG2_A 0x21 87 #define IMU_ACCEL_CTRL_REG4_A 0x23 88 #define IMU_ACCEL_HP_FILTER_RESET_A 0x25 89 #define IMU_ACCEL_OUT_X_L_A 0x28 90 #define IMU_ACCEL_OUT_X_H_A 0x29 91 #define IMU_ACCEL_OUT_Y_L_A 0x2a 92 #define IMU_ACCEL_OUT_Y_H_A 0x2b 93 #define IMU_ACCEL_OUT_Z_L_A 0x2c 94 #define IMU_ACCEL_OUT_Z_H_A 0x2d 95 #define IMU_ACCEL_READ_MANY 0x80 /* accelerometer register increment */ 96 97 /* Accelerometer register values. */ 98 99 #define IMU_ACCEL_CTRL_REG1_50HZ 0x20 /* normal power */ 100 #define IMU_ACCEL_CTRL_REG1_100HZ 0x28 /* normal power */ 101 #define IMU_ACCEL_CTRL_REG1_400HZ 0x30 /* normal power */ 102 #define IMU_ACCEL_CTRL_REG1_1000HZ 0x38 /* normal power */ 103 #define IMU_ACCEL_CTRL_REG1_05HZ 0x40 /* low power, 0.5Hz */ 104 #define IMU_ACCEL_CTRL_REG1_1HZ 0x60 /* low power, 1Hz */ 105 #define IMU_ACCEL_CTRL_REG1_2HZ 0x80 /* low power, 2Hz */ 106 #define IMU_ACCEL_CTRL_REG1_5HZ 0x90 /* low power, 5Hz */ 107 #define IMU_ACCEL_CTRL_REG1_10HZ 0xa0 /* low power, 10Hz */ 108 #define IMU_ACCEL_CTRL_REG1_ZEN 0x04 109 #define IMU_ACCEL_CTRL_REG1_YEN 0x02 110 #define IMU_ACCEL_CTRL_REG1_XEN 0x01 111 #define IMU_ACCEL_CTRL_REG1_ALL 0x07 /* composite of all axes */ 112 #define IMU_ACCEL_CTRL_REG2_FDS 0x10 113 #define IMU_ACCEL_CTRL_REG2_50 0x00 /* filter cut off is rate/50 */ 114 #define IMU_ACCEL_CTRL_REG2_100 0x01 /* filter cut off is rate/100 */ 115 #define IMU_ACCEL_CTRL_REG2_200 0x02 /* filter cut off is rate/200 */ 116 #define IMU_ACCEL_CTRL_REG2_400 0x03 /* filter cut off is rate/400 */ 117 #define IMU_ACCEL_CTRL_REG2_HPCF1 0x02 118 #define IMU_ACCEL_CTRL_REG4_BDU 0x80 119 #define IMU_ACCEL_CTRL_REG4_2G 0x00 /* FS1 = 0; FS0 = 0 */ 120 #define IMU_ACCEL_CTRL_REG4_4G 0x10 /* FS1 = 0; FS0 = 1 */ 121 #define IMU_ACCEL_CTRL_REG4_8G 0x30 /* FS1 = 1; FS0 = 1 */ 122 123 /* Magnetometer registers. */ 124 125 #define IMU_MAGNET_CRA_REG_M 0x00 126 #define IMU_MAGNET_CRB_REG_M 0x01 127 #define IMU_MAGNET_MR_REG_M 0x02 128 #define IMU_MAGNET_OUT_X_H_M 0x03 129 #define IMU_MAGNET_OUT_X_L_M 0x04 130 #define IMU_MAGNET_OUT_Z_H_M 0x05 131 #define IMU_MAGNET_OUT_Z_L_M 0x06 132 #define IMU_MAGNET_OUT_Y_H_M 0x07 133 #define IMU_MAGNET_OUT_Y_L_M 0x08 134 #define IMU_MAGNET_SR_REG_M 0x09 135 #define IMU_MAGNET_IRA_REG_M 0x0a 136 #define IMU_MAGNET_IRB_REG_M 0x0b 137 #define IMU_MAGNET_IRC_REG_M 0x0c 138 #define IMU_MAGNET_WHO_AM_I_M 0x0f 139 140 /* Magnetometer register values. */ 141 142 #define IMU_MAGNET_CRA_REG_0_75HZ 0x00 /* 0.75Hz */ 143 #define IMU_MAGNET_CRA_REG_1_5HZ 0x04 /* 1.5Hz */ 144 #define IMU_MAGNET_CRA_REG_3HZ 0x08 /* 3Hz */ 145 #define IMU_MAGNET_CRA_REG_7_5HZ 0x0c /* 7.5Hz */ 146 #define IMU_MAGNET_CRA_REG_15HZ 0x10 /* 15Hz */ 147 #define IMU_MAGNET_CRA_REG_30HZ 0x14 /* 30Hz */ 148 #define IMU_MAGNET_CRA_REG_75HZ 0x18 /* 75Hz */ 149 #define IMU_MAGNET_CRA_REG_220HZ 0x1c /* 220Hz */ 150 #define IMU_MAGNET_CRB_REG_1_3G 0x20 /* 1.3G */ 151 #define IMU_MAGNET_CRB_REG_1_9G 0x40 /* 1.9G */ 152 #define IMU_MAGNET_CRB_REG_2_5G 0x60 /* 2.5G */ 153 #define IMU_MAGNET_CRB_REG_4_0G 0x80 /* 4.0G */ 154 #define IMU_MAGNET_CRB_REG_4_7G 0xa0 /* 4.7G */ 155 #define IMU_MAGNET_CRB_REG_5_6G 0xc0 /* 5.6G */ 156 #define IMU_MAGNET_CRB_REG_8_1G 0xe0 /* 8.1G */ 157 #define IMU_MAGNET_MR_REG_CONT 0x00 158 #define IMU_MAGNET_MR_REG_SINGLE 0x01 159 #define IMU_MAGNET_MR_REG_SLEEP 0x02 160 161 /* Common values. */ 162 163 #define IMU_250DPS_UNIT 8750 /* ?dps -> 8.75mdps */ 164 #define IMU_500DPS_UNIT 17500 /* ?dps -> 17.5mdps */ 165 #define IMU_2000DPS_UNIT 70000 /* ?dps -> 70mdps */ 166 167 #define IMU_2G_UNIT 1000 /* ?g -> 1mg */ 168 #define IMU_4G_UNIT 2000 /* ?g -> 2mg */ 169 #define IMU_8G_UNIT 3900 /* ?g -> 3.9mg */ 170 171 #define IMU_1_3G_UNIT 909 /* ?G (1G / 1100) */ 172 #define IMU_1_9G_UNIT 1169 /* ?G (1G / 855) */ 173 #define IMU_2_5G_UNIT 1492 /* ?G (1G / 670) */ 174 #define IMU_4_0G_UNIT 2222 /* ?G (1G / 450) */ 175 #define IMU_4_7G_UNIT 2500 /* ?G (1G / 400) */ 176 #define IMU_5_6G_UNIT 3030 /* ?G (1G / 330) */ 177 #define IMU_8_1G_UNIT 4347 /* ?G (1G / 230) */ 178 179 #define IMU_MAGNET_Z_XY_RATIO (9.0 / 8.0) /* adjustments to the above for the smaller Z scale */ 180 181 /* Measurement parameters. */ 182 183 #define IMU_UPDATE_PERIOD 10000 /* ?s -> 10ms == 0.01s (100Hz) */ 184 #define IMU_MAGNET_UPDATE_PERIOD 30000 /* ?s -> 30ms == 0.03s (33Hz) */ 185 #define TEXT_UPDATE_PERIOD 500000 /* ?s */ 186 #define GUI_UPDATE_PERIOD 100000 /* ?s */ 187 188 #define IMU_CALIBRATION_WARMUP 100 189 #define IMU_CALIBRATION_READINGS 100 190 191 #define IMU_ACCEL_BUFFER_SIZE 10 192 193 #define IMU_UDPS_FACTOR IMU_2000DPS_UNIT 194 #define IMU_GYRO_DPS_SCALE IMU_GYRO_CTRL_REG4_2000DPS 195 196 #define IMU_UG_FACTOR IMU_2G_UNIT 197 #define IMU_ACCEL_SCALE IMU_ACCEL_CTRL_REG4_2G 198 #define IMU_ACCEL_FREQ IMU_ACCEL_CTRL_REG1_50HZ 199 #define IMU_ACCEL_FILTER_FREQ IMU_ACCEL_CTRL_REG2_50 200 201 #define IMU_UGAUSS_FACTOR IMU_4_0G_UNIT 202 #define IMU_MAGNET_SCALE IMU_MAGNET_CRB_REG_4_0G 203 #define IMU_MAGNET_FREQ IMU_MAGNET_CRA_REG_30HZ 204 205 #define ACCEL_G 9.81 /* ms**-2 */ 206 207 #define ACCEL_THRESHOLD 0.1 /* g */ 208 #define ACCEL_SUM_THRESHOLD 0.01 /* gs */ 209 #define VELOCITY_DECAY_SEVERE 0.5 210 #define VELOCITY_DECAY_GRADUAL 0.9 211 212 #define ACCEL_REST_MAG_LOWER 0.995 213 #define ACCEL_REST_MAG_UPPER 1.005 214 #define ROTATION_ADJUSTMENT_FACTOR 0.1 215 216 /* Convert microdegrees * microseconds to degrees. */ 217 218 #define to_angle(x) ((double) (x) / ((uint64_t) 1000000000000)) 219 220 /* Convert microg to g. */ 221 222 #define to_accel(x) ((double) (x) / 1000000) 223 224 /* Convert microgauss to gauss. */ 225 226 #define to_field(x) to_accel(x) 227 228 /* Get a period in microseconds. */ 229 230 #define get_period(now, then) ((now.tv_sec - then.tv_sec) * 1000000 + (now.tv_usec - then.tv_usec)) 231 232 /* Function definitions. */ 233 234 bool imu_recv(uint8_t device, uint8_t reg, uint8_t *result, uint8_t len); 235 bool imu_sendone(uint8_t device, uint8_t reg, uint8_t value); 236 int16_t convert(uint8_t raw[]); 237 int16_t convert12(uint8_t raw[]); 238 int16_t convertBE12L(uint8_t raw[]); 239 bool imu_read_vector(uint8_t device, uint8_t reg, vectorf *out, int16_t (*converter)(uint8_t *)); 240 bool imu_read_vector_xzy(uint8_t device, uint8_t reg, vectorf *out, int16_t (*converter)(uint8_t *)); 241 double scale(double value, double lower, double middle, double upper); 242 void normalise(vectorf *in, vectorf *vmin, vectorf *vmax, vectorf *out); 243 void queue(vectorf values[], int *oldest, int length, vectorf *value); 244 void filter(vectorf values[], int oldest, int length, vectorf *result); 245 void calibrate(vectorf *zerolevel, vectorf zerolevels[], int length, uint8_t address, uint8_t reg, uint16_t delay, int16_t (*converter)(uint8_t *)); 246 247 /* Accelerometer-specific functions. */ 248 249 void average_filter(vectorf *value, vectorf buffer[], int *index, int length); 250 void noise_filter(vectorf *value, double threshold); 251 void apply_acceleration(double acc, double acc_old, double *pos, double *neg, double seconds); 252 void apply_decay(double acc, double* pos, double* neg); 253 void update_velocity(vectorf *velocity, vectorf *acceleration, vectorf *acceleration_old, vectorf *apos, vectorf *aneg, double seconds); 254 void update_displacement(vectorf *displacement, vectorf *velocity, vectorf *velocity_old, double seconds); 255 256 /* Function aliases. */ 257 258 #define imu_init i2c_init 259 260 #endif /* __IMU_H__ */