1.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
1.2 +++ b/measure.c Mon Oct 14 13:02:19 2013 +0000
1.3 @@ -0,0 +1,255 @@
1.4 +/*
1.5 + * Ben NanoNote communication with the Pololu MinIMU-9 with the L3G4200D 3-axis
1.6 + * gyroscope and LSM303DLM accelerometer/magnetometer.
1.7 + *
1.8 + * http://www.pololu.com/catalog/product/1265
1.9 + *
1.10 + * Copyright (C) 2013 Paul Boddie
1.11 + *
1.12 + * This program is free software; you can redistribute it and/or modify
1.13 + * it under the terms of the GNU General Public License as published by
1.14 + * the Free Software Foundation; either version 2 of the License, or
1.15 + * (at your option) any later version.
1.16 + */
1.17 +
1.18 +#include <sys/time.h>
1.19 +#include <unistd.h>
1.20 +#include <pthread.h>
1.21 +#include "imu.h"
1.22 +#include "geo.h"
1.23 +
1.24 +#define __MEASURE_H_PRIVATE__
1.25 +#include "measure.h"
1.26 +#undef __MEASURE_H_PRIVATE__
1.27 +
1.28 +static bool setF0 = false;
1.29 +
1.30 +/**
1.31 + * Perform calibration with feedback given in the user interface.
1.32 + */
1.33 +void ui_calibrate(bool using_filter, int (*print)(const char *, ...), void (*flush)())
1.34 +{
1.35 + vectorf tmpB[1];
1.36 +
1.37 + print("Calibrating...\n");
1.38 + flush();
1.39 +
1.40 + /* Calibrate without a filter for rotation. */
1.41 +
1.42 + calibrate(&rotation0, tmpB, 1, IMU_GYRO_ADDRESS, IMU_GYRO_OUT_X_L | IMU_GYRO_READ_MANY, IMU_UPDATE_PERIOD, convert);
1.43 +
1.44 + print("Calibrated using (%.4f, %.4f, %.4f).\n", rotation0.x, rotation0.y, rotation0.z);
1.45 + flush();
1.46 +
1.47 + /* Calibrate using a filter for acceleration. */
1.48 +
1.49 + calibrate(&acceleration0, accelerationB, IMU_ACCEL_BUFFER_SIZE,
1.50 + IMU_ACCEL_ADDRESS, IMU_ACCEL_OUT_X_L_A | IMU_ACCEL_READ_MANY, IMU_UPDATE_PERIOD, convert12);
1.51 +
1.52 + /* Filter out the expected 1g measurement on the z axis. */
1.53 +
1.54 + if (!using_filter)
1.55 + acceleration0.z += acceleration1g;
1.56 +
1.57 + print("Calibrated using (%.4f, %.4f, %.4f).\n", acceleration0.x, acceleration0.y, acceleration0.z);
1.58 + flush();
1.59 +}
1.60 +
1.61 +void *get_measurements(void *arg)
1.62 +{
1.63 + struct timeval now;
1.64 + uint32_t period;
1.65 + bool using_filter = false;
1.66 + double accelerationM;
1.67 + bool set_reference = false;
1.68 +
1.69 + pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
1.70 +
1.71 + if (arg != NULL)
1.72 + using_filter = *((bool *) arg);
1.73 +
1.74 + /* Initialise the default device orientation. */
1.75 +
1.76 + devicex = devicex0;
1.77 + devicey = devicey0;
1.78 + devicez = devicez0;
1.79 +
1.80 + /* Note the time to schedule the next update. */
1.81 +
1.82 + gettimeofday(&imu_updated, NULL);
1.83 + imu_magnet_updated = imu_updated;
1.84 +
1.85 + /* NOTE: Wake up the stupid magnetometer. */
1.86 +
1.87 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_CONT);
1.88 +
1.89 + /* Actual readings. */
1.90 +
1.91 + while (1)
1.92 + {
1.93 + gettimeofday(&now, NULL);
1.94 +
1.95 + period = get_period(now, imu_magnet_updated);
1.96 +
1.97 + if (period >= IMU_MAGNET_UPDATE_PERIOD)
1.98 + {
1.99 + imu_magnet_updated = now;
1.100 +
1.101 + pthread_mutex_lock(&mutex);
1.102 +
1.103 + if (imu_read_vector_xzy(IMU_MAGNET_ADDRESS, IMU_MAGNET_OUT_X_H_M,
1.104 + &field, convertBE12L))
1.105 + {
1.106 + /* NOTE: Handle stupid magnetometer readings. */
1.107 +
1.108 + if (!setF0 && (field.x == 0))
1.109 + {
1.110 + field.y = 0; field.z = 0;
1.111 + }
1.112 + else
1.113 + {
1.114 + normalise(&field, &fieldmin, &fieldmax, &field);
1.115 + field.x = to_field(field.x * IMU_UGAUSS_FACTOR);
1.116 + field.y = to_field(field.y * IMU_UGAUSS_FACTOR);
1.117 + field.z = to_field(field.z * IMU_UGAUSS_FACTOR * IMU_MAGNET_Z_XY_RATIO);
1.118 + vectorf_normalise(&field, &field);
1.119 + }
1.120 + }
1.121 +
1.122 + pthread_mutex_unlock(&mutex);
1.123 + }
1.124 +
1.125 + period = get_period(now, imu_updated);
1.126 +
1.127 + if (period >= IMU_UPDATE_PERIOD)
1.128 + {
1.129 + imu_updated = now;
1.130 +
1.131 + pthread_mutex_lock(&mutex);
1.132 +
1.133 + imu_period = period;
1.134 +
1.135 + if (imu_read_vector(IMU_GYRO_ADDRESS, IMU_GYRO_OUT_X_L | IMU_GYRO_READ_MANY,
1.136 + &rotation, convert))
1.137 + {
1.138 + rotation.x -= rotation0.x;
1.139 + rotation.y -= rotation0.y;
1.140 + rotation.z -= rotation0.z;
1.141 +
1.142 + rotation.x = to_angle(rotation.x * IMU_UDPS_FACTOR * period);
1.143 + rotation.y = to_angle(rotation.y * IMU_UDPS_FACTOR * period);
1.144 + rotation.z = to_angle(rotation.z * IMU_UDPS_FACTOR * period);
1.145 +
1.146 + plane_rotate(&devicey, &devicez, degrad(rotation.x));
1.147 + plane_rotate(&devicez, &devicex, degrad(rotation.y));
1.148 + plane_rotate(&devicex, &devicey, degrad(rotation.z));
1.149 + }
1.150 +
1.151 + if (imu_read_vector(IMU_ACCEL_ADDRESS, IMU_ACCEL_OUT_X_L_A | IMU_ACCEL_READ_MANY,
1.152 + &acceleration, convert12))
1.153 + {
1.154 + acceleration.x -= acceleration0.x;
1.155 + acceleration.y -= acceleration0.y;
1.156 + acceleration.z -= acceleration0.z;
1.157 +
1.158 + /* Convert to g. */
1.159 +
1.160 + acceleration.x /= acceleration1g;
1.161 + acceleration.y /= acceleration1g;
1.162 + acceleration.z /= acceleration1g;
1.163 +
1.164 + /* Detect gravitational acceleration. */
1.165 +
1.166 + accelerationM = vectorf_mag(&acceleration);
1.167 + set_reference = (accelerationM > ACCEL_REST_MAG_LOWER) && (accelerationM < ACCEL_REST_MAG_UPPER);
1.168 +
1.169 + /* Obtain the acceleration in the global space. */
1.170 +
1.171 + vectorf_convert(&acceleration, &devicex, &devicey, &devicez, &accelerationD);
1.172 + }
1.173 +
1.174 + /* Obtain the view axes and device orientation. */
1.175 +
1.176 + vectorf_negate(&devicey, &viewx);
1.177 + vectorf_negate(&devicez, &viewy);
1.178 + vectorf_negate(&devicex, &viewz);
1.179 +
1.180 + direction = vectorf_direction(&viewz);
1.181 + elevation = vectorf_elevation(&viewz);
1.182 + tilt = within(-(vectorf_tilt_in_plane(&viewy0, &viewx, &viewy) - M_PI / 2), M_PI);
1.183 +
1.184 + /* Reset or update the reference acceleration. */
1.185 +
1.186 + if (set_reference)
1.187 + {
1.188 + accelerationRD = accelerationD;
1.189 + }
1.190 + else
1.191 + {
1.192 + vectorf_rotate_in_space(&accelerationRD, &viewz, &viewy, &viewx, degrad(rotation.y), &accelerationRD);
1.193 + vectorf_rotate_in_space(&accelerationRD, &viewx, &viewy, &viewz, degrad(-rotation.x), &accelerationRD);
1.194 + }
1.195 +
1.196 + /* Define the tilt and elevation of the reference acceleration. */
1.197 +
1.198 + elevationA = vectorf_tilt_in_plane(&accelerationRD, &viewy0, &viewz);
1.199 + tiltA = vectorf_tilt_in_plane_with_axis(&accelerationRD, &viewy0, &viewx, &viewz);
1.200 +
1.201 + /* Adjust according to elevation. */
1.202 +
1.203 + if (set_reference && (fabs(elevation) < degrad(60)))
1.204 + {
1.205 + plane_rotate(&devicey, &devicez, -tiltA * ROTATION_ADJUSTMENT_FACTOR);
1.206 + plane_rotate(&devicez, &devicex, -elevationA * ROTATION_ADJUSTMENT_FACTOR);
1.207 +
1.208 + vectorf_negate(&devicey, &viewx);
1.209 + vectorf_negate(&devicez, &viewy);
1.210 + vectorf_negate(&devicex, &viewz);
1.211 + }
1.212 +
1.213 + /* Obtain the magnetic field in the global space. */
1.214 +
1.215 + if (!vectorf_null(&field))
1.216 + {
1.217 + directionF = vectorf_direction(&field);
1.218 + elevationF = vectorf_elevation(&field);
1.219 +
1.220 + /* Define the global vector, remembering the initial value. */
1.221 +
1.222 + vectorf_convert(&field, &devicex, &devicey, &devicez, &fieldD);
1.223 +
1.224 + if (!setF0)
1.225 + {
1.226 + fieldD0 = fieldD;
1.227 + setF0 = true;
1.228 + }
1.229 + }
1.230 +
1.231 + /* Determine the initial field vector in the current device space. */
1.232 +
1.233 + if (setF0)
1.234 + {
1.235 + vectorf_convert_into(&fieldD0, &devicex, &devicey, &devicez, &field0);
1.236 + directionF0 = vectorf_direction(&field0);
1.237 + elevationF0 = vectorf_elevation(&field0);
1.238 +
1.239 + /* Determine the east and north vectors using static field information. */
1.240 +
1.241 + vectorf_cross(&accelerationRD, &fieldD0, &fieldE);
1.242 + vectorf_normalise(&fieldE, &fieldE);
1.243 + vectorf_cross(&fieldE, &accelerationRD, &fieldN);
1.244 + }
1.245 +
1.246 + /* Subtract the constant background acceleration. */
1.247 +
1.248 + if (!using_filter)
1.249 + accelerationD.y -= 1;
1.250 +
1.251 + pthread_mutex_unlock(&mutex);
1.252 + }
1.253 +
1.254 + usleep(IMU_UPDATE_PERIOD);
1.255 + }
1.256 +
1.257 + return NULL;
1.258 +}