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 <stdio.h> 16 #include <stdlib.h> 17 #include <sys/time.h> 18 #include <unistd.h> 19 #include <pthread.h> 20 #include "imu.h" 21 #include "geo.h" 22 23 #define __MEASURE_H_PRIVATE__ 24 #include "measure.h" 25 #undef __MEASURE_H_PRIVATE__ 26 27 static bool setF0 = false; 28 29 /** 30 * Perform calibration with feedback given in the user interface. 31 */ 32 void ui_calibrate(bool using_filter, int (*print)(const char *, ...), void (*flush)()) 33 { 34 vectorf tmpB[1]; 35 36 print("Calibrating...\n"); 37 flush(); 38 39 /* Calibrate without a filter for rotation. */ 40 41 calibrate(&rotation0, tmpB, 1, IMU_GYRO_ADDRESS, IMU_GYRO_OUT_X_L | IMU_GYRO_READ_MANY, IMU_UPDATE_PERIOD, convert); 42 43 print("Calibrated using (%.4f, %.4f, %.4f).\n", rotation0.x, rotation0.y, rotation0.z); 44 flush(); 45 46 /* Calibrate using a filter for acceleration. */ 47 48 calibrate(&acceleration0, accelerationB, IMU_ACCEL_BUFFER_SIZE, 49 IMU_ACCEL_ADDRESS, IMU_ACCEL_OUT_X_L_A | IMU_ACCEL_READ_MANY, IMU_UPDATE_PERIOD, convert12); 50 51 /* Filter out the expected 1g measurement on the z axis. */ 52 53 if (!using_filter) 54 acceleration0.z += acceleration1g; 55 56 print("Calibrated using (%.4f, %.4f, %.4f).\n", acceleration0.x, acceleration0.y, acceleration0.z); 57 flush(); 58 59 mag_calibrate(); 60 61 print("Calibrated using (%.1f, %.1f, %.1f), (%.1f, %.1f, %.1f).\n", 62 fieldmin.x, fieldmin.y, fieldmin.z, 63 fieldmax.x, fieldmax.y, fieldmax.z); 64 } 65 66 void mag_calibrate() 67 { 68 FILE *magnetcfg; 69 char fieldmins[3][8], fieldmaxs[3][8], *endptr; 70 double value; 71 int i; 72 73 /* Read magnetometer settings, if possible. */ 74 75 magnetcfg = fopen(IMU_MAGNET_SETTINGS_FILE, "r"); 76 77 if (magnetcfg != NULL) 78 { 79 fscanf(magnetcfg, "%7s %7s %7s %7s %7s %7s", 80 fieldmins[0], fieldmins[1], fieldmins[2], 81 fieldmaxs[0], fieldmaxs[1], fieldmaxs[2]); 82 83 for (i = 0; i < 3; i++) 84 { 85 value = strtod(fieldmins[i], &endptr); 86 if (endptr != fieldmins[i]) 87 fieldmin.axis[i] = value; 88 value = strtod(fieldmaxs[i], &endptr); 89 if (endptr != fieldmaxs[i]) 90 fieldmax.axis[i] = value; 91 } 92 93 fclose(magnetcfg); 94 } 95 } 96 97 void *get_measurements(void *arg) 98 { 99 struct timeval now; 100 uint32_t period; 101 bool using_filter = false; 102 double accelerationM; 103 bool set_reference = false; 104 105 pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL); 106 107 if (arg != NULL) 108 using_filter = *((bool *) arg); 109 110 /* Initialise the default device orientation. */ 111 112 devicex = devicex0; 113 devicey = devicey0; 114 devicez = devicez0; 115 116 /* Note the time to schedule the next update. */ 117 118 gettimeofday(&imu_updated, NULL); 119 imu_magnet_updated = imu_updated; 120 121 /* NOTE: Wake up the stupid magnetometer. */ 122 123 imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_CONT); 124 125 /* Actual readings. */ 126 127 while (1) 128 { 129 gettimeofday(&now, NULL); 130 131 period = get_period(now, imu_magnet_updated); 132 133 if (period >= IMU_MAGNET_UPDATE_PERIOD) 134 { 135 imu_magnet_updated = now; 136 137 pthread_mutex_lock(&mutex); 138 139 if (imu_read_vector_xzy(IMU_MAGNET_ADDRESS, IMU_MAGNET_OUT_X_H_M, 140 &field, convertBE12L)) 141 { 142 /* NOTE: Handle stupid magnetometer readings. */ 143 144 if (!setF0 && (field.x == 0)) 145 { 146 field.y = 0; field.z = 0; 147 } 148 else 149 { 150 normalise(&field, &fieldmin, &fieldmax, &field); 151 field.x = to_field(field.x * IMU_UGAUSS_FACTOR); 152 field.y = to_field(field.y * IMU_UGAUSS_FACTOR); 153 field.z = to_field(field.z * IMU_UGAUSS_FACTOR * IMU_MAGNET_Z_XY_RATIO); 154 vectorf_normalise(&field, &field); 155 } 156 } 157 158 pthread_mutex_unlock(&mutex); 159 } 160 161 period = get_period(now, imu_updated); 162 163 if (period >= IMU_UPDATE_PERIOD) 164 { 165 imu_updated = now; 166 167 pthread_mutex_lock(&mutex); 168 169 imu_period = period; 170 171 if (imu_read_vector(IMU_GYRO_ADDRESS, IMU_GYRO_OUT_X_L | IMU_GYRO_READ_MANY, 172 &rotation, convert)) 173 { 174 rotation.x -= rotation0.x; 175 rotation.y -= rotation0.y; 176 rotation.z -= rotation0.z; 177 178 rotation.x = to_angle(rotation.x * IMU_UDPS_FACTOR * period); 179 rotation.y = to_angle(rotation.y * IMU_UDPS_FACTOR * period); 180 rotation.z = to_angle(rotation.z * IMU_UDPS_FACTOR * period); 181 182 plane_rotate(&devicey, &devicez, degrad(rotation.x)); 183 plane_rotate(&devicez, &devicex, degrad(rotation.y)); 184 plane_rotate(&devicex, &devicey, degrad(rotation.z)); 185 } 186 187 if (imu_read_vector(IMU_ACCEL_ADDRESS, IMU_ACCEL_OUT_X_L_A | IMU_ACCEL_READ_MANY, 188 &acceleration, convert12)) 189 { 190 acceleration.x -= acceleration0.x; 191 acceleration.y -= acceleration0.y; 192 acceleration.z -= acceleration0.z; 193 194 /* Convert to g. */ 195 196 acceleration.x /= acceleration1g; 197 acceleration.y /= acceleration1g; 198 acceleration.z /= acceleration1g; 199 200 /* Detect gravitational acceleration. */ 201 202 accelerationM = vectorf_mag(&acceleration); 203 set_reference = (accelerationM > ACCEL_REST_MAG_LOWER) && (accelerationM < ACCEL_REST_MAG_UPPER); 204 205 /* Obtain the acceleration in the global space. */ 206 207 vectorf_convert(&acceleration, &devicex, &devicey, &devicez, &accelerationD); 208 } 209 210 /* Obtain the view axes and device orientation. */ 211 212 vectorf_negate(&devicey, &viewx); 213 vectorf_negate(&devicez, &viewy); 214 vectorf_negate(&devicex, &viewz); 215 216 direction = vectorf_direction(&viewz); 217 elevation = vectorf_elevation(&viewz); 218 tilt = within(-(vectorf_tilt_in_plane(&viewy0, &viewx, &viewy) - M_PI / 2), M_PI); 219 220 /* Reset or update the reference acceleration. */ 221 222 if (set_reference) 223 { 224 accelerationRD = accelerationD; 225 } 226 else 227 { 228 vectorf_rotate_in_space(&accelerationRD, &viewz, &viewy, &viewx, degrad(rotation.y), &accelerationRD); 229 vectorf_rotate_in_space(&accelerationRD, &viewx, &viewy, &viewz, degrad(-rotation.x), &accelerationRD); 230 } 231 232 /* Define the tilt and elevation of the reference acceleration. */ 233 234 elevationA = vectorf_tilt_in_plane(&accelerationRD, &viewy0, &viewz); 235 tiltA = vectorf_tilt_in_plane_with_axis(&accelerationRD, &viewy0, &viewx, &viewz); 236 237 /* Adjust according to elevation. */ 238 239 if (set_reference && (fabs(elevation) < degrad(60))) 240 { 241 plane_rotate(&devicey, &devicez, -tiltA * ROTATION_ADJUSTMENT_FACTOR); 242 plane_rotate(&devicez, &devicex, -elevationA * ROTATION_ADJUSTMENT_FACTOR); 243 244 vectorf_negate(&devicey, &viewx); 245 vectorf_negate(&devicez, &viewy); 246 vectorf_negate(&devicex, &viewz); 247 } 248 249 /* Obtain the magnetic field in the global space. */ 250 251 if (!vectorf_null(&field)) 252 { 253 directionF = vectorf_direction(&field); 254 elevationF = vectorf_elevation(&field); 255 256 /* Define the global vector, remembering the initial value. */ 257 258 vectorf_convert(&field, &devicex, &devicey, &devicez, &fieldD); 259 260 if (!setF0) 261 { 262 fieldD0 = fieldD; 263 setF0 = true; 264 } 265 } 266 267 /* Determine the initial field vector in the current device space. */ 268 269 if (setF0) 270 { 271 vectorf_convert_into(&fieldD0, &devicex, &devicey, &devicez, &field0); 272 directionF0 = vectorf_direction(&field0); 273 elevationF0 = vectorf_elevation(&field0); 274 275 /* Determine the east and north vectors using dynamic field information. */ 276 277 vectorf_cross(&viewy0, &fieldD, &fieldE); 278 vectorf_normalise(&fieldE, &fieldE); 279 vectorf_cross(&fieldE, &viewy0, &fieldN); 280 } 281 282 /* Subtract the constant background acceleration. */ 283 284 if (!using_filter) 285 accelerationD.y -= 1; 286 287 pthread_mutex_unlock(&mutex); 288 } 289 290 usleep(IMU_UPDATE_PERIOD); 291 } 292 293 return NULL; 294 }