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