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