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