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@0 | 15 | #include <stdio.h> |
paul@0 | 16 | #include <signal.h> |
paul@0 | 17 | #include <string.h> |
paul@0 | 18 | #include <sys/time.h> |
paul@0 | 19 | #include <unistd.h> |
paul@0 | 20 | #include <pthread.h> |
paul@0 | 21 | #include "imu.h" |
paul@0 | 22 | #include "shutdown.h" |
paul@0 | 23 | #include "gui.h" |
paul@0 | 24 | #include "ui.h" |
paul@0 | 25 | #include "measure.h" |
paul@0 | 26 | |
paul@0 | 27 | /* Main program. */ |
paul@0 | 28 | |
paul@0 | 29 | int main(int argc, char *argv[]) |
paul@0 | 30 | { |
paul@0 | 31 | int argno = 1; |
paul@0 | 32 | void *threadresult; |
paul@0 | 33 | uint8_t result[6]; |
paul@0 | 34 | bool using_filter = false; |
paul@0 | 35 | |
paul@0 | 36 | /* Local view state. */ |
paul@0 | 37 | |
paul@0 | 38 | vectorf _viewx, _viewy, _viewz, |
paul@0 | 39 | _accelerationD, _accelerationRD, |
paul@4 | 40 | _fieldD, _fieldN; |
paul@0 | 41 | |
paul@0 | 42 | /* Timekeeping. */ |
paul@0 | 43 | |
paul@0 | 44 | struct timeval now, text_updated, gui_updated; |
paul@0 | 45 | |
paul@0 | 46 | /* Graphical mode variables. */ |
paul@0 | 47 | |
paul@0 | 48 | bool graphical = false; |
paul@0 | 49 | int (*print)(const char *, ...) = printf; |
paul@0 | 50 | void (*flush)() = text_flush; |
paul@0 | 51 | void (*clear)() = text_clear; |
paul@0 | 52 | void (*quit)() = text_quit; |
paul@0 | 53 | void (*shutdown)(int) = text_shutdown; |
paul@0 | 54 | void (*shutdown_threaded)(int) = text_shutdown_threaded; |
paul@0 | 55 | imu_ui_op (*handle_events)() = text_handle_events; |
paul@0 | 56 | |
paul@0 | 57 | memset(accelerationB, 0, sizeof(accelerationB)); |
paul@0 | 58 | |
paul@0 | 59 | /* Enable a high-pass filter if requested. */ |
paul@0 | 60 | |
paul@0 | 61 | if ((argc > argno) && (strcmp(argv[argno], "-f") == 0)) |
paul@0 | 62 | { |
paul@0 | 63 | argno++; |
paul@0 | 64 | using_filter = true; |
paul@0 | 65 | } |
paul@0 | 66 | |
paul@0 | 67 | /* Initialise graphical or textual mode. */ |
paul@0 | 68 | |
paul@0 | 69 | graphical = (argc > argno) && (strcmp(argv[argno], "-g") == 0); |
paul@0 | 70 | |
paul@0 | 71 | if (graphical) |
paul@0 | 72 | { |
paul@0 | 73 | argno++; |
paul@0 | 74 | |
paul@0 | 75 | print = gui_printf; |
paul@0 | 76 | flush = gui_flush; |
paul@0 | 77 | clear = gui_clear; |
paul@0 | 78 | quit = gui_quit; |
paul@0 | 79 | shutdown = gui_shutdown; |
paul@0 | 80 | shutdown_threaded = gui_shutdown_threaded; |
paul@0 | 81 | handle_events = gui_handle_events; |
paul@0 | 82 | |
paul@0 | 83 | gui_init(); |
paul@0 | 84 | gui_display_init(); |
paul@0 | 85 | } |
paul@0 | 86 | |
paul@0 | 87 | signal(SIGINT, init_shutdown); |
paul@0 | 88 | |
paul@0 | 89 | /* Access the 8:10 port. */ |
paul@0 | 90 | |
paul@0 | 91 | if (ubb_open(0) < 0) { |
paul@0 | 92 | perror("ubb_open"); |
paul@0 | 93 | return 1; |
paul@0 | 94 | } |
paul@0 | 95 | |
paul@0 | 96 | ubb_power(1); |
paul@0 | 97 | printf("Power on.\n"); |
paul@0 | 98 | |
paul@0 | 99 | /* Bring the IMU up. */ |
paul@0 | 100 | |
paul@0 | 101 | imu_init(); |
paul@0 | 102 | |
paul@0 | 103 | imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG1, IMU_GYRO_CTRL_REG1_ALL); |
paul@0 | 104 | imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG1_A, IMU_ACCEL_CTRL_REG1_ALL | IMU_ACCEL_FREQ); |
paul@0 | 105 | |
paul@0 | 106 | imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG5, 0); |
paul@0 | 107 | |
paul@0 | 108 | if (using_filter) |
paul@0 | 109 | { |
paul@0 | 110 | imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, IMU_ACCEL_CTRL_REG2_FDS | IMU_ACCEL_FILTER_FREQ); |
paul@0 | 111 | imu_recv(IMU_ACCEL_ADDRESS, IMU_ACCEL_HP_FILTER_RESET_A, result, 1); |
paul@0 | 112 | } |
paul@0 | 113 | else |
paul@0 | 114 | imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, 0); |
paul@0 | 115 | |
paul@0 | 116 | imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG4, IMU_GYRO_CTRL_REG4_BDU | IMU_GYRO_DPS_SCALE); |
paul@0 | 117 | imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG4_A, IMU_ACCEL_CTRL_REG4_BDU | IMU_ACCEL_SCALE); |
paul@0 | 118 | |
paul@0 | 119 | imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRA_REG_M, IMU_MAGNET_FREQ); |
paul@0 | 120 | imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRB_REG_M, IMU_MAGNET_SCALE); |
paul@0 | 121 | imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_CONT); |
paul@0 | 122 | |
paul@0 | 123 | usleep(IMU_MAGNET_UPDATE_PERIOD); |
paul@0 | 124 | |
paul@0 | 125 | if (imu_recv(IMU_GYRO_ADDRESS, IMU_GYRO_WHO_AM_I, result, 1)) |
paul@0 | 126 | printf("Who am I? %x\n", result[0]); |
paul@0 | 127 | |
paul@0 | 128 | if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_WHO_AM_I_M, result, 1)) |
paul@0 | 129 | printf("Who am I? %x\n", result[0]); |
paul@0 | 130 | |
paul@0 | 131 | if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRA_REG_M, result, 1)) |
paul@0 | 132 | printf("Identification A? %x\n", result[0]); |
paul@0 | 133 | |
paul@0 | 134 | if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRB_REG_M, result, 1)) |
paul@0 | 135 | printf("Identification B? %x\n", result[0]); |
paul@0 | 136 | |
paul@0 | 137 | if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRC_REG_M, result, 1)) |
paul@0 | 138 | printf("Identification C? %x\n", result[0]); |
paul@0 | 139 | |
paul@0 | 140 | /* Get average values for the gyroscope and accelerometer. */ |
paul@0 | 141 | |
paul@0 | 142 | if ((argc > argno) && (strcmp(argv[argno], "-c") == 0)) |
paul@0 | 143 | { |
paul@0 | 144 | argno++; |
paul@0 | 145 | ui_calibrate(using_filter, print, flush); |
paul@0 | 146 | } |
paul@0 | 147 | |
paul@0 | 148 | /* Reset the acceleration buffer. */ |
paul@0 | 149 | |
paul@0 | 150 | memset(accelerationB, 0, sizeof(accelerationB)); |
paul@0 | 151 | |
paul@0 | 152 | /* Create a measurement thread. */ |
paul@0 | 153 | |
paul@0 | 154 | if (pthread_create(&thread, NULL, get_measurements, &using_filter) != 0) |
paul@0 | 155 | { |
paul@0 | 156 | perror("pthread_create"); |
paul@0 | 157 | shutdown(0); |
paul@0 | 158 | } |
paul@0 | 159 | |
paul@0 | 160 | signal(SIGINT, shutdown_threaded); |
paul@0 | 161 | |
paul@0 | 162 | pthread_mutex_init(&mutex, NULL); |
paul@0 | 163 | |
paul@0 | 164 | gettimeofday(&now, NULL); |
paul@0 | 165 | text_updated = now; gui_updated = now; |
paul@0 | 166 | |
paul@0 | 167 | /* Refresh the display by obtaining measurements made in the measurement |
paul@0 | 168 | thread. */ |
paul@0 | 169 | |
paul@0 | 170 | while (1) |
paul@0 | 171 | { |
paul@0 | 172 | gettimeofday(&now, NULL); |
paul@0 | 173 | |
paul@0 | 174 | if (get_period(now, text_updated) >= TEXT_UPDATE_PERIOD) |
paul@0 | 175 | { |
paul@0 | 176 | pthread_mutex_lock(&mutex); |
paul@0 | 177 | |
paul@0 | 178 | /* Show textual details. */ |
paul@0 | 179 | |
paul@0 | 180 | if (!graphical) |
paul@0 | 181 | { |
paul@0 | 182 | clear(); |
paul@0 | 183 | print("Rotation? %.4f, %.4f, %.4f\n", rotation.x, rotation.y, rotation.z); |
paul@0 | 184 | print("Vector? %.4f, %.4f, %.4f\n", viewx.x, viewx.y, viewx.z); |
paul@0 | 185 | print("Vector? %.4f, %.4f, %.4f\n", viewy.x, viewy.y, viewy.z); |
paul@0 | 186 | print("Vector? %.4f, %.4f, %.4f\n", viewz.x, viewz.y, viewz.z); |
paul@0 | 187 | print("Acceleration? %.4f, %.4f, %.4f\n", accelerationD.x, accelerationD.y, accelerationD.z); |
paul@0 | 188 | print("Field vector? %.4f, %.4f, %.4f\n", fieldD.x, fieldD.y, fieldD.z); |
paul@0 | 189 | flush(); |
paul@0 | 190 | } |
paul@0 | 191 | |
paul@4 | 192 | /* Additional textual output. */ |
paul@4 | 193 | |
paul@0 | 194 | printf("Period? %d\n", imu_period); |
paul@0 | 195 | printf("Direction? %.4f\n", raddeg(direction)); |
paul@0 | 196 | printf("Elevation? %.4f (%.4f)\n", raddeg(elevation), raddeg(elevationA)); |
paul@0 | 197 | printf("Tilt? %.4f (%.4f)\n", raddeg(tilt), raddeg(tiltA)); |
paul@0 | 198 | printf("Acceleration? %.4f, %.4f, %.4f\n", accelerationD.x, accelerationD.y, accelerationD.z); |
paul@0 | 199 | |
paul@0 | 200 | pthread_mutex_unlock(&mutex); |
paul@0 | 201 | |
paul@0 | 202 | text_updated = now; |
paul@0 | 203 | } |
paul@0 | 204 | |
paul@0 | 205 | if (graphical && (get_period(now, gui_updated) >= GUI_UPDATE_PERIOD)) |
paul@0 | 206 | { |
paul@0 | 207 | pthread_mutex_lock(&mutex); |
paul@0 | 208 | _viewx = viewx; |
paul@0 | 209 | _viewy = viewy; |
paul@0 | 210 | _viewz = viewz; |
paul@0 | 211 | _accelerationD = accelerationD; |
paul@0 | 212 | _accelerationRD = accelerationRD; |
paul@0 | 213 | _fieldD = fieldD; |
paul@4 | 214 | _fieldN = fieldN; |
paul@0 | 215 | pthread_mutex_unlock(&mutex); |
paul@0 | 216 | |
paul@0 | 217 | clear(); |
paul@0 | 218 | gui_sky(&_viewx, &_viewy, &_viewz); |
paul@0 | 219 | |
paul@0 | 220 | _accelerationRD.x *= 10; |
paul@0 | 221 | _accelerationRD.y *= 10; |
paul@0 | 222 | _accelerationRD.z *= 10; |
paul@0 | 223 | gui_point(&_viewx, &_viewy, &_viewz, &_accelerationRD, 0, 255, 0, 127); |
paul@0 | 224 | gui_bar(&_accelerationD); |
paul@0 | 225 | |
paul@0 | 226 | _fieldD.x *= 10; |
paul@0 | 227 | _fieldD.y *= 10; |
paul@0 | 228 | _fieldD.z *= 10; |
paul@0 | 229 | gui_point(&_viewx, &_viewy, &_viewz, &_fieldD, 0, 0, 255, 127); |
paul@0 | 230 | |
paul@4 | 231 | _fieldN.x *= 10; |
paul@4 | 232 | _fieldN.y *= 10; |
paul@4 | 233 | _fieldN.z *= 10; |
paul@4 | 234 | gui_point(&_viewx, &_viewy, &_viewz, &_fieldN, 255, 0, 0, 127); |
paul@4 | 235 | |
paul@0 | 236 | flush(); |
paul@0 | 237 | |
paul@0 | 238 | gui_updated = now; |
paul@0 | 239 | } |
paul@0 | 240 | |
paul@0 | 241 | /* Respond to instructions from the UI. */ |
paul@0 | 242 | |
paul@0 | 243 | switch (handle_events()) |
paul@0 | 244 | { |
paul@0 | 245 | case IMU_UI_OP_QUIT: |
paul@0 | 246 | pthread_cancel(thread); |
paul@0 | 247 | pthread_join(thread, &threadresult); |
paul@0 | 248 | quit(); |
paul@0 | 249 | return 0; |
paul@0 | 250 | |
paul@0 | 251 | case IMU_UI_OP_RESET: |
paul@0 | 252 | pthread_mutex_lock(&mutex); |
paul@0 | 253 | devicex = devicex0; |
paul@0 | 254 | devicey = devicey0; |
paul@0 | 255 | devicez = devicez0; |
paul@0 | 256 | vectorf_reset(&accelerationD); |
paul@0 | 257 | pthread_mutex_unlock(&mutex); |
paul@0 | 258 | break; |
paul@0 | 259 | |
paul@0 | 260 | case IMU_UI_OP_CALIBRATE: |
paul@0 | 261 | pthread_mutex_lock(&mutex); |
paul@0 | 262 | clear(); |
paul@0 | 263 | ui_calibrate(using_filter, print, flush); |
paul@0 | 264 | flush(); |
paul@0 | 265 | memset(accelerationB, 0, sizeof(accelerationB)); |
paul@0 | 266 | gettimeofday(&imu_updated, NULL); |
paul@0 | 267 | pthread_mutex_unlock(&mutex); |
paul@0 | 268 | break; |
paul@0 | 269 | |
paul@0 | 270 | default: |
paul@0 | 271 | break; |
paul@0 | 272 | } |
paul@0 | 273 | } |
paul@0 | 274 | |
paul@0 | 275 | /* This should be unreachable. */ |
paul@0 | 276 | |
paul@0 | 277 | pthread_cancel(thread); |
paul@0 | 278 | pthread_join(thread, &threadresult); |
paul@0 | 279 | quit(); |
paul@0 | 280 | return 0; |
paul@0 | 281 | } |