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