1.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
1.2 +++ b/mtest.c Tue Dec 16 18:07:08 2014 +0000
1.3 @@ -0,0 +1,123 @@
1.4 +/*
1.5 + * Sample Pololu MinIMU-9 measurements.
1.6 + *
1.7 + * Copyright (C) 2013, 2014 Paul Boddie
1.8 + *
1.9 + * This program is free software; you can redistribute it and/or modify
1.10 + * it under the terms of the GNU General Public License as published by
1.11 + * the Free Software Foundation; either version 2 of the License, or
1.12 + * (at your option) any later version.
1.13 + */
1.14 +
1.15 +#include <stdio.h>
1.16 +#include <stdlib.h>
1.17 +#include <signal.h>
1.18 +#include <string.h>
1.19 +#include <unistd.h>
1.20 +#include "imu.h"
1.21 +#include "shutdown.h"
1.22 +#include "geo.h"
1.23 +#include "measure.h"
1.24 +
1.25 +vectorf xymax = {{0, 0, 0}},
1.26 + yzmax = {{0, 0, 0}},
1.27 + xzmax = {{0, 0, 0}};
1.28 +
1.29 +void mtest_shutdown(int signum)
1.30 +{
1.31 + printf("\n%-6.4f %-6.4f %-6.4f %-6.4f %-6.4f %-6.4f\n",
1.32 + xymax.x, xymax.y, yzmax.y, yzmax.z, xzmax.x, xzmax.z);
1.33 + init_shutdown(signum);
1.34 +}
1.35 +
1.36 +/* Main program. */
1.37 +
1.38 +int main(int argc, char *argv[])
1.39 +{
1.40 + uint8_t result[6];
1.41 + vectorf value;
1.42 + double xymag, yzmag, xzmag;
1.43 + double xymagmax = 0, yzmagmax = 0, xzmagmax = 0;
1.44 +
1.45 + signal(SIGINT, mtest_shutdown);
1.46 +
1.47 + /* Access the 8:10 port. */
1.48 +
1.49 + if (ubb_open(0) < 0) {
1.50 + perror("ubb_open");
1.51 + return 1;
1.52 + }
1.53 +
1.54 + ubb_power(1);
1.55 + printf("Power on.\n");
1.56 +
1.57 + /* Bring the IMU up. */
1.58 +
1.59 + imu_init();
1.60 +
1.61 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRA_REG_M, IMU_MAGNET_FREQ);
1.62 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRB_REG_M, IMU_MAGNET_SCALE);
1.63 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_CONT);
1.64 +
1.65 + if (imu_recv(IMU_GYRO_ADDRESS, IMU_GYRO_WHO_AM_I, result, 1))
1.66 + printf("Who am I? %x\n", result[0]);
1.67 +
1.68 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_WHO_AM_I_M, result, 1))
1.69 + printf("Who am I? %x\n", result[0]);
1.70 +
1.71 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRA_REG_M, result, 1))
1.72 + printf("Identification A? %x\n", result[0]);
1.73 +
1.74 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRB_REG_M, result, 1))
1.75 + printf("Identification B? %x\n", result[0]);
1.76 +
1.77 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRC_REG_M, result, 1))
1.78 + printf("Identification C? %x\n", result[0]);
1.79 +
1.80 + /* Initialise the recorded magnetometer limits. */
1.81 +
1.82 + mag_calibrate();
1.83 +
1.84 + while (1)
1.85 + {
1.86 + imu_read_vector_xzy(IMU_MAGNET_ADDRESS, IMU_MAGNET_OUT_X_H_M, &value, convertBE12L);
1.87 + normalise(&value, &fieldmin, &fieldmax, &value);
1.88 +
1.89 + xymag = sqrt(pow(value.x, 2) + pow(value.y, 2));
1.90 + yzmag = sqrt(pow(value.y, 2) + pow(value.z, 2));
1.91 + xzmag = sqrt(pow(value.x, 2) + pow(value.z, 2));
1.92 +
1.93 + if (xymag > xymagmax)
1.94 + {
1.95 + xymagmax = xymag;
1.96 + xymax.x = value.x; xymax.y = value.y;
1.97 + }
1.98 +
1.99 + if (yzmag > yzmagmax)
1.100 + {
1.101 + yzmagmax = yzmag;
1.102 + yzmax.y = value.y; yzmax.z = value.z;
1.103 + }
1.104 +
1.105 + if (xzmag > xzmagmax)
1.106 + {
1.107 + xzmagmax = xzmag;
1.108 + xzmax.x = value.x; xzmax.z = value.z;
1.109 + }
1.110 +
1.111 + printf("V: % 6.4f, % 6.4f, % 6.4f "
1.112 + "XY: % 6.4f "
1.113 + "YZ: % 6.4f "
1.114 + "XZ: % 6.4f\n",
1.115 + value.x, value.y, value.z,
1.116 + xymag, yzmag, xzmag
1.117 + );
1.118 +
1.119 + usleep(IMU_MAGNET_UPDATE_PERIOD);
1.120 + }
1.121 +
1.122 + /* This should be unreachable. */
1.123 +
1.124 + ubb_close(0);
1.125 + return 0;
1.126 +}