1.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
1.2 +++ b/main.c Mon Oct 14 13:02:19 2013 +0000
1.3 @@ -0,0 +1,274 @@
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 <stdio.h>
1.19 +#include <signal.h>
1.20 +#include <string.h>
1.21 +#include <sys/time.h>
1.22 +#include <unistd.h>
1.23 +#include <pthread.h>
1.24 +#include "imu.h"
1.25 +#include "shutdown.h"
1.26 +#include "gui.h"
1.27 +#include "ui.h"
1.28 +#include "measure.h"
1.29 +
1.30 +/* Main program. */
1.31 +
1.32 +int main(int argc, char *argv[])
1.33 +{
1.34 + int argno = 1;
1.35 + void *threadresult;
1.36 + uint8_t result[6];
1.37 + bool using_filter = false;
1.38 +
1.39 + /* Local view state. */
1.40 +
1.41 + vectorf _viewx, _viewy, _viewz,
1.42 + _accelerationD, _accelerationRD,
1.43 + _fieldD;
1.44 +
1.45 + /* Timekeeping. */
1.46 +
1.47 + struct timeval now, text_updated, gui_updated;
1.48 +
1.49 + /* Graphical mode variables. */
1.50 +
1.51 + bool graphical = false;
1.52 + int (*print)(const char *, ...) = printf;
1.53 + void (*flush)() = text_flush;
1.54 + void (*clear)() = text_clear;
1.55 + void (*quit)() = text_quit;
1.56 + void (*shutdown)(int) = text_shutdown;
1.57 + void (*shutdown_threaded)(int) = text_shutdown_threaded;
1.58 + imu_ui_op (*handle_events)() = text_handle_events;
1.59 +
1.60 + memset(accelerationB, 0, sizeof(accelerationB));
1.61 +
1.62 + /* Enable a high-pass filter if requested. */
1.63 +
1.64 + if ((argc > argno) && (strcmp(argv[argno], "-f") == 0))
1.65 + {
1.66 + argno++;
1.67 + using_filter = true;
1.68 + }
1.69 +
1.70 + /* Initialise graphical or textual mode. */
1.71 +
1.72 + graphical = (argc > argno) && (strcmp(argv[argno], "-g") == 0);
1.73 +
1.74 + if (graphical)
1.75 + {
1.76 + argno++;
1.77 +
1.78 + print = gui_printf;
1.79 + flush = gui_flush;
1.80 + clear = gui_clear;
1.81 + quit = gui_quit;
1.82 + shutdown = gui_shutdown;
1.83 + shutdown_threaded = gui_shutdown_threaded;
1.84 + handle_events = gui_handle_events;
1.85 +
1.86 + gui_init();
1.87 + gui_display_init();
1.88 + }
1.89 +
1.90 + signal(SIGINT, init_shutdown);
1.91 +
1.92 + /* Access the 8:10 port. */
1.93 +
1.94 + if (ubb_open(0) < 0) {
1.95 + perror("ubb_open");
1.96 + return 1;
1.97 + }
1.98 +
1.99 + ubb_power(1);
1.100 + printf("Power on.\n");
1.101 +
1.102 + /* Bring the IMU up. */
1.103 +
1.104 + imu_init();
1.105 +
1.106 + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG1, IMU_GYRO_CTRL_REG1_ALL);
1.107 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG1_A, IMU_ACCEL_CTRL_REG1_ALL | IMU_ACCEL_FREQ);
1.108 +
1.109 + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG5, 0);
1.110 +
1.111 + if (using_filter)
1.112 + {
1.113 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, IMU_ACCEL_CTRL_REG2_FDS | IMU_ACCEL_FILTER_FREQ);
1.114 + imu_recv(IMU_ACCEL_ADDRESS, IMU_ACCEL_HP_FILTER_RESET_A, result, 1);
1.115 + }
1.116 + else
1.117 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, 0);
1.118 +
1.119 + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG4, IMU_GYRO_CTRL_REG4_BDU | IMU_GYRO_DPS_SCALE);
1.120 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG4_A, IMU_ACCEL_CTRL_REG4_BDU | IMU_ACCEL_SCALE);
1.121 +
1.122 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRA_REG_M, IMU_MAGNET_FREQ);
1.123 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRB_REG_M, IMU_MAGNET_SCALE);
1.124 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_SINGLE);
1.125 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_CONT);
1.126 +
1.127 + usleep(IMU_MAGNET_UPDATE_PERIOD);
1.128 +
1.129 + if (imu_recv(IMU_GYRO_ADDRESS, IMU_GYRO_WHO_AM_I, result, 1))
1.130 + printf("Who am I? %x\n", result[0]);
1.131 +
1.132 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_WHO_AM_I_M, result, 1))
1.133 + printf("Who am I? %x\n", result[0]);
1.134 +
1.135 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRA_REG_M, result, 1))
1.136 + printf("Identification A? %x\n", result[0]);
1.137 +
1.138 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRB_REG_M, result, 1))
1.139 + printf("Identification B? %x\n", result[0]);
1.140 +
1.141 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRC_REG_M, result, 1))
1.142 + printf("Identification C? %x\n", result[0]);
1.143 +
1.144 + /* Get average values for the gyroscope and accelerometer. */
1.145 +
1.146 + if ((argc > argno) && (strcmp(argv[argno], "-c") == 0))
1.147 + {
1.148 + argno++;
1.149 + ui_calibrate(using_filter, print, flush);
1.150 + }
1.151 +
1.152 + /* Reset the acceleration buffer. */
1.153 +
1.154 + memset(accelerationB, 0, sizeof(accelerationB));
1.155 +
1.156 + /* Create a measurement thread. */
1.157 +
1.158 + if (pthread_create(&thread, NULL, get_measurements, &using_filter) != 0)
1.159 + {
1.160 + perror("pthread_create");
1.161 + shutdown(0);
1.162 + }
1.163 +
1.164 + signal(SIGINT, shutdown_threaded);
1.165 +
1.166 + pthread_mutex_init(&mutex, NULL);
1.167 +
1.168 + gettimeofday(&now, NULL);
1.169 + text_updated = now; gui_updated = now;
1.170 +
1.171 + /* Refresh the display by obtaining measurements made in the measurement
1.172 + thread. */
1.173 +
1.174 + while (1)
1.175 + {
1.176 + gettimeofday(&now, NULL);
1.177 +
1.178 + if (get_period(now, text_updated) >= TEXT_UPDATE_PERIOD)
1.179 + {
1.180 + pthread_mutex_lock(&mutex);
1.181 +
1.182 + /* Show textual details. */
1.183 +
1.184 + if (!graphical)
1.185 + {
1.186 + clear();
1.187 + print("Rotation? %.4f, %.4f, %.4f\n", rotation.x, rotation.y, rotation.z);
1.188 + print("Vector? %.4f, %.4f, %.4f\n", viewx.x, viewx.y, viewx.z);
1.189 + print("Vector? %.4f, %.4f, %.4f\n", viewy.x, viewy.y, viewy.z);
1.190 + print("Vector? %.4f, %.4f, %.4f\n", viewz.x, viewz.y, viewz.z);
1.191 + print("Acceleration? %.4f, %.4f, %.4f\n", accelerationD.x, accelerationD.y, accelerationD.z);
1.192 + print("Field vector? %.4f, %.4f, %.4f\n", fieldD.x, fieldD.y, fieldD.z);
1.193 + flush();
1.194 + }
1.195 +
1.196 + printf("Period? %d\n", imu_period);
1.197 + printf("Direction? %.4f\n", raddeg(direction));
1.198 + printf("Elevation? %.4f (%.4f)\n", raddeg(elevation), raddeg(elevationA));
1.199 + printf("Tilt? %.4f (%.4f)\n", raddeg(tilt), raddeg(tiltA));
1.200 + printf("Acceleration? %.4f, %.4f, %.4f\n", accelerationD.x, accelerationD.y, accelerationD.z);
1.201 +
1.202 + pthread_mutex_unlock(&mutex);
1.203 +
1.204 + text_updated = now;
1.205 + }
1.206 +
1.207 + if (graphical && (get_period(now, gui_updated) >= GUI_UPDATE_PERIOD))
1.208 + {
1.209 + pthread_mutex_lock(&mutex);
1.210 + _viewx = viewx;
1.211 + _viewy = viewy;
1.212 + _viewz = viewz;
1.213 + _accelerationD = accelerationD;
1.214 + _accelerationRD = accelerationRD;
1.215 + _fieldD = fieldD;
1.216 + pthread_mutex_unlock(&mutex);
1.217 +
1.218 + clear();
1.219 + gui_sky(&_viewx, &_viewy, &_viewz);
1.220 +
1.221 + _accelerationRD.x *= 10;
1.222 + _accelerationRD.y *= 10;
1.223 + _accelerationRD.z *= 10;
1.224 + gui_point(&_viewx, &_viewy, &_viewz, &_accelerationRD, 0, 255, 0, 127);
1.225 + gui_bar(&_accelerationD);
1.226 +
1.227 + _fieldD.x *= 10;
1.228 + _fieldD.y *= 10;
1.229 + _fieldD.z *= 10;
1.230 + gui_point(&_viewx, &_viewy, &_viewz, &_fieldD, 0, 0, 255, 127);
1.231 +
1.232 + flush();
1.233 +
1.234 + gui_updated = now;
1.235 + }
1.236 +
1.237 + /* Respond to instructions from the UI. */
1.238 +
1.239 + switch (handle_events())
1.240 + {
1.241 + case IMU_UI_OP_QUIT:
1.242 + pthread_cancel(thread);
1.243 + pthread_join(thread, &threadresult);
1.244 + quit();
1.245 + return 0;
1.246 +
1.247 + case IMU_UI_OP_RESET:
1.248 + pthread_mutex_lock(&mutex);
1.249 + devicex = devicex0;
1.250 + devicey = devicey0;
1.251 + devicez = devicez0;
1.252 + vectorf_reset(&accelerationD);
1.253 + pthread_mutex_unlock(&mutex);
1.254 + break;
1.255 +
1.256 + case IMU_UI_OP_CALIBRATE:
1.257 + pthread_mutex_lock(&mutex);
1.258 + clear();
1.259 + ui_calibrate(using_filter, print, flush);
1.260 + flush();
1.261 + memset(accelerationB, 0, sizeof(accelerationB));
1.262 + gettimeofday(&imu_updated, NULL);
1.263 + pthread_mutex_unlock(&mutex);
1.264 + break;
1.265 +
1.266 + default:
1.267 + break;
1.268 + }
1.269 + }
1.270 +
1.271 + /* This should be unreachable. */
1.272 +
1.273 + pthread_cancel(thread);
1.274 + pthread_join(thread, &threadresult);
1.275 + quit();
1.276 + return 0;
1.277 +}