1 /* 2 * Sample Pololu MinIMU-9 measurements. 3 * 4 * Copyright (C) 2013, 2014 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 #include "measure.h" 21 22 vectorf xymax = {{0, 0, 0}}, 23 yzmax = {{0, 0, 0}}, 24 xzmax = {{0, 0, 0}}; 25 26 void mtest_shutdown(int signum) 27 { 28 printf("\n%-6.4f %-6.4f %-6.4f %-6.4f %-6.4f %-6.4f\n", 29 xymax.x, xymax.y, yzmax.y, yzmax.z, xzmax.x, xzmax.z); 30 init_shutdown(signum); 31 } 32 33 /* Main program. */ 34 35 int main(int argc, char *argv[]) 36 { 37 uint8_t result[6]; 38 vectorf value; 39 double xymag, yzmag, xzmag; 40 double xymagmax = 0, yzmagmax = 0, xzmagmax = 0; 41 42 signal(SIGINT, mtest_shutdown); 43 44 /* Access the 8:10 port. */ 45 46 if (ubb_open(0) < 0) { 47 perror("ubb_open"); 48 return 1; 49 } 50 51 ubb_power(1); 52 printf("Power on.\n"); 53 54 /* Bring the IMU up. */ 55 56 imu_init(); 57 58 imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRA_REG_M, IMU_MAGNET_FREQ); 59 imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRB_REG_M, IMU_MAGNET_SCALE); 60 imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_CONT); 61 62 if (imu_recv(IMU_GYRO_ADDRESS, IMU_GYRO_WHO_AM_I, result, 1)) 63 printf("Who am I? %x\n", result[0]); 64 65 if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_WHO_AM_I_M, result, 1)) 66 printf("Who am I? %x\n", result[0]); 67 68 if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRA_REG_M, result, 1)) 69 printf("Identification A? %x\n", result[0]); 70 71 if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRB_REG_M, result, 1)) 72 printf("Identification B? %x\n", result[0]); 73 74 if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRC_REG_M, result, 1)) 75 printf("Identification C? %x\n", result[0]); 76 77 /* Initialise the recorded magnetometer limits. */ 78 79 mag_calibrate(); 80 81 while (1) 82 { 83 imu_read_vector_xzy(IMU_MAGNET_ADDRESS, IMU_MAGNET_OUT_X_H_M, &value, convertBE12L); 84 normalise(&value, &fieldmin, &fieldmax, &value); 85 86 xymag = sqrt(pow(value.x, 2) + pow(value.y, 2)); 87 yzmag = sqrt(pow(value.y, 2) + pow(value.z, 2)); 88 xzmag = sqrt(pow(value.x, 2) + pow(value.z, 2)); 89 90 if (xymag > xymagmax) 91 { 92 xymagmax = xymag; 93 xymax.x = value.x; xymax.y = value.y; 94 } 95 96 if (yzmag > yzmagmax) 97 { 98 yzmagmax = yzmag; 99 yzmax.y = value.y; yzmax.z = value.z; 100 } 101 102 if (xzmag > xzmagmax) 103 { 104 xzmagmax = xzmag; 105 xzmax.x = value.x; xzmax.z = value.z; 106 } 107 108 printf("V: % 6.4f, % 6.4f, % 6.4f " 109 "XY: % 6.4f " 110 "YZ: % 6.4f " 111 "XZ: % 6.4f\n", 112 value.x, value.y, value.z, 113 xymag, yzmag, xzmag 114 ); 115 116 usleep(IMU_MAGNET_UPDATE_PERIOD); 117 } 118 119 /* This should be unreachable. */ 120 121 ubb_close(0); 122 return 0; 123 }