ben-pololu-imu

Changeset

11:e7c0bbc0a71e
2014-12-16 Paul Boddie raw files shortlog changelog graph Added a magnetometer test program and updated the Makefile. default tip
Makefile (file) mtest.c (file)
     1.1 --- a/Makefile	Tue Dec 16 18:04:56 2014 +0000
     1.2 +++ b/Makefile	Tue Dec 16 18:07:08 2014 +0000
     1.3 @@ -18,9 +18,10 @@
     1.4  LDFLAGS = -lm -lubb -L$(LIBUBB) $(shell $(SYSROOT)/usr/bin/sdl-config --libs) -Wl,-rpath-link,$(SYSROOT)/usr/lib -lSDL_gfx
     1.5  
     1.6  IMU = imu
     1.7 -TEST = itest
     1.8 +ITEST = itest
     1.9 +MTEST = mtest
    1.10  CALIBRATE = calibrate
    1.11 -TARGETS = $(IMU) $(TEST) $(CALIBRATE)
    1.12 +TARGETS = $(IMU) $(ITEST) $(MTEST) $(CALIBRATE)
    1.13  
    1.14  BASICSRC = imu.c i2c.c shutdown.c geo.c
    1.15  UISRC = measure.c ui.c gui.c
    1.16 @@ -28,13 +29,16 @@
    1.17  IMUSRC = $(BASICSRC) $(UISRC) main.c
    1.18  IMUOBJ = $(IMUSRC:.c=.o)
    1.19  
    1.20 -TESTSRC = $(BASICSRC) $(UISRC) itest.c
    1.21 -TESTOBJ = $(TESTSRC:.c=.o)
    1.22 +ITESTSRC = $(BASICSRC) $(UISRC) itest.c
    1.23 +ITESTOBJ = $(ITESTSRC:.c=.o)
    1.24 +
    1.25 +MTESTSRC = $(BASICSRC) $(UISRC) mtest.c
    1.26 +MTESTOBJ = $(MTESTSRC:.c=.o)
    1.27  
    1.28  CALIBRATESRC = $(BASICSRC) calibrate.c
    1.29  CALIBRATEOBJ = $(CALIBRATESRC:.c=.o)
    1.30  
    1.31 -ALLSRC = $(BASICSRC) $(UISRC) main.c itest.c calibrate.c
    1.32 +ALLSRC = $(BASICSRC) $(UISRC) main.c itest.c mtest.c calibrate.c
    1.33  ALLOBJ = $(ALLSRC:.c=.o)
    1.34  
    1.35  .PHONY:	all clean distclean
    1.36 @@ -50,8 +54,11 @@
    1.37  $(IMU): $(IMUOBJ)
    1.38  	$(CC) $(LDFLAGS) $(IMUOBJ) -o $@
    1.39  
    1.40 -$(TEST): $(TESTOBJ)
    1.41 -	$(CC) $(LDFLAGS) $(TESTOBJ) -o $@
    1.42 +$(ITEST): $(ITESTOBJ)
    1.43 +	$(CC) $(LDFLAGS) $(ITESTOBJ) -o $@
    1.44 +
    1.45 +$(MTEST): $(MTESTOBJ)
    1.46 +	$(CC) $(LDFLAGS) $(MTESTOBJ) -o $@
    1.47  
    1.48  $(CALIBRATE): $(CALIBRATEOBJ)
    1.49  	$(CC) $(LDFLAGS) $(CALIBRATEOBJ) -o $@
     2.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     2.2 +++ b/mtest.c	Tue Dec 16 18:07:08 2014 +0000
     2.3 @@ -0,0 +1,123 @@
     2.4 +/*
     2.5 + * Sample Pololu MinIMU-9 measurements.
     2.6 + *
     2.7 + * Copyright (C) 2013, 2014 Paul Boddie
     2.8 + *
     2.9 + * This program is free software; you can redistribute it and/or modify
    2.10 + * it under the terms of the GNU General Public License as published by
    2.11 + * the Free Software Foundation; either version 2 of the License, or
    2.12 + * (at your option) any later version.
    2.13 + */
    2.14 +
    2.15 +#include <stdio.h>
    2.16 +#include <stdlib.h>
    2.17 +#include <signal.h>
    2.18 +#include <string.h>
    2.19 +#include <unistd.h>
    2.20 +#include "imu.h"
    2.21 +#include "shutdown.h"
    2.22 +#include "geo.h"
    2.23 +#include "measure.h"
    2.24 +
    2.25 +vectorf xymax = {{0, 0, 0}},
    2.26 +        yzmax = {{0, 0, 0}},
    2.27 +        xzmax = {{0, 0, 0}};
    2.28 +
    2.29 +void mtest_shutdown(int signum)
    2.30 +{
    2.31 +    printf("\n%-6.4f %-6.4f %-6.4f %-6.4f %-6.4f %-6.4f\n",
    2.32 +        xymax.x, xymax.y, yzmax.y, yzmax.z, xzmax.x, xzmax.z);
    2.33 +    init_shutdown(signum);
    2.34 +}
    2.35 +
    2.36 +/* Main program. */
    2.37 +
    2.38 +int main(int argc, char *argv[])
    2.39 +{
    2.40 +    uint8_t result[6];
    2.41 +    vectorf value;
    2.42 +    double xymag, yzmag, xzmag;
    2.43 +    double xymagmax = 0, yzmagmax = 0, xzmagmax = 0;
    2.44 +
    2.45 +    signal(SIGINT, mtest_shutdown);
    2.46 +
    2.47 +    /* Access the 8:10 port. */
    2.48 +
    2.49 +    if (ubb_open(0) < 0) {
    2.50 +        perror("ubb_open");
    2.51 +        return 1;
    2.52 +    }
    2.53 +
    2.54 +    ubb_power(1);
    2.55 +    printf("Power on.\n");
    2.56 +
    2.57 +    /* Bring the IMU up. */
    2.58 +
    2.59 +    imu_init();
    2.60 +
    2.61 +    imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRA_REG_M, IMU_MAGNET_FREQ);
    2.62 +    imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRB_REG_M, IMU_MAGNET_SCALE);
    2.63 +    imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_CONT);
    2.64 +
    2.65 +    if (imu_recv(IMU_GYRO_ADDRESS, IMU_GYRO_WHO_AM_I, result, 1))
    2.66 +        printf("Who am I? %x\n", result[0]);
    2.67 +
    2.68 +    if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_WHO_AM_I_M, result, 1))
    2.69 +        printf("Who am I? %x\n", result[0]);
    2.70 +
    2.71 +    if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRA_REG_M, result, 1))
    2.72 +        printf("Identification A? %x\n", result[0]);
    2.73 +
    2.74 +    if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRB_REG_M, result, 1))
    2.75 +        printf("Identification B? %x\n", result[0]);
    2.76 +
    2.77 +    if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRC_REG_M, result, 1))
    2.78 +        printf("Identification C? %x\n", result[0]);
    2.79 +
    2.80 +    /* Initialise the recorded magnetometer limits. */
    2.81 +
    2.82 +    mag_calibrate();
    2.83 +
    2.84 +    while (1)
    2.85 +    {
    2.86 +        imu_read_vector_xzy(IMU_MAGNET_ADDRESS, IMU_MAGNET_OUT_X_H_M, &value, convertBE12L);
    2.87 +        normalise(&value, &fieldmin, &fieldmax, &value);
    2.88 +
    2.89 +        xymag = sqrt(pow(value.x, 2) + pow(value.y, 2));
    2.90 +        yzmag = sqrt(pow(value.y, 2) + pow(value.z, 2));
    2.91 +        xzmag = sqrt(pow(value.x, 2) + pow(value.z, 2));
    2.92 +
    2.93 +        if (xymag > xymagmax)
    2.94 +        {
    2.95 +            xymagmax = xymag;
    2.96 +            xymax.x = value.x; xymax.y = value.y;
    2.97 +        }
    2.98 +
    2.99 +        if (yzmag > yzmagmax)
   2.100 +        {
   2.101 +            yzmagmax = yzmag;
   2.102 +            yzmax.y = value.y; yzmax.z = value.z;
   2.103 +        }
   2.104 +
   2.105 +        if (xzmag > xzmagmax)
   2.106 +        {
   2.107 +            xzmagmax = xzmag;
   2.108 +            xzmax.x = value.x; xzmax.z = value.z;
   2.109 +        }
   2.110 +
   2.111 +        printf("V: % 6.4f, % 6.4f, % 6.4f  "
   2.112 +               "XY: % 6.4f  "
   2.113 +               "YZ: % 6.4f  "
   2.114 +               "XZ: % 6.4f\n",
   2.115 +               value.x, value.y, value.z,
   2.116 +               xymag, yzmag, xzmag
   2.117 +               );
   2.118 +
   2.119 +        usleep(IMU_MAGNET_UPDATE_PERIOD);
   2.120 +    }
   2.121 +
   2.122 +    /* This should be unreachable. */
   2.123 +
   2.124 +    ubb_close(0);
   2.125 +    return 0;
   2.126 +}