1 /* 2 * Calibrate 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 "imu.h" 18 #include "shutdown.h" 19 #include "geo.h" 20 21 /* Main program. */ 22 23 int main(int argc, char *argv[]) 24 { 25 uint8_t result[6]; 26 vectorf value, 27 vmin = {{1, 1, 1}}, 28 vmax = {{-1, -1, -1}}, 29 valueB[IMU_ACCEL_BUFFER_SIZE]; 30 int argno = 1, vindex = 0; 31 bool gyroscope = false, accelerometer = false, magnetometer = false, 32 averaging = false, normalised = false; 33 34 if ((argc > argno) && (strcmp(argv[argno], "-g") == 0)) 35 { 36 argno++; 37 gyroscope = true; 38 } 39 else if ((argc > argno) && (strcmp(argv[argno], "-a") == 0)) 40 { 41 argno++; 42 accelerometer = true; 43 } 44 else if ((argc > argno) && (strcmp(argv[argno], "-m") == 0)) 45 { 46 argno++; 47 magnetometer = true; 48 } 49 else 50 { 51 printf("Need -g, -a or -m to select gyroscope, accelerometer or magnetometer respectively.\n"); 52 exit(1); 53 } 54 55 if ((argc > argno) && (strcmp(argv[argno], "-v") == 0)) 56 { 57 argno++; 58 averaging = true; 59 } 60 61 if ((argc > argno) && (strcmp(argv[argno], "-n") == 0)) 62 { 63 argno++; 64 normalised = true; 65 } 66 67 signal(SIGINT, init_shutdown); 68 69 /* Access the 8:10 port. */ 70 71 if (ubb_open(0) < 0) { 72 perror("ubb_open"); 73 return 1; 74 } 75 76 ubb_power(1); 77 printf("Power on.\n"); 78 79 /* Bring the IMU up. */ 80 81 imu_init(); 82 83 if (gyroscope) 84 { 85 imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG1, IMU_GYRO_CTRL_REG1_ALL); 86 imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG5, 0); 87 imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG4, IMU_GYRO_CTRL_REG4_BDU | IMU_GYRO_DPS_SCALE); 88 } 89 else if (accelerometer) 90 { 91 imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG1_A, IMU_ACCEL_CTRL_REG1_ALL | IMU_ACCEL_CTRL_REG1_50HZ); 92 93 if ((argc > argno) && (strcmp(argv[argno], "-f") == 0)) 94 { 95 argno++; 96 imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, IMU_ACCEL_CTRL_REG2_FDS | IMU_ACCEL_FILTER_FREQ); 97 imu_recv(IMU_ACCEL_ADDRESS, IMU_ACCEL_HP_FILTER_RESET_A, result, 1); 98 } 99 else 100 imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, 0); 101 102 imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG4_A, IMU_ACCEL_CTRL_REG4_BDU | IMU_ACCEL_SCALE); 103 } 104 else if (magnetometer) 105 { 106 imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRA_REG_M, IMU_MAGNET_CRA_REG_30HZ); 107 imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRB_REG_M, IMU_MAGNET_CRB_REG_4_0G); 108 imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_SINGLE); 109 imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_CONT); 110 } 111 112 if (imu_recv(IMU_GYRO_ADDRESS, IMU_GYRO_WHO_AM_I, result, 1)) 113 printf("Who am I? %x\n", result[0]); 114 115 if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_WHO_AM_I_M, result, 1)) 116 printf("Who am I? %x\n", result[0]); 117 118 if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRA_REG_M, result, 1)) 119 printf("Identification A? %x\n", result[0]); 120 121 if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRB_REG_M, result, 1)) 122 printf("Identification B? %x\n", result[0]); 123 124 if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRC_REG_M, result, 1)) 125 printf("Identification C? %x\n", result[0]); 126 127 memset(valueB, 0, sizeof(valueB)); 128 129 while (1) 130 { 131 if (accelerometer) 132 imu_read_vector(IMU_ACCEL_ADDRESS, IMU_ACCEL_OUT_X_L_A | IMU_ACCEL_READ_MANY, &value, convert12); 133 else if (gyroscope) 134 imu_read_vector(IMU_GYRO_ADDRESS, IMU_GYRO_OUT_X_L | IMU_GYRO_READ_MANY, &value, convert); 135 else if (magnetometer) 136 imu_read_vector_xzy(IMU_MAGNET_ADDRESS, IMU_MAGNET_OUT_X_H_M, &value, convertBE12L); 137 138 if (averaging) 139 { 140 queue(valueB, &vindex, IMU_ACCEL_BUFFER_SIZE, &value); 141 filter(valueB, vindex, IMU_ACCEL_BUFFER_SIZE, &value); 142 } 143 144 vmin.x = min(value.x, vmin.x); 145 vmin.y = min(value.y, vmin.y); 146 vmin.z = min(value.z, vmin.z); 147 vmax.x = max(value.x, vmax.x); 148 vmax.y = max(value.y, vmax.y); 149 vmax.z = max(value.z, vmax.z); 150 151 printf("V: % 6.1f, % 6.1f, % 6.1f " 152 "-: % 6.1f, % 6.1f, % 6.1f " 153 "+: % 6.1f, % 6.1f, % 6.1f\n", 154 normalised ? scale(value.x, vmin.x, (vmin.x + vmax.x) / 2, vmax.x) : value.x, 155 normalised ? scale(value.y, vmin.y, (vmin.y + vmax.y) / 2, vmax.y) : value.y, 156 normalised ? scale(value.z, vmin.z, (vmin.z + vmax.z) / 2, vmax.z) : value.z, 157 vmin.x, vmin.y, vmin.z, 158 vmax.x, vmax.y, vmax.z); 159 160 usleep(magnetometer ? IMU_MAGNET_UPDATE_PERIOD : IMU_UPDATE_PERIOD); 161 } 162 163 /* This should be unreachable. */ 164 165 ubb_close(0); 166 return 0; 167 }