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