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