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 | #include <string.h> |
paul@0 | 16 | #include <unistd.h> |
paul@0 | 17 | #include "imu.h" |
paul@0 | 18 | |
paul@0 | 19 | const uint32_t I2C_SCL = IMU_SCL, I2C_SDA = IMU_SDA; |
paul@0 | 20 | |
paul@0 | 21 | /** |
paul@0 | 22 | * Receive data from the indicated device, using the given device register, and |
paul@0 | 23 | * storing the result in the buffer provided, expecting a transmission of the |
paul@0 | 24 | * specified length in bytes. Return whether the transaction was successful. |
paul@0 | 25 | */ |
paul@0 | 26 | bool imu_recv(uint8_t device, uint8_t reg, uint8_t *result, uint8_t len) |
paul@0 | 27 | { |
paul@0 | 28 | uint8_t data[] = {device | I2C_WRITE, reg}; |
paul@0 | 29 | |
paul@0 | 30 | memset(result, 0, len); |
paul@0 | 31 | |
paul@0 | 32 | /* Select the register. */ |
paul@0 | 33 | |
paul@0 | 34 | i2c_start(); |
paul@0 | 35 | if (!i2c_sendmany(data, 2)) |
paul@0 | 36 | { |
paul@0 | 37 | i2c_stop(); |
paul@0 | 38 | return false; |
paul@0 | 39 | } |
paul@0 | 40 | |
paul@0 | 41 | /* Continue with a read from the device. */ |
paul@0 | 42 | |
paul@0 | 43 | i2c_repeated_start(); |
paul@0 | 44 | if (!i2c_send(device | I2C_READ)) |
paul@0 | 45 | { |
paul@0 | 46 | i2c_stop(); |
paul@0 | 47 | return false; |
paul@0 | 48 | } |
paul@0 | 49 | i2c_recvmany(result, len); |
paul@0 | 50 | |
paul@0 | 51 | /* Stop the communication. */ |
paul@0 | 52 | |
paul@0 | 53 | i2c_stop(); |
paul@0 | 54 | return true; |
paul@0 | 55 | } |
paul@0 | 56 | |
paul@0 | 57 | /** |
paul@0 | 58 | * Send a single byte of data to the indicated device, using the given device |
paul@0 | 59 | * register. Return whether the transaction was successful. |
paul@0 | 60 | */ |
paul@0 | 61 | bool imu_sendone(uint8_t device, uint8_t reg, uint8_t value) |
paul@0 | 62 | { |
paul@0 | 63 | uint8_t data[] = {device | I2C_WRITE, reg, value}; |
paul@0 | 64 | |
paul@0 | 65 | /* Select the register. */ |
paul@0 | 66 | |
paul@0 | 67 | i2c_start(); |
paul@0 | 68 | if (!i2c_sendmany(data, 3)) |
paul@0 | 69 | { |
paul@0 | 70 | i2c_stop(); |
paul@0 | 71 | return false; |
paul@0 | 72 | } |
paul@0 | 73 | |
paul@0 | 74 | /* Stop the communication. */ |
paul@0 | 75 | |
paul@0 | 76 | i2c_stop(); |
paul@0 | 77 | return true; |
paul@0 | 78 | } |
paul@0 | 79 | |
paul@0 | 80 | /** |
paul@0 | 81 | * Convert the first two bytes of the given raw data to a signed integer. |
paul@0 | 82 | */ |
paul@0 | 83 | int16_t convert(uint8_t raw[]) |
paul@0 | 84 | { |
paul@0 | 85 | uint16_t raw16 = raw[0] | raw[1] << 8; |
paul@0 | 86 | if (raw16 & 0x8000) |
paul@0 | 87 | return -(raw16 ^ 0xffff) - 1; |
paul@0 | 88 | else |
paul@0 | 89 | return raw16; |
paul@0 | 90 | } |
paul@0 | 91 | |
paul@0 | 92 | /** |
paul@0 | 93 | * Convert the first two bytes of the given raw data to a signed integer, |
paul@0 | 94 | * discarding the least significant four bits of the raw data. |
paul@0 | 95 | */ |
paul@0 | 96 | int16_t convert12(uint8_t raw[]) |
paul@0 | 97 | { |
paul@0 | 98 | uint16_t raw16 = ((raw[0] >> 4) & 0x0f) | (raw[1] << 4); |
paul@0 | 99 | if (raw16 & 0x0800) |
paul@0 | 100 | return -(raw16 ^ 0x0fff) - 1; |
paul@0 | 101 | else |
paul@0 | 102 | return raw16; |
paul@0 | 103 | } |
paul@0 | 104 | |
paul@0 | 105 | /** |
paul@0 | 106 | * Convert the first two bytes of the given raw data to a signed integer, |
paul@0 | 107 | * discarding the most significant four bits of the raw data. |
paul@0 | 108 | */ |
paul@0 | 109 | int16_t convertBE12L(uint8_t raw[]) |
paul@0 | 110 | { |
paul@0 | 111 | uint16_t raw16 = ((raw[0] & 0x0f) << 8) | raw[1]; |
paul@0 | 112 | if (raw16 & 0x0800) |
paul@0 | 113 | return -(raw16 ^ 0x0fff) - 1; |
paul@0 | 114 | else |
paul@0 | 115 | return raw16; |
paul@0 | 116 | } |
paul@0 | 117 | |
paul@0 | 118 | /** |
paul@0 | 119 | * Receive data from the indicated device, using the given device register, and |
paul@0 | 120 | * storing the result in the output parameters provided. The given converter |
paul@0 | 121 | * function will convert the raw data to the output form. Return whether the |
paul@0 | 122 | * transaction was successful. |
paul@0 | 123 | */ |
paul@0 | 124 | bool imu_read_vector(uint8_t device, uint8_t reg, vectorf *out, int16_t (*converter)(uint8_t *)) |
paul@0 | 125 | { |
paul@0 | 126 | uint8_t result[6]; |
paul@0 | 127 | |
paul@0 | 128 | if (!imu_recv(device, reg, result, 6)) |
paul@0 | 129 | return false; |
paul@0 | 130 | |
paul@0 | 131 | out->x = converter(result); |
paul@0 | 132 | out->y = converter(result + 2); |
paul@0 | 133 | out->z = converter(result + 4); |
paul@0 | 134 | |
paul@0 | 135 | return true; |
paul@0 | 136 | } |
paul@0 | 137 | |
paul@0 | 138 | bool imu_read_vector_xzy(uint8_t device, uint8_t reg, vectorf *out, int16_t (*converter)(uint8_t *)) |
paul@0 | 139 | { |
paul@0 | 140 | double tmp; |
paul@0 | 141 | |
paul@0 | 142 | if (imu_read_vector(device, reg, out, converter)) |
paul@0 | 143 | { |
paul@0 | 144 | tmp = out->y; |
paul@0 | 145 | out->y = out->z; |
paul@0 | 146 | out->z = tmp; |
paul@0 | 147 | return true; |
paul@0 | 148 | } |
paul@0 | 149 | |
paul@0 | 150 | return false; |
paul@0 | 151 | } |
paul@0 | 152 | |
paul@0 | 153 | /** |
paul@0 | 154 | * Return the given value scaled to the range defined by lower, middle and upper |
paul@0 | 155 | * points. |
paul@0 | 156 | */ |
paul@0 | 157 | double scale(double value, double lower, double middle, double upper) |
paul@0 | 158 | { |
paul@0 | 159 | if (value < middle) |
paul@0 | 160 | return (value - middle) / (middle - lower); |
paul@0 | 161 | else |
paul@0 | 162 | return (value - middle) / (upper - middle); |
paul@0 | 163 | } |
paul@0 | 164 | |
paul@0 | 165 | /** |
paul@0 | 166 | * Normalise the given vector based on the minimum and maximum values. |
paul@0 | 167 | */ |
paul@0 | 168 | void normalise(vectorf *in, vectorf *vmin, vectorf *vmax, vectorf *out) |
paul@0 | 169 | { |
paul@0 | 170 | out->x = scale(in->x, vmin->x, (vmin->x + vmax->x) / 2, vmax->x); |
paul@0 | 171 | out->y = scale(in->y, vmin->y, (vmin->y + vmax->y) / 2, vmax->y); |
paul@0 | 172 | out->z = scale(in->z, vmin->z, (vmin->z + vmax->z) / 2, vmax->z); |
paul@0 | 173 | } |
paul@0 | 174 | |
paul@0 | 175 | /** |
paul@0 | 176 | * Add a measurement to a circular buffer usable for filtering. |
paul@0 | 177 | */ |
paul@0 | 178 | void queue(vectorf values[], int *oldest, int length, vectorf *value) |
paul@0 | 179 | { |
paul@0 | 180 | values[*oldest] = *value; |
paul@0 | 181 | *oldest = (*oldest + 1) % length; |
paul@0 | 182 | } |
paul@0 | 183 | |
paul@0 | 184 | /** |
paul@0 | 185 | * Return a filtered measurement using the given values from a circular buffer |
paul@0 | 186 | * with the specified oldest measurement index, and with the buffer having the |
paul@0 | 187 | * given length. The filtered measurement is placed in the result vector. |
paul@0 | 188 | */ |
paul@0 | 189 | void filter(vectorf values[], int oldest, int length, vectorf *result) |
paul@0 | 190 | { |
paul@0 | 191 | int i; |
paul@0 | 192 | |
paul@0 | 193 | result->x = 0; |
paul@0 | 194 | result->y = 0; |
paul@0 | 195 | result->z = 0; |
paul@0 | 196 | |
paul@0 | 197 | /* NOTE: For now, a plain average is used. */ |
paul@0 | 198 | |
paul@0 | 199 | for (i = 0; i < length; i++) |
paul@0 | 200 | { |
paul@0 | 201 | result->x += values[i].x; |
paul@0 | 202 | result->y += values[i].y; |
paul@0 | 203 | result->z += values[i].z; |
paul@0 | 204 | } |
paul@0 | 205 | |
paul@0 | 206 | result->x /= length; |
paul@0 | 207 | result->y /= length; |
paul@0 | 208 | result->z /= length; |
paul@0 | 209 | } |
paul@0 | 210 | |
paul@0 | 211 | /** |
paul@0 | 212 | * Populate the given zero base/levels for the device with the given address, |
paul@0 | 213 | * using the specified register to obtain measurements, performing the given |
paul@0 | 214 | * number of warm-up measurements and then proper readings, with the specified |
paul@0 | 215 | * delay between each one. |
paul@0 | 216 | */ |
paul@0 | 217 | void calibrate(vectorf *zerolevel, vectorf zerolevels[], int length, uint8_t address, uint8_t reg, uint16_t delay, int16_t (*converter)(uint8_t *)) |
paul@0 | 218 | { |
paul@0 | 219 | int reading = 0, index = 0; |
paul@0 | 220 | vectorf level; |
paul@0 | 221 | |
paul@0 | 222 | zerolevel->x = 0; |
paul@0 | 223 | zerolevel->y = 0; |
paul@0 | 224 | zerolevel->z = 0; |
paul@0 | 225 | |
paul@0 | 226 | while (reading < IMU_CALIBRATION_WARMUP + IMU_CALIBRATION_READINGS) |
paul@0 | 227 | { |
paul@0 | 228 | if (imu_read_vector(address, reg, &zerolevels[index], converter)) |
paul@0 | 229 | { |
paul@0 | 230 | index = (index + 1) % length; |
paul@0 | 231 | |
paul@0 | 232 | /* Ignore the first few readings. */ |
paul@0 | 233 | |
paul@0 | 234 | if (reading >= IMU_CALIBRATION_WARMUP) |
paul@0 | 235 | { |
paul@0 | 236 | filter(zerolevels, index, length, &level); |
paul@0 | 237 | zerolevel->x += level.x; |
paul@0 | 238 | zerolevel->y += level.y; |
paul@0 | 239 | zerolevel->z += level.z; |
paul@0 | 240 | } |
paul@0 | 241 | |
paul@0 | 242 | reading += 1; |
paul@0 | 243 | } |
paul@0 | 244 | |
paul@0 | 245 | usleep(delay); |
paul@0 | 246 | } |
paul@0 | 247 | |
paul@0 | 248 | zerolevel->x /= IMU_CALIBRATION_READINGS; |
paul@0 | 249 | zerolevel->y /= IMU_CALIBRATION_READINGS; |
paul@0 | 250 | zerolevel->z /= IMU_CALIBRATION_READINGS; |
paul@0 | 251 | } |
paul@0 | 252 | |
paul@0 | 253 | /* Accelerometer-specific functions. */ |
paul@0 | 254 | |
paul@0 | 255 | void average_filter(vectorf *value, vectorf buffer[], int *index, int length) |
paul@0 | 256 | { |
paul@0 | 257 | queue(buffer, index, length, value); |
paul@0 | 258 | filter(buffer, *index, length, value); |
paul@0 | 259 | } |
paul@0 | 260 | |
paul@0 | 261 | void noise_filter(vectorf *value, double threshold) |
paul@0 | 262 | { |
paul@0 | 263 | value->x = noise(value->x, threshold); |
paul@0 | 264 | value->y = noise(value->y, threshold); |
paul@0 | 265 | value->z = noise(value->z, threshold); |
paul@0 | 266 | } |
paul@0 | 267 | |
paul@0 | 268 | void apply_acceleration(double acc, double acc_old, double *pos, double *neg, double seconds) |
paul@0 | 269 | { |
paul@0 | 270 | if (acc > 0) |
paul@0 | 271 | *pos += (acc + acc_old) / 2 * seconds; |
paul@0 | 272 | else |
paul@0 | 273 | *neg += (acc + acc_old) / 2 * seconds; |
paul@0 | 274 | } |
paul@0 | 275 | |
paul@0 | 276 | void apply_decay(double acc, double* pos, double* neg) |
paul@0 | 277 | { |
paul@0 | 278 | double decay = 1.0; |
paul@0 | 279 | |
paul@0 | 280 | if (*pos < -*neg) |
paul@0 | 281 | { |
paul@0 | 282 | if ((acc < -ACCEL_THRESHOLD) && (*pos > ACCEL_SUM_THRESHOLD)) |
paul@0 | 283 | decay = VELOCITY_DECAY_SEVERE; |
paul@0 | 284 | if (fabs(acc) <= ACCEL_THRESHOLD) |
paul@0 | 285 | decay = VELOCITY_DECAY_GRADUAL; |
paul@0 | 286 | *neg *= decay; |
paul@0 | 287 | } |
paul@0 | 288 | if (*pos > -*neg) |
paul@0 | 289 | { |
paul@0 | 290 | if ((acc > ACCEL_THRESHOLD) && (*neg < -ACCEL_SUM_THRESHOLD)) |
paul@0 | 291 | decay = VELOCITY_DECAY_SEVERE; |
paul@0 | 292 | if (fabs(acc) <= ACCEL_THRESHOLD) |
paul@0 | 293 | decay = VELOCITY_DECAY_GRADUAL; |
paul@0 | 294 | *pos *= decay; |
paul@0 | 295 | } |
paul@0 | 296 | } |
paul@0 | 297 | |
paul@0 | 298 | void update_velocity(vectorf *velocity, vectorf *acceleration, vectorf *acceleration_old, vectorf *apos, vectorf *aneg, double seconds) |
paul@0 | 299 | { |
paul@0 | 300 | apply_acceleration(acceleration->x, acceleration_old->x, &(apos->x), &(aneg->x), seconds); |
paul@0 | 301 | apply_acceleration(acceleration->y, acceleration_old->y, &(apos->y), &(aneg->y), seconds); |
paul@0 | 302 | apply_acceleration(acceleration->z, acceleration_old->z, &(apos->z), &(aneg->z), seconds); |
paul@0 | 303 | |
paul@0 | 304 | apply_decay(acceleration->x, &(apos->x), &(aneg->x)); |
paul@0 | 305 | apply_decay(acceleration->y, &(apos->y), &(aneg->y)); |
paul@0 | 306 | apply_decay(acceleration->z, &(apos->z), &(aneg->z)); |
paul@0 | 307 | |
paul@0 | 308 | velocity->x = apos->x + aneg->x; |
paul@0 | 309 | velocity->y = apos->y + aneg->y; |
paul@0 | 310 | velocity->z = apos->z + aneg->z; |
paul@0 | 311 | } |
paul@0 | 312 | |
paul@0 | 313 | void update_displacement(vectorf *displacement, vectorf *velocity, vectorf *velocity_old, double seconds) |
paul@0 | 314 | { |
paul@0 | 315 | displacement->x += (velocity->x + velocity_old->x) / 2 * seconds; |
paul@0 | 316 | displacement->y += (velocity->y + velocity_old->y) / 2 * seconds; |
paul@0 | 317 | displacement->z += (velocity->z + velocity_old->z) / 2 * seconds; |
paul@0 | 318 | } |