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