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