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