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