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