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_CONT); 122 123 usleep(IMU_MAGNET_UPDATE_PERIOD); 124 125 if (imu_recv(IMU_GYRO_ADDRESS, IMU_GYRO_WHO_AM_I, result, 1)) 126 printf("Who am I? %x\n", result[0]); 127 128 if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_WHO_AM_I_M, result, 1)) 129 printf("Who am I? %x\n", result[0]); 130 131 if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRA_REG_M, result, 1)) 132 printf("Identification A? %x\n", result[0]); 133 134 if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRB_REG_M, result, 1)) 135 printf("Identification B? %x\n", result[0]); 136 137 if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRC_REG_M, result, 1)) 138 printf("Identification C? %x\n", result[0]); 139 140 /* Get average values for the gyroscope and accelerometer. */ 141 142 if ((argc > argno) && (strcmp(argv[argno], "-c") == 0)) 143 { 144 argno++; 145 ui_calibrate(using_filter, print, flush); 146 } 147 148 /* Reset the acceleration buffer. */ 149 150 memset(accelerationB, 0, sizeof(accelerationB)); 151 152 /* Create a measurement thread. */ 153 154 if (pthread_create(&thread, NULL, get_measurements, &using_filter) != 0) 155 { 156 perror("pthread_create"); 157 shutdown(0); 158 } 159 160 signal(SIGINT, shutdown_threaded); 161 162 pthread_mutex_init(&mutex, NULL); 163 164 gettimeofday(&now, NULL); 165 text_updated = now; gui_updated = now; 166 167 /* Refresh the display by obtaining measurements made in the measurement 168 thread. */ 169 170 while (1) 171 { 172 gettimeofday(&now, NULL); 173 174 if (get_period(now, text_updated) >= TEXT_UPDATE_PERIOD) 175 { 176 pthread_mutex_lock(&mutex); 177 178 /* Show textual details. */ 179 180 if (!graphical) 181 { 182 clear(); 183 print("Rotation? %.4f, %.4f, %.4f\n", rotation.x, rotation.y, rotation.z); 184 print("Vector? %.4f, %.4f, %.4f\n", viewx.x, viewx.y, viewx.z); 185 print("Vector? %.4f, %.4f, %.4f\n", viewy.x, viewy.y, viewy.z); 186 print("Vector? %.4f, %.4f, %.4f\n", viewz.x, viewz.y, viewz.z); 187 print("Acceleration? %.4f, %.4f, %.4f\n", accelerationD.x, accelerationD.y, accelerationD.z); 188 print("Field vector? %.4f, %.4f, %.4f\n", fieldD.x, fieldD.y, fieldD.z); 189 flush(); 190 } 191 192 /* Additional textual output. */ 193 194 printf("Period? %d\n", imu_period); 195 printf("Direction? %.4f\n", raddeg(direction)); 196 printf("Elevation? %.4f (%.4f)\n", raddeg(elevation), raddeg(elevationA)); 197 printf("Tilt? %.4f (%.4f)\n", raddeg(tilt), raddeg(tiltA)); 198 printf("Acceleration? %.4f, %.4f, %.4f\n", accelerationD.x, accelerationD.y, accelerationD.z); 199 200 pthread_mutex_unlock(&mutex); 201 202 text_updated = now; 203 } 204 205 if (graphical && (get_period(now, gui_updated) >= GUI_UPDATE_PERIOD)) 206 { 207 pthread_mutex_lock(&mutex); 208 _viewx = viewx; 209 _viewy = viewy; 210 _viewz = viewz; 211 _accelerationD = accelerationD; 212 _accelerationRD = accelerationRD; 213 _fieldD = fieldD; 214 _fieldN = fieldN; 215 pthread_mutex_unlock(&mutex); 216 217 clear(); 218 gui_sky(&_viewx, &_viewy, &_viewz); 219 220 _accelerationRD.x *= 10; 221 _accelerationRD.y *= 10; 222 _accelerationRD.z *= 10; 223 gui_point(&_viewx, &_viewy, &_viewz, &_accelerationRD, 0, 255, 0, 127); 224 gui_bar(&_accelerationD); 225 226 _fieldD.x *= 10; 227 _fieldD.y *= 10; 228 _fieldD.z *= 10; 229 gui_point(&_viewx, &_viewy, &_viewz, &_fieldD, 0, 0, 255, 127); 230 231 _fieldN.x *= 10; 232 _fieldN.y *= 10; 233 _fieldN.z *= 10; 234 gui_point(&_viewx, &_viewy, &_viewz, &_fieldN, 255, 0, 0, 127); 235 236 flush(); 237 238 gui_updated = now; 239 } 240 241 /* Respond to instructions from the UI. */ 242 243 switch (handle_events()) 244 { 245 case IMU_UI_OP_QUIT: 246 pthread_cancel(thread); 247 pthread_join(thread, &threadresult); 248 quit(); 249 return 0; 250 251 case IMU_UI_OP_RESET: 252 pthread_mutex_lock(&mutex); 253 devicex = devicex0; 254 devicey = devicey0; 255 devicez = devicez0; 256 vectorf_reset(&accelerationD); 257 pthread_mutex_unlock(&mutex); 258 break; 259 260 case IMU_UI_OP_CALIBRATE: 261 pthread_mutex_lock(&mutex); 262 clear(); 263 ui_calibrate(using_filter, print, flush); 264 flush(); 265 memset(accelerationB, 0, sizeof(accelerationB)); 266 gettimeofday(&imu_updated, NULL); 267 pthread_mutex_unlock(&mutex); 268 break; 269 270 default: 271 break; 272 } 273 } 274 275 /* This should be unreachable. */ 276 277 pthread_cancel(thread); 278 pthread_join(thread, &threadresult); 279 quit(); 280 return 0; 281 }