1 /* 2 * Test Pololu MinIMU-9 measurements. 3 * 4 * Copyright (C) 2013 Paul Boddie 5 * 6 * This program is free software; you can redistribute it and/or modify 7 * it under the terms of the GNU General Public License as published by 8 * the Free Software Foundation; either version 2 of the License, or 9 * (at your option) any later version. 10 */ 11 12 #include <stdio.h> 13 #include <stdlib.h> 14 #include <signal.h> 15 #include <string.h> 16 #include <unistd.h> 17 #include <sys/time.h> 18 #include <pthread.h> 19 #include "imu.h" 20 #include "shutdown.h" 21 #include "gui.h" 22 #include "measure.h" 23 24 int crosses_axis(double current, double previous) 25 { 26 if ((current == 0) && (previous != 0)) 27 return -sign(previous); 28 if ((current != previous) && (sign(current) == -sign(previous))) 29 return sign(current); 30 return 0; 31 } 32 33 /* Main program. */ 34 35 int main(int argc, char *argv[]) 36 { 37 void *threadresult; 38 uint8_t result[6]; 39 int readings; 40 uint32_t period; 41 struct timeval now, ui_updated; 42 bool paused = false, using_filter = false; 43 44 signal(SIGINT, init_shutdown); 45 46 /* Access the 8:10 port. */ 47 48 if (ubb_open(0) < 0) { 49 perror("ubb_open"); 50 return 1; 51 } 52 53 ubb_power(1); 54 printf("Power on.\n"); 55 56 /* Bring the IMU up. */ 57 58 imu_init(); 59 60 imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG1, IMU_GYRO_CTRL_REG1_ALL); 61 imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG1_A, IMU_ACCEL_CTRL_REG1_ALL | IMU_ACCEL_CTRL_REG1_50HZ); 62 63 imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG5, 0); 64 65 if ((argc > 1) && (strcmp(argv[1], "-f") == 0)) 66 { 67 imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, IMU_ACCEL_CTRL_REG2_FDS | IMU_ACCEL_FILTER_FREQ); 68 imu_recv(IMU_ACCEL_ADDRESS, IMU_ACCEL_HP_FILTER_RESET_A, result, 1); 69 using_filter = true; 70 } 71 else 72 imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, 0); 73 74 imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG4, IMU_GYRO_CTRL_REG4_BDU | IMU_GYRO_DPS_SCALE); 75 imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG4_A, IMU_ACCEL_CTRL_REG4_BDU | IMU_ACCEL_SCALE); 76 77 if (imu_recv(IMU_GYRO_ADDRESS, IMU_GYRO_WHO_AM_I, result, 1)) 78 printf("Who am I? %x\n", result[0]); 79 80 if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_WHO_AM_I_M, result, 1)) 81 printf("Who am I? %x\n", result[0]); 82 83 if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRA_REG_M, result, 1)) 84 printf("Identification A? %x\n", result[0]); 85 86 if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRB_REG_M, result, 1)) 87 printf("Identification B? %x\n", result[0]); 88 89 if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRC_REG_M, result, 1)) 90 printf("Identification C? %x\n", result[0]); 91 92 gui_init(); 93 gui_display_init(); 94 95 signal(SIGINT, gui_shutdown); 96 97 memset(accelerationB, 0, sizeof(accelerationB)); 98 99 ui_calibrate(using_filter, gui_printf, gui_flush); 100 101 memset(accelerationB, 0, sizeof(accelerationB)); 102 103 readings = 0; 104 gui_clear(); 105 gui_printf("A0: % 8.1f, % 8.1f, % 8.1f\n", acceleration0.x, acceleration0.y, acceleration0.z); 106 107 /* Create a measurement thread. */ 108 109 if (pthread_create(&thread, NULL, get_measurements, &using_filter) != 0) 110 { 111 perror("pthread_create"); 112 gui_shutdown(0); 113 } 114 115 signal(SIGINT, gui_shutdown_threaded); 116 117 pthread_mutex_init(&mutex, NULL); 118 119 gettimeofday(&ui_updated, NULL); 120 121 while (1) 122 { 123 gettimeofday(&now, NULL); 124 125 period = get_period(now, ui_updated); 126 127 if ((period >= IMU_UPDATE_PERIOD) && (!paused)) 128 { 129 ui_updated = now; 130 readings = (readings + 1) % SCREEN_WIDTH; 131 132 if (readings == 0) 133 gui_clear(); 134 135 pthread_mutex_lock(&mutex); 136 137 gui_plot(readings % SCREEN_WIDTH, SCREEN_HEIGHT / 2 * (1 - acceleration.x), 255, 0, 0, 255); 138 gui_plot(readings % SCREEN_WIDTH, SCREEN_HEIGHT / 2 * (1 - acceleration.y), 0, 255, 0, 255); 139 gui_plot(readings % SCREEN_WIDTH, SCREEN_HEIGHT / 2 * (1 - acceleration.z), 0, 0, 255, 255); 140 141 gui_flush(); 142 143 printf("A: % 6.4f, % 6.4f, % 6.4f ", 144 acceleration.x, acceleration.y, acceleration.z); 145 146 printf("DET: % 6.4f, % 6.4f, % 6.4f\n", direction, elevation, tilt); 147 148 pthread_mutex_unlock(&mutex); 149 } 150 151 switch (gui_handle_events()) 152 { 153 case IMU_UI_OP_QUIT: 154 pthread_cancel(thread); 155 pthread_join(thread, &threadresult); 156 gui_quit(); 157 return 0; 158 159 case IMU_UI_OP_PAUSE: 160 paused = !paused; 161 break; 162 163 default: 164 break; 165 } 166 167 usleep(IMU_UPDATE_PERIOD); 168 } 169 170 /* This should be unreachable. */ 171 172 pthread_cancel(thread); 173 pthread_join(thread, &threadresult); 174 gui_quit(); 175 return 0; 176 }