1.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
1.2 +++ b/calibrate.c Mon Oct 14 13:02:19 2013 +0000
1.3 @@ -0,0 +1,167 @@
1.4 +/*
1.5 + * Calibrate Pololu MinIMU-9 measurements.
1.6 + *
1.7 + * Copyright (C) 2013 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 +
1.24 +/* Main program. */
1.25 +
1.26 +int main(int argc, char *argv[])
1.27 +{
1.28 + uint8_t result[6];
1.29 + vectorf value,
1.30 + vmin = {{1, 1, 1}},
1.31 + vmax = {{-1, -1, -1}},
1.32 + valueB[IMU_ACCEL_BUFFER_SIZE];
1.33 + int argno = 1, vindex = 0;
1.34 + bool gyroscope = false, accelerometer = false, magnetometer = false,
1.35 + averaging = false, normalised = false;
1.36 +
1.37 + if ((argc > argno) && (strcmp(argv[argno], "-g") == 0))
1.38 + {
1.39 + argno++;
1.40 + gyroscope = true;
1.41 + }
1.42 + else if ((argc > argno) && (strcmp(argv[argno], "-a") == 0))
1.43 + {
1.44 + argno++;
1.45 + accelerometer = true;
1.46 + }
1.47 + else if ((argc > argno) && (strcmp(argv[argno], "-m") == 0))
1.48 + {
1.49 + argno++;
1.50 + magnetometer = true;
1.51 + }
1.52 + else
1.53 + {
1.54 + printf("Need -g, -a or -m to select gyroscope, accelerometer or magnetometer respectively.\n");
1.55 + exit(1);
1.56 + }
1.57 +
1.58 + if ((argc > argno) && (strcmp(argv[argno], "-v") == 0))
1.59 + {
1.60 + argno++;
1.61 + averaging = true;
1.62 + }
1.63 +
1.64 + if ((argc > argno) && (strcmp(argv[argno], "-n") == 0))
1.65 + {
1.66 + argno++;
1.67 + normalised = true;
1.68 + }
1.69 +
1.70 + signal(SIGINT, init_shutdown);
1.71 +
1.72 + /* Access the 8:10 port. */
1.73 +
1.74 + if (ubb_open(0) < 0) {
1.75 + perror("ubb_open");
1.76 + return 1;
1.77 + }
1.78 +
1.79 + ubb_power(1);
1.80 + printf("Power on.\n");
1.81 +
1.82 + /* Bring the IMU up. */
1.83 +
1.84 + imu_init();
1.85 +
1.86 + if (gyroscope)
1.87 + {
1.88 + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG1, IMU_GYRO_CTRL_REG1_ALL);
1.89 + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG5, 0);
1.90 + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG4, IMU_GYRO_CTRL_REG4_BDU | IMU_GYRO_DPS_SCALE);
1.91 + }
1.92 + else if (accelerometer)
1.93 + {
1.94 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG1_A, IMU_ACCEL_CTRL_REG1_ALL | IMU_ACCEL_CTRL_REG1_50HZ);
1.95 +
1.96 + if ((argc > argno) && (strcmp(argv[argno], "-f") == 0))
1.97 + {
1.98 + argno++;
1.99 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, IMU_ACCEL_CTRL_REG2_FDS | IMU_ACCEL_FILTER_FREQ);
1.100 + imu_recv(IMU_ACCEL_ADDRESS, IMU_ACCEL_HP_FILTER_RESET_A, result, 1);
1.101 + }
1.102 + else
1.103 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, 0);
1.104 +
1.105 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG4_A, IMU_ACCEL_CTRL_REG4_BDU | IMU_ACCEL_SCALE);
1.106 + }
1.107 + else if (magnetometer)
1.108 + {
1.109 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRA_REG_M, IMU_MAGNET_CRA_REG_30HZ);
1.110 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRB_REG_M, IMU_MAGNET_CRB_REG_4_0G);
1.111 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_SINGLE);
1.112 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_CONT);
1.113 + }
1.114 +
1.115 + if (imu_recv(IMU_GYRO_ADDRESS, IMU_GYRO_WHO_AM_I, result, 1))
1.116 + printf("Who am I? %x\n", result[0]);
1.117 +
1.118 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_WHO_AM_I_M, result, 1))
1.119 + printf("Who am I? %x\n", result[0]);
1.120 +
1.121 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRA_REG_M, result, 1))
1.122 + printf("Identification A? %x\n", result[0]);
1.123 +
1.124 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRB_REG_M, result, 1))
1.125 + printf("Identification B? %x\n", result[0]);
1.126 +
1.127 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRC_REG_M, result, 1))
1.128 + printf("Identification C? %x\n", result[0]);
1.129 +
1.130 + memset(valueB, 0, sizeof(valueB));
1.131 +
1.132 + while (1)
1.133 + {
1.134 + if (accelerometer)
1.135 + imu_read_vector(IMU_ACCEL_ADDRESS, IMU_ACCEL_OUT_X_L_A | IMU_ACCEL_READ_MANY, &value, convert12);
1.136 + else if (gyroscope)
1.137 + imu_read_vector(IMU_GYRO_ADDRESS, IMU_GYRO_OUT_X_L | IMU_GYRO_READ_MANY, &value, convert);
1.138 + else if (magnetometer)
1.139 + imu_read_vector_xzy(IMU_MAGNET_ADDRESS, IMU_MAGNET_OUT_X_H_M, &value, convertBE12L);
1.140 +
1.141 + if (averaging)
1.142 + {
1.143 + queue(valueB, &vindex, IMU_ACCEL_BUFFER_SIZE, &value);
1.144 + filter(valueB, vindex, IMU_ACCEL_BUFFER_SIZE, &value);
1.145 + }
1.146 +
1.147 + vmin.x = min(value.x, vmin.x);
1.148 + vmin.y = min(value.y, vmin.y);
1.149 + vmin.z = min(value.z, vmin.z);
1.150 + vmax.x = max(value.x, vmax.x);
1.151 + vmax.y = max(value.y, vmax.y);
1.152 + vmax.z = max(value.z, vmax.z);
1.153 +
1.154 + printf("V: % 6.1f, % 6.1f, % 6.1f "
1.155 + "-: % 6.1f, % 6.1f, % 6.1f "
1.156 + "+: % 6.1f, % 6.1f, % 6.1f\n",
1.157 + normalised ? scale(value.x, vmin.x, (vmin.x + vmax.x) / 2, vmax.x) : value.x,
1.158 + normalised ? scale(value.y, vmin.y, (vmin.y + vmax.y) / 2, vmax.y) : value.y,
1.159 + normalised ? scale(value.z, vmin.z, (vmin.z + vmax.z) / 2, vmax.z) : value.z,
1.160 + vmin.x, vmin.y, vmin.z,
1.161 + vmax.x, vmax.y, vmax.z);
1.162 +
1.163 + usleep(magnetometer ? IMU_MAGNET_UPDATE_PERIOD : IMU_UPDATE_PERIOD);
1.164 + }
1.165 +
1.166 + /* This should be unreachable. */
1.167 +
1.168 + ubb_close(0);
1.169 + return 0;
1.170 +}