1.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
1.2 +++ b/Makefile Mon Oct 14 13:02:19 2013 +0000
1.3 @@ -0,0 +1,60 @@
1.4 +# Makefile - Build the Pololu IMU software
1.5 +#
1.6 +# Copyright (C) 2013 Paul Boddie
1.7 +#
1.8 +# This program is free software; you can redistribute it and/or modify
1.9 +# it under the terms of the GNU General Public License as published by
1.10 +# the Free Software Foundation; either version 2 of the License, or
1.11 +# (at your option) any later version.
1.12 +
1.13 +LIBUBB = ../ben-blinkenlights/libubb
1.14 +SYSROOT = ../openwrt-xburst/staging_dir/target-mipsel_eglibc-2.15
1.15 +TOOLBIN = ../openwrt-xburst/staging_dir/toolchain-mipsel_gcc-4.6-linaro_eglibc-2.15/bin
1.16 +
1.17 +ARCH = mipsel-openwrt-linux
1.18 +CC = $(TOOLBIN)/$(ARCH)-gcc
1.19 +
1.20 +CFLAGS = -g -Wall -fPIC -march=mips32 -I$(LIBUBB)/include $(shell $(SYSROOT)/usr/bin/sdl-config --cflags) # -DDEBUG=1
1.21 +LDFLAGS = -lm -lubb -L$(LIBUBB) $(shell $(SYSROOT)/usr/bin/sdl-config --libs) -Wl,-rpath-link,$(SYSROOT)/usr/lib -lSDL_gfx
1.22 +
1.23 +IMU = imu
1.24 +TEST = itest
1.25 +CALIBRATE = calibrate
1.26 +TARGETS = $(IMU) $(TEST) $(CALIBRATE)
1.27 +
1.28 +BASICSRC = imu.c i2c.c shutdown.c geo.c
1.29 +UISRC = measure.c ui.c gui.c
1.30 +
1.31 +IMUSRC = $(BASICSRC) $(UISRC) main.c
1.32 +IMUOBJ = $(IMUSRC:.c=.o)
1.33 +
1.34 +TESTSRC = $(BASICSRC) $(UISRC) itest.c
1.35 +TESTOBJ = $(TESTSRC:.c=.o)
1.36 +
1.37 +CALIBRATESRC = $(BASICSRC) calibrate.c
1.38 +CALIBRATEOBJ = $(CALIBRATESRC:.c=.o)
1.39 +
1.40 +ALLSRC = $(BASICSRC) $(UISRC) main.c itest.c calibrate.c
1.41 +ALLOBJ = $(ALLSRC:.c=.o)
1.42 +
1.43 +.PHONY: all clean distclean
1.44 +
1.45 +all: $(TARGETS)
1.46 +
1.47 +clean:
1.48 + rm -f $(ALLOBJ) $(TARGETS)
1.49 +
1.50 +distclean: clean
1.51 + echo "Nothing else to clean."
1.52 +
1.53 +$(IMU): $(IMUOBJ)
1.54 + $(CC) $(LDFLAGS) $(IMUOBJ) -o $@
1.55 +
1.56 +$(TEST): $(TESTOBJ)
1.57 + $(CC) $(LDFLAGS) $(TESTOBJ) -o $@
1.58 +
1.59 +$(CALIBRATE): $(CALIBRATEOBJ)
1.60 + $(CC) $(LDFLAGS) $(CALIBRATEOBJ) -o $@
1.61 +
1.62 +.c.o:
1.63 + $(CC) -c $(CFLAGS) $< -o $@
2.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
2.2 +++ b/bool.h Mon Oct 14 13:02:19 2013 +0000
2.3 @@ -0,0 +1,12 @@
2.4 +/*
2.5 + * A simple boolean definition found in Python's asdl.h.
2.6 + */
2.7 +
2.8 +#ifndef __BOOL_H__
2.9 +#define __BOOL_H__
2.10 +
2.11 +#ifndef __cplusplus
2.12 +typedef enum {false, true} bool;
2.13 +#endif
2.14 +
2.15 +#endif /* __BOOL_H__ */
3.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
3.2 +++ b/calibrate.c Mon Oct 14 13:02:19 2013 +0000
3.3 @@ -0,0 +1,167 @@
3.4 +/*
3.5 + * Calibrate Pololu MinIMU-9 measurements.
3.6 + *
3.7 + * Copyright (C) 2013 Paul Boddie
3.8 + *
3.9 + * This program is free software; you can redistribute it and/or modify
3.10 + * it under the terms of the GNU General Public License as published by
3.11 + * the Free Software Foundation; either version 2 of the License, or
3.12 + * (at your option) any later version.
3.13 + */
3.14 +
3.15 +#include <stdio.h>
3.16 +#include <stdlib.h>
3.17 +#include <signal.h>
3.18 +#include <string.h>
3.19 +#include <unistd.h>
3.20 +#include "imu.h"
3.21 +#include "shutdown.h"
3.22 +#include "geo.h"
3.23 +
3.24 +/* Main program. */
3.25 +
3.26 +int main(int argc, char *argv[])
3.27 +{
3.28 + uint8_t result[6];
3.29 + vectorf value,
3.30 + vmin = {{1, 1, 1}},
3.31 + vmax = {{-1, -1, -1}},
3.32 + valueB[IMU_ACCEL_BUFFER_SIZE];
3.33 + int argno = 1, vindex = 0;
3.34 + bool gyroscope = false, accelerometer = false, magnetometer = false,
3.35 + averaging = false, normalised = false;
3.36 +
3.37 + if ((argc > argno) && (strcmp(argv[argno], "-g") == 0))
3.38 + {
3.39 + argno++;
3.40 + gyroscope = true;
3.41 + }
3.42 + else if ((argc > argno) && (strcmp(argv[argno], "-a") == 0))
3.43 + {
3.44 + argno++;
3.45 + accelerometer = true;
3.46 + }
3.47 + else if ((argc > argno) && (strcmp(argv[argno], "-m") == 0))
3.48 + {
3.49 + argno++;
3.50 + magnetometer = true;
3.51 + }
3.52 + else
3.53 + {
3.54 + printf("Need -g, -a or -m to select gyroscope, accelerometer or magnetometer respectively.\n");
3.55 + exit(1);
3.56 + }
3.57 +
3.58 + if ((argc > argno) && (strcmp(argv[argno], "-v") == 0))
3.59 + {
3.60 + argno++;
3.61 + averaging = true;
3.62 + }
3.63 +
3.64 + if ((argc > argno) && (strcmp(argv[argno], "-n") == 0))
3.65 + {
3.66 + argno++;
3.67 + normalised = true;
3.68 + }
3.69 +
3.70 + signal(SIGINT, init_shutdown);
3.71 +
3.72 + /* Access the 8:10 port. */
3.73 +
3.74 + if (ubb_open(0) < 0) {
3.75 + perror("ubb_open");
3.76 + return 1;
3.77 + }
3.78 +
3.79 + ubb_power(1);
3.80 + printf("Power on.\n");
3.81 +
3.82 + /* Bring the IMU up. */
3.83 +
3.84 + imu_init();
3.85 +
3.86 + if (gyroscope)
3.87 + {
3.88 + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG1, IMU_GYRO_CTRL_REG1_ALL);
3.89 + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG5, 0);
3.90 + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG4, IMU_GYRO_CTRL_REG4_BDU | IMU_GYRO_DPS_SCALE);
3.91 + }
3.92 + else if (accelerometer)
3.93 + {
3.94 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG1_A, IMU_ACCEL_CTRL_REG1_ALL | IMU_ACCEL_CTRL_REG1_50HZ);
3.95 +
3.96 + if ((argc > argno) && (strcmp(argv[argno], "-f") == 0))
3.97 + {
3.98 + argno++;
3.99 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, IMU_ACCEL_CTRL_REG2_FDS | IMU_ACCEL_FILTER_FREQ);
3.100 + imu_recv(IMU_ACCEL_ADDRESS, IMU_ACCEL_HP_FILTER_RESET_A, result, 1);
3.101 + }
3.102 + else
3.103 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, 0);
3.104 +
3.105 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG4_A, IMU_ACCEL_CTRL_REG4_BDU | IMU_ACCEL_SCALE);
3.106 + }
3.107 + else if (magnetometer)
3.108 + {
3.109 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRA_REG_M, IMU_MAGNET_CRA_REG_30HZ);
3.110 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRB_REG_M, IMU_MAGNET_CRB_REG_4_0G);
3.111 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_SINGLE);
3.112 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_CONT);
3.113 + }
3.114 +
3.115 + if (imu_recv(IMU_GYRO_ADDRESS, IMU_GYRO_WHO_AM_I, result, 1))
3.116 + printf("Who am I? %x\n", result[0]);
3.117 +
3.118 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_WHO_AM_I_M, result, 1))
3.119 + printf("Who am I? %x\n", result[0]);
3.120 +
3.121 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRA_REG_M, result, 1))
3.122 + printf("Identification A? %x\n", result[0]);
3.123 +
3.124 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRB_REG_M, result, 1))
3.125 + printf("Identification B? %x\n", result[0]);
3.126 +
3.127 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRC_REG_M, result, 1))
3.128 + printf("Identification C? %x\n", result[0]);
3.129 +
3.130 + memset(valueB, 0, sizeof(valueB));
3.131 +
3.132 + while (1)
3.133 + {
3.134 + if (accelerometer)
3.135 + imu_read_vector(IMU_ACCEL_ADDRESS, IMU_ACCEL_OUT_X_L_A | IMU_ACCEL_READ_MANY, &value, convert12);
3.136 + else if (gyroscope)
3.137 + imu_read_vector(IMU_GYRO_ADDRESS, IMU_GYRO_OUT_X_L | IMU_GYRO_READ_MANY, &value, convert);
3.138 + else if (magnetometer)
3.139 + imu_read_vector_xzy(IMU_MAGNET_ADDRESS, IMU_MAGNET_OUT_X_H_M, &value, convertBE12L);
3.140 +
3.141 + if (averaging)
3.142 + {
3.143 + queue(valueB, &vindex, IMU_ACCEL_BUFFER_SIZE, &value);
3.144 + filter(valueB, vindex, IMU_ACCEL_BUFFER_SIZE, &value);
3.145 + }
3.146 +
3.147 + vmin.x = min(value.x, vmin.x);
3.148 + vmin.y = min(value.y, vmin.y);
3.149 + vmin.z = min(value.z, vmin.z);
3.150 + vmax.x = max(value.x, vmax.x);
3.151 + vmax.y = max(value.y, vmax.y);
3.152 + vmax.z = max(value.z, vmax.z);
3.153 +
3.154 + printf("V: % 6.1f, % 6.1f, % 6.1f "
3.155 + "-: % 6.1f, % 6.1f, % 6.1f "
3.156 + "+: % 6.1f, % 6.1f, % 6.1f\n",
3.157 + normalised ? scale(value.x, vmin.x, (vmin.x + vmax.x) / 2, vmax.x) : value.x,
3.158 + normalised ? scale(value.y, vmin.y, (vmin.y + vmax.y) / 2, vmax.y) : value.y,
3.159 + normalised ? scale(value.z, vmin.z, (vmin.z + vmax.z) / 2, vmax.z) : value.z,
3.160 + vmin.x, vmin.y, vmin.z,
3.161 + vmax.x, vmax.y, vmax.z);
3.162 +
3.163 + usleep(magnetometer ? IMU_MAGNET_UPDATE_PERIOD : IMU_UPDATE_PERIOD);
3.164 + }
3.165 +
3.166 + /* This should be unreachable. */
3.167 +
3.168 + ubb_close(0);
3.169 + return 0;
3.170 +}
4.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
4.2 +++ b/docs/Ben_Pololu_IMU Mon Oct 14 13:02:19 2013 +0000
4.3 @@ -0,0 +1,23 @@
4.4 +Ben Pololu IMU is an experiment with the Ben NanoNote and the [http://www.pololu.com/catalog/product/1265 MinIMU-9 Gyro, Accelerometer, and Compass (L3G4200D and LSM303DLM Carrier)] board from [http://www.pololu.com/ Pololu].
4.5 +
4.6 +== Goals ==
4.7 +
4.8 +* Experiment with I2C communication using the 8:10 port (see [[UBB]])
4.9 +* Evaluate the accuracy of digital gyroscope and accelerometer components
4.10 +* Consider applications of orientation and positioning data
4.11 +* Refresh mostly dormant vector mathematics knowledge
4.12 +
4.13 +== Progress ==
4.14 +
4.15 +The MinIMU-9, with the supplied male headers attached, can be conveniently connected to female headers soldered to the Sparkfun microSD Sniffer: as is the case with the 8:10 pin ordering, the VIN and GND pins are adjacent on the MinIMU-9, and thus the connection can be built around them without the need for jumper wires. The board is quite able to function on power supplied by the Ben.
4.16 +
4.17 +Communication with the board is done using the I<sup>2</sup>C protocol which requires only two additional wires from the 8:10 port. Although Linux kernel support could probably be employed to conduct I<sup>2</sup>C communication using the GPIO lines associated with the 8:10 interface, a user space implementation is currently being used, with threading involved to reduce latency between measurements.
4.18 +
4.19 +A [http://hgweb.boddie.org.uk/ben-pololu-imu/ repository] is available containing code to allow the Ben to communicate with the IMU, and some results are shown below.
4.20 +
4.21 +<gallery widths=400px heights=300px>
4.22 +File:Pololu-MinIMU-9-with-headers.jpg|The unlabelled underside of the MinIMU-9 with protruding header pins (with the soldering having been done on the labelled side of the board)
4.23 +File:Sparkfun-microSD-Sniffer-with-Pololu-MinIMU-9.jpg|The MinIMU-9 connected to a Sparkfun microSD sniffer (in correct orientation for insertion into the 8:10 port)
4.24 +</gallery>
4.25 +
4.26 +[[Category:Ben NanoNote]]
5.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
5.2 +++ b/docs/COPYING.txt Mon Oct 14 13:02:19 2013 +0000
5.3 @@ -0,0 +1,17 @@
5.4 +Licence Agreement for ben-pololu-imu
5.5 +------------------------------------
5.6 +
5.7 +Copyright (C) 2013 Paul Boddie
5.8 +
5.9 +This program is free software; you can redistribute it and/or modify it under
5.10 +the terms of the GNU General Public License as published by the Free Software
5.11 +Foundation; either version 3 of the License, or (at your option) any later
5.12 +version.
5.13 +
5.14 +This program is distributed in the hope that it will be useful, but WITHOUT
5.15 +ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
5.16 +FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
5.17 +details.
5.18 +
5.19 +You should have received a copy of the GNU General Public License along with
5.20 +this program. If not, see <http://www.gnu.org/licenses/>.
6.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
6.2 +++ b/docs/gpl-3.0.txt Mon Oct 14 13:02:19 2013 +0000
6.3 @@ -0,0 +1,674 @@
6.4 + GNU GENERAL PUBLIC LICENSE
6.5 + Version 3, 29 June 2007
6.6 +
6.7 + Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
6.8 + Everyone is permitted to copy and distribute verbatim copies
6.9 + of this license document, but changing it is not allowed.
6.10 +
6.11 + Preamble
6.12 +
6.13 + The GNU General Public License is a free, copyleft license for
6.14 +software and other kinds of works.
6.15 +
6.16 + The licenses for most software and other practical works are designed
6.17 +to take away your freedom to share and change the works. By contrast,
6.18 +the GNU General Public License is intended to guarantee your freedom to
6.19 +share and change all versions of a program--to make sure it remains free
6.20 +software for all its users. We, the Free Software Foundation, use the
6.21 +GNU General Public License for most of our software; it applies also to
6.22 +any other work released this way by its authors. You can apply it to
6.23 +your programs, too.
6.24 +
6.25 + When we speak of free software, we are referring to freedom, not
6.26 +price. Our General Public Licenses are designed to make sure that you
6.27 +have the freedom to distribute copies of free software (and charge for
6.28 +them if you wish), that you receive source code or can get it if you
6.29 +want it, that you can change the software or use pieces of it in new
6.30 +free programs, and that you know you can do these things.
6.31 +
6.32 + To protect your rights, we need to prevent others from denying you
6.33 +these rights or asking you to surrender the rights. Therefore, you have
6.34 +certain responsibilities if you distribute copies of the software, or if
6.35 +you modify it: responsibilities to respect the freedom of others.
6.36 +
6.37 + For example, if you distribute copies of such a program, whether
6.38 +gratis or for a fee, you must pass on to the recipients the same
6.39 +freedoms that you received. You must make sure that they, too, receive
6.40 +or can get the source code. And you must show them these terms so they
6.41 +know their rights.
6.42 +
6.43 + Developers that use the GNU GPL protect your rights with two steps:
6.44 +(1) assert copyright on the software, and (2) offer you this License
6.45 +giving you legal permission to copy, distribute and/or modify it.
6.46 +
6.47 + For the developers' and authors' protection, the GPL clearly explains
6.48 +that there is no warranty for this free software. For both users' and
6.49 +authors' sake, the GPL requires that modified versions be marked as
6.50 +changed, so that their problems will not be attributed erroneously to
6.51 +authors of previous versions.
6.52 +
6.53 + Some devices are designed to deny users access to install or run
6.54 +modified versions of the software inside them, although the manufacturer
6.55 +can do so. This is fundamentally incompatible with the aim of
6.56 +protecting users' freedom to change the software. The systematic
6.57 +pattern of such abuse occurs in the area of products for individuals to
6.58 +use, which is precisely where it is most unacceptable. Therefore, we
6.59 +have designed this version of the GPL to prohibit the practice for those
6.60 +products. If such problems arise substantially in other domains, we
6.61 +stand ready to extend this provision to those domains in future versions
6.62 +of the GPL, as needed to protect the freedom of users.
6.63 +
6.64 + Finally, every program is threatened constantly by software patents.
6.65 +States should not allow patents to restrict development and use of
6.66 +software on general-purpose computers, but in those that do, we wish to
6.67 +avoid the special danger that patents applied to a free program could
6.68 +make it effectively proprietary. To prevent this, the GPL assures that
6.69 +patents cannot be used to render the program non-free.
6.70 +
6.71 + The precise terms and conditions for copying, distribution and
6.72 +modification follow.
6.73 +
6.74 + TERMS AND CONDITIONS
6.75 +
6.76 + 0. Definitions.
6.77 +
6.78 + "This License" refers to version 3 of the GNU General Public License.
6.79 +
6.80 + "Copyright" also means copyright-like laws that apply to other kinds of
6.81 +works, such as semiconductor masks.
6.82 +
6.83 + "The Program" refers to any copyrightable work licensed under this
6.84 +License. Each licensee is addressed as "you". "Licensees" and
6.85 +"recipients" may be individuals or organizations.
6.86 +
6.87 + To "modify" a work means to copy from or adapt all or part of the work
6.88 +in a fashion requiring copyright permission, other than the making of an
6.89 +exact copy. The resulting work is called a "modified version" of the
6.90 +earlier work or a work "based on" the earlier work.
6.91 +
6.92 + A "covered work" means either the unmodified Program or a work based
6.93 +on the Program.
6.94 +
6.95 + To "propagate" a work means to do anything with it that, without
6.96 +permission, would make you directly or secondarily liable for
6.97 +infringement under applicable copyright law, except executing it on a
6.98 +computer or modifying a private copy. Propagation includes copying,
6.99 +distribution (with or without modification), making available to the
6.100 +public, and in some countries other activities as well.
6.101 +
6.102 + To "convey" a work means any kind of propagation that enables other
6.103 +parties to make or receive copies. Mere interaction with a user through
6.104 +a computer network, with no transfer of a copy, is not conveying.
6.105 +
6.106 + An interactive user interface displays "Appropriate Legal Notices"
6.107 +to the extent that it includes a convenient and prominently visible
6.108 +feature that (1) displays an appropriate copyright notice, and (2)
6.109 +tells the user that there is no warranty for the work (except to the
6.110 +extent that warranties are provided), that licensees may convey the
6.111 +work under this License, and how to view a copy of this License. If
6.112 +the interface presents a list of user commands or options, such as a
6.113 +menu, a prominent item in the list meets this criterion.
6.114 +
6.115 + 1. Source Code.
6.116 +
6.117 + The "source code" for a work means the preferred form of the work
6.118 +for making modifications to it. "Object code" means any non-source
6.119 +form of a work.
6.120 +
6.121 + A "Standard Interface" means an interface that either is an official
6.122 +standard defined by a recognized standards body, or, in the case of
6.123 +interfaces specified for a particular programming language, one that
6.124 +is widely used among developers working in that language.
6.125 +
6.126 + The "System Libraries" of an executable work include anything, other
6.127 +than the work as a whole, that (a) is included in the normal form of
6.128 +packaging a Major Component, but which is not part of that Major
6.129 +Component, and (b) serves only to enable use of the work with that
6.130 +Major Component, or to implement a Standard Interface for which an
6.131 +implementation is available to the public in source code form. A
6.132 +"Major Component", in this context, means a major essential component
6.133 +(kernel, window system, and so on) of the specific operating system
6.134 +(if any) on which the executable work runs, or a compiler used to
6.135 +produce the work, or an object code interpreter used to run it.
6.136 +
6.137 + The "Corresponding Source" for a work in object code form means all
6.138 +the source code needed to generate, install, and (for an executable
6.139 +work) run the object code and to modify the work, including scripts to
6.140 +control those activities. However, it does not include the work's
6.141 +System Libraries, or general-purpose tools or generally available free
6.142 +programs which are used unmodified in performing those activities but
6.143 +which are not part of the work. For example, Corresponding Source
6.144 +includes interface definition files associated with source files for
6.145 +the work, and the source code for shared libraries and dynamically
6.146 +linked subprograms that the work is specifically designed to require,
6.147 +such as by intimate data communication or control flow between those
6.148 +subprograms and other parts of the work.
6.149 +
6.150 + The Corresponding Source need not include anything that users
6.151 +can regenerate automatically from other parts of the Corresponding
6.152 +Source.
6.153 +
6.154 + The Corresponding Source for a work in source code form is that
6.155 +same work.
6.156 +
6.157 + 2. Basic Permissions.
6.158 +
6.159 + All rights granted under this License are granted for the term of
6.160 +copyright on the Program, and are irrevocable provided the stated
6.161 +conditions are met. This License explicitly affirms your unlimited
6.162 +permission to run the unmodified Program. The output from running a
6.163 +covered work is covered by this License only if the output, given its
6.164 +content, constitutes a covered work. This License acknowledges your
6.165 +rights of fair use or other equivalent, as provided by copyright law.
6.166 +
6.167 + You may make, run and propagate covered works that you do not
6.168 +convey, without conditions so long as your license otherwise remains
6.169 +in force. You may convey covered works to others for the sole purpose
6.170 +of having them make modifications exclusively for you, or provide you
6.171 +with facilities for running those works, provided that you comply with
6.172 +the terms of this License in conveying all material for which you do
6.173 +not control copyright. Those thus making or running the covered works
6.174 +for you must do so exclusively on your behalf, under your direction
6.175 +and control, on terms that prohibit them from making any copies of
6.176 +your copyrighted material outside their relationship with you.
6.177 +
6.178 + Conveying under any other circumstances is permitted solely under
6.179 +the conditions stated below. Sublicensing is not allowed; section 10
6.180 +makes it unnecessary.
6.181 +
6.182 + 3. Protecting Users' Legal Rights From Anti-Circumvention Law.
6.183 +
6.184 + No covered work shall be deemed part of an effective technological
6.185 +measure under any applicable law fulfilling obligations under article
6.186 +11 of the WIPO copyright treaty adopted on 20 December 1996, or
6.187 +similar laws prohibiting or restricting circumvention of such
6.188 +measures.
6.189 +
6.190 + When you convey a covered work, you waive any legal power to forbid
6.191 +circumvention of technological measures to the extent such circumvention
6.192 +is effected by exercising rights under this License with respect to
6.193 +the covered work, and you disclaim any intention to limit operation or
6.194 +modification of the work as a means of enforcing, against the work's
6.195 +users, your or third parties' legal rights to forbid circumvention of
6.196 +technological measures.
6.197 +
6.198 + 4. Conveying Verbatim Copies.
6.199 +
6.200 + You may convey verbatim copies of the Program's source code as you
6.201 +receive it, in any medium, provided that you conspicuously and
6.202 +appropriately publish on each copy an appropriate copyright notice;
6.203 +keep intact all notices stating that this License and any
6.204 +non-permissive terms added in accord with section 7 apply to the code;
6.205 +keep intact all notices of the absence of any warranty; and give all
6.206 +recipients a copy of this License along with the Program.
6.207 +
6.208 + You may charge any price or no price for each copy that you convey,
6.209 +and you may offer support or warranty protection for a fee.
6.210 +
6.211 + 5. Conveying Modified Source Versions.
6.212 +
6.213 + You may convey a work based on the Program, or the modifications to
6.214 +produce it from the Program, in the form of source code under the
6.215 +terms of section 4, provided that you also meet all of these conditions:
6.216 +
6.217 + a) The work must carry prominent notices stating that you modified
6.218 + it, and giving a relevant date.
6.219 +
6.220 + b) The work must carry prominent notices stating that it is
6.221 + released under this License and any conditions added under section
6.222 + 7. This requirement modifies the requirement in section 4 to
6.223 + "keep intact all notices".
6.224 +
6.225 + c) You must license the entire work, as a whole, under this
6.226 + License to anyone who comes into possession of a copy. This
6.227 + License will therefore apply, along with any applicable section 7
6.228 + additional terms, to the whole of the work, and all its parts,
6.229 + regardless of how they are packaged. This License gives no
6.230 + permission to license the work in any other way, but it does not
6.231 + invalidate such permission if you have separately received it.
6.232 +
6.233 + d) If the work has interactive user interfaces, each must display
6.234 + Appropriate Legal Notices; however, if the Program has interactive
6.235 + interfaces that do not display Appropriate Legal Notices, your
6.236 + work need not make them do so.
6.237 +
6.238 + A compilation of a covered work with other separate and independent
6.239 +works, which are not by their nature extensions of the covered work,
6.240 +and which are not combined with it such as to form a larger program,
6.241 +in or on a volume of a storage or distribution medium, is called an
6.242 +"aggregate" if the compilation and its resulting copyright are not
6.243 +used to limit the access or legal rights of the compilation's users
6.244 +beyond what the individual works permit. Inclusion of a covered work
6.245 +in an aggregate does not cause this License to apply to the other
6.246 +parts of the aggregate.
6.247 +
6.248 + 6. Conveying Non-Source Forms.
6.249 +
6.250 + You may convey a covered work in object code form under the terms
6.251 +of sections 4 and 5, provided that you also convey the
6.252 +machine-readable Corresponding Source under the terms of this License,
6.253 +in one of these ways:
6.254 +
6.255 + a) Convey the object code in, or embodied in, a physical product
6.256 + (including a physical distribution medium), accompanied by the
6.257 + Corresponding Source fixed on a durable physical medium
6.258 + customarily used for software interchange.
6.259 +
6.260 + b) Convey the object code in, or embodied in, a physical product
6.261 + (including a physical distribution medium), accompanied by a
6.262 + written offer, valid for at least three years and valid for as
6.263 + long as you offer spare parts or customer support for that product
6.264 + model, to give anyone who possesses the object code either (1) a
6.265 + copy of the Corresponding Source for all the software in the
6.266 + product that is covered by this License, on a durable physical
6.267 + medium customarily used for software interchange, for a price no
6.268 + more than your reasonable cost of physically performing this
6.269 + conveying of source, or (2) access to copy the
6.270 + Corresponding Source from a network server at no charge.
6.271 +
6.272 + c) Convey individual copies of the object code with a copy of the
6.273 + written offer to provide the Corresponding Source. This
6.274 + alternative is allowed only occasionally and noncommercially, and
6.275 + only if you received the object code with such an offer, in accord
6.276 + with subsection 6b.
6.277 +
6.278 + d) Convey the object code by offering access from a designated
6.279 + place (gratis or for a charge), and offer equivalent access to the
6.280 + Corresponding Source in the same way through the same place at no
6.281 + further charge. You need not require recipients to copy the
6.282 + Corresponding Source along with the object code. If the place to
6.283 + copy the object code is a network server, the Corresponding Source
6.284 + may be on a different server (operated by you or a third party)
6.285 + that supports equivalent copying facilities, provided you maintain
6.286 + clear directions next to the object code saying where to find the
6.287 + Corresponding Source. Regardless of what server hosts the
6.288 + Corresponding Source, you remain obligated to ensure that it is
6.289 + available for as long as needed to satisfy these requirements.
6.290 +
6.291 + e) Convey the object code using peer-to-peer transmission, provided
6.292 + you inform other peers where the object code and Corresponding
6.293 + Source of the work are being offered to the general public at no
6.294 + charge under subsection 6d.
6.295 +
6.296 + A separable portion of the object code, whose source code is excluded
6.297 +from the Corresponding Source as a System Library, need not be
6.298 +included in conveying the object code work.
6.299 +
6.300 + A "User Product" is either (1) a "consumer product", which means any
6.301 +tangible personal property which is normally used for personal, family,
6.302 +or household purposes, or (2) anything designed or sold for incorporation
6.303 +into a dwelling. In determining whether a product is a consumer product,
6.304 +doubtful cases shall be resolved in favor of coverage. For a particular
6.305 +product received by a particular user, "normally used" refers to a
6.306 +typical or common use of that class of product, regardless of the status
6.307 +of the particular user or of the way in which the particular user
6.308 +actually uses, or expects or is expected to use, the product. A product
6.309 +is a consumer product regardless of whether the product has substantial
6.310 +commercial, industrial or non-consumer uses, unless such uses represent
6.311 +the only significant mode of use of the product.
6.312 +
6.313 + "Installation Information" for a User Product means any methods,
6.314 +procedures, authorization keys, or other information required to install
6.315 +and execute modified versions of a covered work in that User Product from
6.316 +a modified version of its Corresponding Source. The information must
6.317 +suffice to ensure that the continued functioning of the modified object
6.318 +code is in no case prevented or interfered with solely because
6.319 +modification has been made.
6.320 +
6.321 + If you convey an object code work under this section in, or with, or
6.322 +specifically for use in, a User Product, and the conveying occurs as
6.323 +part of a transaction in which the right of possession and use of the
6.324 +User Product is transferred to the recipient in perpetuity or for a
6.325 +fixed term (regardless of how the transaction is characterized), the
6.326 +Corresponding Source conveyed under this section must be accompanied
6.327 +by the Installation Information. But this requirement does not apply
6.328 +if neither you nor any third party retains the ability to install
6.329 +modified object code on the User Product (for example, the work has
6.330 +been installed in ROM).
6.331 +
6.332 + The requirement to provide Installation Information does not include a
6.333 +requirement to continue to provide support service, warranty, or updates
6.334 +for a work that has been modified or installed by the recipient, or for
6.335 +the User Product in which it has been modified or installed. Access to a
6.336 +network may be denied when the modification itself materially and
6.337 +adversely affects the operation of the network or violates the rules and
6.338 +protocols for communication across the network.
6.339 +
6.340 + Corresponding Source conveyed, and Installation Information provided,
6.341 +in accord with this section must be in a format that is publicly
6.342 +documented (and with an implementation available to the public in
6.343 +source code form), and must require no special password or key for
6.344 +unpacking, reading or copying.
6.345 +
6.346 + 7. Additional Terms.
6.347 +
6.348 + "Additional permissions" are terms that supplement the terms of this
6.349 +License by making exceptions from one or more of its conditions.
6.350 +Additional permissions that are applicable to the entire Program shall
6.351 +be treated as though they were included in this License, to the extent
6.352 +that they are valid under applicable law. If additional permissions
6.353 +apply only to part of the Program, that part may be used separately
6.354 +under those permissions, but the entire Program remains governed by
6.355 +this License without regard to the additional permissions.
6.356 +
6.357 + When you convey a copy of a covered work, you may at your option
6.358 +remove any additional permissions from that copy, or from any part of
6.359 +it. (Additional permissions may be written to require their own
6.360 +removal in certain cases when you modify the work.) You may place
6.361 +additional permissions on material, added by you to a covered work,
6.362 +for which you have or can give appropriate copyright permission.
6.363 +
6.364 + Notwithstanding any other provision of this License, for material you
6.365 +add to a covered work, you may (if authorized by the copyright holders of
6.366 +that material) supplement the terms of this License with terms:
6.367 +
6.368 + a) Disclaiming warranty or limiting liability differently from the
6.369 + terms of sections 15 and 16 of this License; or
6.370 +
6.371 + b) Requiring preservation of specified reasonable legal notices or
6.372 + author attributions in that material or in the Appropriate Legal
6.373 + Notices displayed by works containing it; or
6.374 +
6.375 + c) Prohibiting misrepresentation of the origin of that material, or
6.376 + requiring that modified versions of such material be marked in
6.377 + reasonable ways as different from the original version; or
6.378 +
6.379 + d) Limiting the use for publicity purposes of names of licensors or
6.380 + authors of the material; or
6.381 +
6.382 + e) Declining to grant rights under trademark law for use of some
6.383 + trade names, trademarks, or service marks; or
6.384 +
6.385 + f) Requiring indemnification of licensors and authors of that
6.386 + material by anyone who conveys the material (or modified versions of
6.387 + it) with contractual assumptions of liability to the recipient, for
6.388 + any liability that these contractual assumptions directly impose on
6.389 + those licensors and authors.
6.390 +
6.391 + All other non-permissive additional terms are considered "further
6.392 +restrictions" within the meaning of section 10. If the Program as you
6.393 +received it, or any part of it, contains a notice stating that it is
6.394 +governed by this License along with a term that is a further
6.395 +restriction, you may remove that term. If a license document contains
6.396 +a further restriction but permits relicensing or conveying under this
6.397 +License, you may add to a covered work material governed by the terms
6.398 +of that license document, provided that the further restriction does
6.399 +not survive such relicensing or conveying.
6.400 +
6.401 + If you add terms to a covered work in accord with this section, you
6.402 +must place, in the relevant source files, a statement of the
6.403 +additional terms that apply to those files, or a notice indicating
6.404 +where to find the applicable terms.
6.405 +
6.406 + Additional terms, permissive or non-permissive, may be stated in the
6.407 +form of a separately written license, or stated as exceptions;
6.408 +the above requirements apply either way.
6.409 +
6.410 + 8. Termination.
6.411 +
6.412 + You may not propagate or modify a covered work except as expressly
6.413 +provided under this License. Any attempt otherwise to propagate or
6.414 +modify it is void, and will automatically terminate your rights under
6.415 +this License (including any patent licenses granted under the third
6.416 +paragraph of section 11).
6.417 +
6.418 + However, if you cease all violation of this License, then your
6.419 +license from a particular copyright holder is reinstated (a)
6.420 +provisionally, unless and until the copyright holder explicitly and
6.421 +finally terminates your license, and (b) permanently, if the copyright
6.422 +holder fails to notify you of the violation by some reasonable means
6.423 +prior to 60 days after the cessation.
6.424 +
6.425 + Moreover, your license from a particular copyright holder is
6.426 +reinstated permanently if the copyright holder notifies you of the
6.427 +violation by some reasonable means, this is the first time you have
6.428 +received notice of violation of this License (for any work) from that
6.429 +copyright holder, and you cure the violation prior to 30 days after
6.430 +your receipt of the notice.
6.431 +
6.432 + Termination of your rights under this section does not terminate the
6.433 +licenses of parties who have received copies or rights from you under
6.434 +this License. If your rights have been terminated and not permanently
6.435 +reinstated, you do not qualify to receive new licenses for the same
6.436 +material under section 10.
6.437 +
6.438 + 9. Acceptance Not Required for Having Copies.
6.439 +
6.440 + You are not required to accept this License in order to receive or
6.441 +run a copy of the Program. Ancillary propagation of a covered work
6.442 +occurring solely as a consequence of using peer-to-peer transmission
6.443 +to receive a copy likewise does not require acceptance. However,
6.444 +nothing other than this License grants you permission to propagate or
6.445 +modify any covered work. These actions infringe copyright if you do
6.446 +not accept this License. Therefore, by modifying or propagating a
6.447 +covered work, you indicate your acceptance of this License to do so.
6.448 +
6.449 + 10. Automatic Licensing of Downstream Recipients.
6.450 +
6.451 + Each time you convey a covered work, the recipient automatically
6.452 +receives a license from the original licensors, to run, modify and
6.453 +propagate that work, subject to this License. You are not responsible
6.454 +for enforcing compliance by third parties with this License.
6.455 +
6.456 + An "entity transaction" is a transaction transferring control of an
6.457 +organization, or substantially all assets of one, or subdividing an
6.458 +organization, or merging organizations. If propagation of a covered
6.459 +work results from an entity transaction, each party to that
6.460 +transaction who receives a copy of the work also receives whatever
6.461 +licenses to the work the party's predecessor in interest had or could
6.462 +give under the previous paragraph, plus a right to possession of the
6.463 +Corresponding Source of the work from the predecessor in interest, if
6.464 +the predecessor has it or can get it with reasonable efforts.
6.465 +
6.466 + You may not impose any further restrictions on the exercise of the
6.467 +rights granted or affirmed under this License. For example, you may
6.468 +not impose a license fee, royalty, or other charge for exercise of
6.469 +rights granted under this License, and you may not initiate litigation
6.470 +(including a cross-claim or counterclaim in a lawsuit) alleging that
6.471 +any patent claim is infringed by making, using, selling, offering for
6.472 +sale, or importing the Program or any portion of it.
6.473 +
6.474 + 11. Patents.
6.475 +
6.476 + A "contributor" is a copyright holder who authorizes use under this
6.477 +License of the Program or a work on which the Program is based. The
6.478 +work thus licensed is called the contributor's "contributor version".
6.479 +
6.480 + A contributor's "essential patent claims" are all patent claims
6.481 +owned or controlled by the contributor, whether already acquired or
6.482 +hereafter acquired, that would be infringed by some manner, permitted
6.483 +by this License, of making, using, or selling its contributor version,
6.484 +but do not include claims that would be infringed only as a
6.485 +consequence of further modification of the contributor version. For
6.486 +purposes of this definition, "control" includes the right to grant
6.487 +patent sublicenses in a manner consistent with the requirements of
6.488 +this License.
6.489 +
6.490 + Each contributor grants you a non-exclusive, worldwide, royalty-free
6.491 +patent license under the contributor's essential patent claims, to
6.492 +make, use, sell, offer for sale, import and otherwise run, modify and
6.493 +propagate the contents of its contributor version.
6.494 +
6.495 + In the following three paragraphs, a "patent license" is any express
6.496 +agreement or commitment, however denominated, not to enforce a patent
6.497 +(such as an express permission to practice a patent or covenant not to
6.498 +sue for patent infringement). To "grant" such a patent license to a
6.499 +party means to make such an agreement or commitment not to enforce a
6.500 +patent against the party.
6.501 +
6.502 + If you convey a covered work, knowingly relying on a patent license,
6.503 +and the Corresponding Source of the work is not available for anyone
6.504 +to copy, free of charge and under the terms of this License, through a
6.505 +publicly available network server or other readily accessible means,
6.506 +then you must either (1) cause the Corresponding Source to be so
6.507 +available, or (2) arrange to deprive yourself of the benefit of the
6.508 +patent license for this particular work, or (3) arrange, in a manner
6.509 +consistent with the requirements of this License, to extend the patent
6.510 +license to downstream recipients. "Knowingly relying" means you have
6.511 +actual knowledge that, but for the patent license, your conveying the
6.512 +covered work in a country, or your recipient's use of the covered work
6.513 +in a country, would infringe one or more identifiable patents in that
6.514 +country that you have reason to believe are valid.
6.515 +
6.516 + If, pursuant to or in connection with a single transaction or
6.517 +arrangement, you convey, or propagate by procuring conveyance of, a
6.518 +covered work, and grant a patent license to some of the parties
6.519 +receiving the covered work authorizing them to use, propagate, modify
6.520 +or convey a specific copy of the covered work, then the patent license
6.521 +you grant is automatically extended to all recipients of the covered
6.522 +work and works based on it.
6.523 +
6.524 + A patent license is "discriminatory" if it does not include within
6.525 +the scope of its coverage, prohibits the exercise of, or is
6.526 +conditioned on the non-exercise of one or more of the rights that are
6.527 +specifically granted under this License. You may not convey a covered
6.528 +work if you are a party to an arrangement with a third party that is
6.529 +in the business of distributing software, under which you make payment
6.530 +to the third party based on the extent of your activity of conveying
6.531 +the work, and under which the third party grants, to any of the
6.532 +parties who would receive the covered work from you, a discriminatory
6.533 +patent license (a) in connection with copies of the covered work
6.534 +conveyed by you (or copies made from those copies), or (b) primarily
6.535 +for and in connection with specific products or compilations that
6.536 +contain the covered work, unless you entered into that arrangement,
6.537 +or that patent license was granted, prior to 28 March 2007.
6.538 +
6.539 + Nothing in this License shall be construed as excluding or limiting
6.540 +any implied license or other defenses to infringement that may
6.541 +otherwise be available to you under applicable patent law.
6.542 +
6.543 + 12. No Surrender of Others' Freedom.
6.544 +
6.545 + If conditions are imposed on you (whether by court order, agreement or
6.546 +otherwise) that contradict the conditions of this License, they do not
6.547 +excuse you from the conditions of this License. If you cannot convey a
6.548 +covered work so as to satisfy simultaneously your obligations under this
6.549 +License and any other pertinent obligations, then as a consequence you may
6.550 +not convey it at all. For example, if you agree to terms that obligate you
6.551 +to collect a royalty for further conveying from those to whom you convey
6.552 +the Program, the only way you could satisfy both those terms and this
6.553 +License would be to refrain entirely from conveying the Program.
6.554 +
6.555 + 13. Use with the GNU Affero General Public License.
6.556 +
6.557 + Notwithstanding any other provision of this License, you have
6.558 +permission to link or combine any covered work with a work licensed
6.559 +under version 3 of the GNU Affero General Public License into a single
6.560 +combined work, and to convey the resulting work. The terms of this
6.561 +License will continue to apply to the part which is the covered work,
6.562 +but the special requirements of the GNU Affero General Public License,
6.563 +section 13, concerning interaction through a network will apply to the
6.564 +combination as such.
6.565 +
6.566 + 14. Revised Versions of this License.
6.567 +
6.568 + The Free Software Foundation may publish revised and/or new versions of
6.569 +the GNU General Public License from time to time. Such new versions will
6.570 +be similar in spirit to the present version, but may differ in detail to
6.571 +address new problems or concerns.
6.572 +
6.573 + Each version is given a distinguishing version number. If the
6.574 +Program specifies that a certain numbered version of the GNU General
6.575 +Public License "or any later version" applies to it, you have the
6.576 +option of following the terms and conditions either of that numbered
6.577 +version or of any later version published by the Free Software
6.578 +Foundation. If the Program does not specify a version number of the
6.579 +GNU General Public License, you may choose any version ever published
6.580 +by the Free Software Foundation.
6.581 +
6.582 + If the Program specifies that a proxy can decide which future
6.583 +versions of the GNU General Public License can be used, that proxy's
6.584 +public statement of acceptance of a version permanently authorizes you
6.585 +to choose that version for the Program.
6.586 +
6.587 + Later license versions may give you additional or different
6.588 +permissions. However, no additional obligations are imposed on any
6.589 +author or copyright holder as a result of your choosing to follow a
6.590 +later version.
6.591 +
6.592 + 15. Disclaimer of Warranty.
6.593 +
6.594 + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
6.595 +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
6.596 +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
6.597 +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
6.598 +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
6.599 +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
6.600 +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
6.601 +ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
6.602 +
6.603 + 16. Limitation of Liability.
6.604 +
6.605 + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
6.606 +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
6.607 +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
6.608 +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
6.609 +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
6.610 +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
6.611 +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
6.612 +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
6.613 +SUCH DAMAGES.
6.614 +
6.615 + 17. Interpretation of Sections 15 and 16.
6.616 +
6.617 + If the disclaimer of warranty and limitation of liability provided
6.618 +above cannot be given local legal effect according to their terms,
6.619 +reviewing courts shall apply local law that most closely approximates
6.620 +an absolute waiver of all civil liability in connection with the
6.621 +Program, unless a warranty or assumption of liability accompanies a
6.622 +copy of the Program in return for a fee.
6.623 +
6.624 + END OF TERMS AND CONDITIONS
6.625 +
6.626 + How to Apply These Terms to Your New Programs
6.627 +
6.628 + If you develop a new program, and you want it to be of the greatest
6.629 +possible use to the public, the best way to achieve this is to make it
6.630 +free software which everyone can redistribute and change under these terms.
6.631 +
6.632 + To do so, attach the following notices to the program. It is safest
6.633 +to attach them to the start of each source file to most effectively
6.634 +state the exclusion of warranty; and each file should have at least
6.635 +the "copyright" line and a pointer to where the full notice is found.
6.636 +
6.637 + <one line to give the program's name and a brief idea of what it does.>
6.638 + Copyright (C) <year> <name of author>
6.639 +
6.640 + This program is free software: you can redistribute it and/or modify
6.641 + it under the terms of the GNU General Public License as published by
6.642 + the Free Software Foundation, either version 3 of the License, or
6.643 + (at your option) any later version.
6.644 +
6.645 + This program is distributed in the hope that it will be useful,
6.646 + but WITHOUT ANY WARRANTY; without even the implied warranty of
6.647 + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
6.648 + GNU General Public License for more details.
6.649 +
6.650 + You should have received a copy of the GNU General Public License
6.651 + along with this program. If not, see <http://www.gnu.org/licenses/>.
6.652 +
6.653 +Also add information on how to contact you by electronic and paper mail.
6.654 +
6.655 + If the program does terminal interaction, make it output a short
6.656 +notice like this when it starts in an interactive mode:
6.657 +
6.658 + <program> Copyright (C) <year> <name of author>
6.659 + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
6.660 + This is free software, and you are welcome to redistribute it
6.661 + under certain conditions; type `show c' for details.
6.662 +
6.663 +The hypothetical commands `show w' and `show c' should show the appropriate
6.664 +parts of the General Public License. Of course, your program's commands
6.665 +might be different; for a GUI interface, you would use an "about box".
6.666 +
6.667 + You should also get your employer (if you work as a programmer) or school,
6.668 +if any, to sign a "copyright disclaimer" for the program, if necessary.
6.669 +For more information on this, and how to apply and follow the GNU GPL, see
6.670 +<http://www.gnu.org/licenses/>.
6.671 +
6.672 + The GNU General Public License does not permit incorporating your program
6.673 +into proprietary programs. If your program is a subroutine library, you
6.674 +may consider it more useful to permit linking proprietary applications with
6.675 +the library. If this is what you want to do, use the GNU Lesser General
6.676 +Public License instead of this License. But first, please read
6.677 +<http://www.gnu.org/philosophy/why-not-lgpl.html>.
7.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
7.2 +++ b/geo.c Mon Oct 14 13:02:19 2013 +0000
7.3 @@ -0,0 +1,232 @@
7.4 +/*
7.5 + * Geometric operations.
7.6 + *
7.7 + * Copyright (C) 2013 Paul Boddie
7.8 + *
7.9 + * This program is free software; you can redistribute it and/or modify
7.10 + * it under the terms of the GNU General Public License as published by
7.11 + * the Free Software Foundation; either version 2 of the License, or
7.12 + * (at your option) any later version.
7.13 + */
7.14 +
7.15 +#include "geo.h"
7.16 +
7.17 +/**
7.18 + * Return a value within the given magnitude.
7.19 + */
7.20 +double within(double value, double mag)
7.21 +{
7.22 + double offset = fmod(value, mag);
7.23 + int num = (int) fdiv(value, mag);
7.24 +
7.25 + if (num % 2 == 0)
7.26 + return offset;
7.27 + else if (offset > 0)
7.28 + return offset - mag;
7.29 + else
7.30 + return offset + mag;
7.31 +}
7.32 +
7.33 +/**
7.34 + * Return whether the given vector is null.
7.35 + */
7.36 +bool vectorf_null(const vectorf *in)
7.37 +{
7.38 + return ((in->x == 0) && (in->y == 0) && (in->z == 0));
7.39 +}
7.40 +
7.41 +/**
7.42 + * Reset the given vector to have zero magnitude.
7.43 + */
7.44 +void vectorf_reset(vectorf *in)
7.45 +{
7.46 + in->x = 0; in->y = 0; in->z = 0;
7.47 +}
7.48 +
7.49 +/**
7.50 + * Return the cross product of the two input vectors in the output vector
7.51 + * provided.
7.52 + */
7.53 +void vectorf_cross(const vectorf *in1, const vectorf *in2, vectorf *out)
7.54 +{
7.55 + out->x = in1->y * in2->z - in1->z * in2->y;
7.56 + out->y = in1->z * in2->x - in1->x * in2->z;
7.57 + out->z = in1->x * in2->y - in1->y * in2->x;
7.58 +}
7.59 +
7.60 +/**
7.61 + * Return the dot product of the two input vectors.
7.62 + */
7.63 +double vectorf_dot(const vectorf *in1, const vectorf *in2)
7.64 +{
7.65 + return in1->x * in2->x + in1->y * in2->y + in1->z * in2->z;
7.66 +}
7.67 +
7.68 +/**
7.69 + * Return the magnitude of the given vector.
7.70 + */
7.71 +double vectorf_mag(const vectorf *in)
7.72 +{
7.73 + return sqrt(pow(in->x, 2) + pow(in->y, 2) + pow(in->z, 2));
7.74 +}
7.75 +
7.76 +/**
7.77 + * Normalise the given vector.
7.78 + */
7.79 +void vectorf_normalise(vectorf *in, vectorf *out)
7.80 +{
7.81 + double mag = vectorf_mag(in);
7.82 +
7.83 + out->x = in->x / mag;
7.84 + out->y = in->y / mag;
7.85 + out->z = in->z / mag;
7.86 +}
7.87 +
7.88 +/**
7.89 + * Rotate the plane defined by the given input vectors by the given angle,
7.90 + * in radians, modifying the input vectors to define the resulting plane.
7.91 + */
7.92 +void plane_rotate(vectorf *in1, vectorf *in2, double angle)
7.93 +{
7.94 + double c = cos(angle), s = sin(angle);
7.95 + vectorf out1, out2;
7.96 +
7.97 + /* Rotation around the plane normal with contributions from both plane
7.98 + vectors. */
7.99 +
7.100 + out1.x = c * in1->x + s * in2->x;
7.101 + out1.y = c * in1->y + s * in2->y;
7.102 + out1.z = c * in1->z + s * in2->z;
7.103 +
7.104 + /* The second plane vector is 90 degrees (pi/2) ahead of the first:
7.105 + cos(angle + pi/2) == -sin(angle)
7.106 + sin(angle + pi/2) == cos(angle) */
7.107 +
7.108 + out2.x = c * in2->x - s * in1->x;
7.109 + out2.y = c * in2->y - s * in1->y;
7.110 + out2.z = c * in2->z - s * in1->z;
7.111 +
7.112 + *in1 = out1;
7.113 + *in2 = out2;
7.114 +}
7.115 +
7.116 +/**
7.117 + * Populate the output vector with the negated input vector.
7.118 + */
7.119 +void vectorf_negate(vectorf *in, vectorf *out)
7.120 +{
7.121 + out->x = -in->x;
7.122 + out->y = -in->y;
7.123 + out->z = -in->z;
7.124 +}
7.125 +
7.126 +/**
7.127 + * Return the direction of the given vector in radians.
7.128 + */
7.129 +double vectorf_direction(vectorf *in)
7.130 +{
7.131 + return atan2(in->x, in->z);
7.132 +}
7.133 +
7.134 +/**
7.135 + * Return the elevation of the given unit vector in radians.
7.136 + */
7.137 +double vectorf_elevation(vectorf *in)
7.138 +{
7.139 + return asin(in->y);
7.140 +}
7.141 +
7.142 +/**
7.143 + * Obtain the direction vector for the given direction and elevation.
7.144 + */
7.145 +void vectorf_polar(double direction, double elevation, vectorf *out)
7.146 +{
7.147 + out->x = sin(direction) * cos(elevation);
7.148 + out->y = sin(elevation);
7.149 + out->z = cos(direction) * cos(elevation);
7.150 +}
7.151 +
7.152 +/**
7.153 + * Convert the input vector expressed in terms of the given axis vectors into
7.154 + * the global coordinate space.
7.155 + */
7.156 +void vectorf_convert(const vectorf *in, const vectorf *devicex, const vectorf *devicey, const vectorf *devicez, vectorf *out)
7.157 +{
7.158 + out->x = in->x * devicex->x + in->y * devicey->x + in->z * devicez->x;
7.159 + out->y = in->x * devicex->y + in->y * devicey->y + in->z * devicez->y;
7.160 + out->z = in->x * devicex->z + in->y * devicey->z + in->z * devicez->z;
7.161 +}
7.162 +
7.163 +/**
7.164 + * Convert the input vector expressed in terms of the global coordinate space
7.165 + * into a vector expressed in terms of the given axis vectors.
7.166 + */
7.167 +void vectorf_convert_into(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z, vectorf *out)
7.168 +{
7.169 + out->x = vectorf_dot(in, x);
7.170 + out->y = vectorf_dot(in, y);
7.171 + out->z = vectorf_dot(in, z);
7.172 +}
7.173 +
7.174 +/**
7.175 + * Return the tilt/roll of the given vector in radians projected into the given
7.176 + * plane.
7.177 + */
7.178 +double vectorf_tilt_in_plane(const vectorf *in, const vectorf *x, const vectorf *y)
7.179 +{
7.180 + vectorf v, z, y2;
7.181 +
7.182 + vectorf_cross(x, y, &z);
7.183 + vectorf_normalise(&z, &z);
7.184 + vectorf_cross(&z, x, &y2);
7.185 + vectorf_convert_into(in, x, &y2, &z, &v);
7.186 + return atan2(v.y, v.x);
7.187 +}
7.188 +
7.189 +/**
7.190 + * Return the tilt/roll of the given vector in radians projected into the given
7.191 + * space.
7.192 + */
7.193 +double vectorf_tilt_in_plane_with_axis(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z)
7.194 +{
7.195 + vectorf v, z2, y2;
7.196 +
7.197 + vectorf_cross(x, y, &z2);
7.198 + if (vectorf_dot(z, &z2) < 0)
7.199 + vectorf_negate(&z2, &z2);
7.200 + vectorf_cross(&z2, x, &y2);
7.201 + vectorf_convert_into(in, x, &y2, &z2, &v);
7.202 + return atan2(v.y, v.x);
7.203 +}
7.204 +
7.205 +/**
7.206 + * Rotate a vector around the given z axis by the given angle in radians.
7.207 + */
7.208 +void vectorf_rotate_in_space(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z, double angle, vectorf *out)
7.209 +{
7.210 + vectorf v;
7.211 + double tilt, c, s, vz, mag;
7.212 +
7.213 + /* Project the vector into the xy plane. */
7.214 +
7.215 + vectorf_convert_into(in, x, y, z, &v);
7.216 +
7.217 + /* Remember the z value before obtaining a two dimensional vector. */
7.218 +
7.219 + vz = v.z;
7.220 + v.z = 0;
7.221 + mag = vectorf_mag(&v);
7.222 +
7.223 + /* Get the current angle. */
7.224 +
7.225 + tilt = atan2(v.y, v.x);
7.226 +
7.227 + /* Calculate the rotated vector. */
7.228 +
7.229 + c = cos(tilt + angle);
7.230 + s = sin(tilt + angle);
7.231 +
7.232 + out->x = mag * (c * x->x + s * y->x) + vz * z->x;
7.233 + out->y = mag * (c * x->y + s * y->y) + vz * z->y;
7.234 + out->z = mag * (c * x->z + s * y->z) + vz * z->z;
7.235 +}
8.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
8.2 +++ b/geo.h Mon Oct 14 13:02:19 2013 +0000
8.3 @@ -0,0 +1,64 @@
8.4 +/*
8.5 + * Geometric operations.
8.6 + *
8.7 + * Copyright (C) 2013 Paul Boddie
8.8 + *
8.9 + * This program is free software; you can redistribute it and/or modify
8.10 + * it under the terms of the GNU General Public License as published by
8.11 + * the Free Software Foundation; either version 2 of the License, or
8.12 + * (at your option) any later version.
8.13 + */
8.14 +
8.15 +#ifndef __GEO_H__
8.16 +#define __GEO_H__
8.17 +
8.18 +#include <math.h>
8.19 +#include "bool.h"
8.20 +
8.21 +typedef union
8.22 +{
8.23 + struct { double x, y, z; };
8.24 + double axis[3];
8.25 +} vectorf;
8.26 +
8.27 +double within(double value, double mag);
8.28 +bool vectorf_null(const vectorf *in);
8.29 +void vectorf_reset(vectorf *in);
8.30 +void vectorf_cross(const vectorf *in1, const vectorf *in2, vectorf *out);
8.31 +double vectorf_dot(const vectorf *in1, const vectorf *in2);
8.32 +double vectorf_mag(const vectorf *in);
8.33 +void vectorf_normalise(vectorf *in, vectorf *out);
8.34 +void plane_rotate(vectorf *in1, vectorf *in2, double angle);
8.35 +void vectorf_negate(vectorf *in, vectorf *out);
8.36 +double vectorf_direction(vectorf *in);
8.37 +double vectorf_elevation(vectorf *in);
8.38 +void vectorf_polar(double direction, double elevation, vectorf *out);
8.39 +void vectorf_convert(const vectorf *in, const vectorf *devicex, const vectorf *devicey, const vectorf *devicez, vectorf *out);
8.40 +void vectorf_convert_into(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z, vectorf *out);
8.41 +double vectorf_tilt_in_plane(const vectorf *in, const vectorf *x, const vectorf *y);
8.42 +double vectorf_tilt_in_plane_with_axis(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z);
8.43 +void vectorf_rotate_in_space(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z, double angle, vectorf *out);
8.44 +
8.45 +#define degrad(x) ((x) * M_PI / 180.0)
8.46 +#define raddeg(x) ((x) * 180.0 / M_PI)
8.47 +
8.48 +#define fdiv(x, y) (floor((int) ((x) / (y))))
8.49 +
8.50 +/* Get the closest number at the given resolution. */
8.51 +
8.52 +#define closest(x, res) (((int) (x) / (res)) * (res))
8.53 +
8.54 +/* Get the maximum and minimum of the given numbers. */
8.55 +
8.56 +#define max(x, y) ((x) >= (y) ? (x) : (y))
8.57 +#define min(x, y) ((x) <= (y) ? (x) : (y))
8.58 +
8.59 +/* Get the sign of a number. */
8.60 +
8.61 +#define sign(x) ((x) == 0 ? 0 : (x) < 0 ? -1 : 1)
8.62 +
8.63 +/* Eliminate noise using the given threshold. */
8.64 +
8.65 +#define noise(x, y) (fabs(x) < (y) ? 0 : (x))
8.66 +
8.67 +#endif /* __GEO_H__ */
9.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
9.2 +++ b/gui.c Mon Oct 14 13:02:19 2013 +0000
9.3 @@ -0,0 +1,366 @@
9.4 +/*
9.5 + * Ben NanoNote graphical user interface utilities.
9.6 + *
9.7 + * Copyright (C) 2013 Paul Boddie
9.8 + *
9.9 + * This program is free software; you can redistribute it and/or modify
9.10 + * it under the terms of the GNU General Public License as published by
9.11 + * the Free Software Foundation; either version 2 of the License, or
9.12 + * (at your option) any later version.
9.13 + */
9.14 +
9.15 +#include <stdio.h>
9.16 +#include "gui.h"
9.17 +
9.18 +SDL_Surface *screen;
9.19 +gui_printer printer = {0, 0, 255, 255, 255, 255};
9.20 +
9.21 +void gui_init()
9.22 +{
9.23 + if (SDL_Init(SDL_INIT_VIDEO) < 0)
9.24 + {
9.25 + fprintf(stderr, "SDL error: %s\n", SDL_GetError());
9.26 + exit(1);
9.27 + }
9.28 +}
9.29 +
9.30 +void gui_display_init()
9.31 +{
9.32 + screen = SDL_SetVideoMode(SCREEN_WIDTH, SCREEN_HEIGHT, 0, 0);
9.33 + SDL_ShowCursor(SDL_DISABLE);
9.34 +}
9.35 +
9.36 +void gui_shutdown(int signum)
9.37 +{
9.38 + SDL_Quit();
9.39 + text_shutdown(signum);
9.40 +}
9.41 +
9.42 +void gui_shutdown_threaded(int signum)
9.43 +{
9.44 + SDL_Quit();
9.45 + text_shutdown_threaded(signum);
9.46 +}
9.47 +
9.48 +void gui_quit()
9.49 +{
9.50 + SDL_Quit();
9.51 + text_quit();
9.52 +}
9.53 +
9.54 +void gui_clear()
9.55 +{
9.56 + gui_fill(0, 0, 0);
9.57 + printer.x = 0;
9.58 + printer.y = 0;
9.59 +}
9.60 +
9.61 +void gui_fill(uint8_t r, uint8_t g, uint8_t b)
9.62 +{
9.63 + SDL_FillRect(screen, NULL, SDL_MapRGB(screen->format, r, g, b));
9.64 +}
9.65 +
9.66 +void gui_next_row(gui_printer *printer, uint16_t rows)
9.67 +{
9.68 + printer->x = 0;
9.69 + printer->y = (printer->y + rows * SCREEN_ROW_HEIGHT) % SCREEN_HEIGHT;
9.70 +}
9.71 +
9.72 +void gui_next_column(gui_printer *printer, uint16_t columns)
9.73 +{
9.74 + printer->x += columns * SCREEN_COLUMN_WIDTH;
9.75 + if (printer->x >= SCREEN_WIDTH)
9.76 + {
9.77 + printer->x = 0;
9.78 + printer->y = (printer->y + SCREEN_ROW_HEIGHT) % SCREEN_HEIGHT;
9.79 + }
9.80 +}
9.81 +
9.82 +int gui_printf(const char *format, ...)
9.83 +{
9.84 + va_list args;
9.85 + static char buffer[GUI_BUFSIZE];
9.86 + char *this = buffer, *next;
9.87 + int printed = 0;
9.88 +
9.89 + va_start(args, format);
9.90 + vsnprintf(buffer, GUI_BUFSIZE, format, args);
9.91 + va_end(args);
9.92 +
9.93 + do
9.94 + {
9.95 + next = strchr(this, (int) '\n');
9.96 + if (next != NULL)
9.97 + *next = '\0';
9.98 + stringRGBA(screen, printer.x, printer.y, this, printer.r, printer.g, printer.b, printer.a);
9.99 + if (next != NULL)
9.100 + gui_next_row(&printer, 1);
9.101 + else
9.102 + gui_next_column(&printer, strlen(this));
9.103 + printed += strlen(this);
9.104 + this = next;
9.105 + }
9.106 + while (next != NULL);
9.107 +
9.108 + return printed;
9.109 +}
9.110 +
9.111 +/**
9.112 + * Generate sky points.
9.113 + */
9.114 +void gui_sky(vectorf *viewx, vectorf *viewy, vectorf *viewz)
9.115 +{
9.116 + double direction, elevation;
9.117 + int d, e, estart, efinish, dcount;
9.118 + int16_t lastminx, lastminy, thisminx, thisminy,
9.119 + lastmaxx, lastmaxy, thismaxx, thismaxy;
9.120 + uint32_t colour;
9.121 + vectorf point;
9.122 +
9.123 + direction = vectorf_direction(viewz);
9.124 + elevation = vectorf_elevation(viewz);
9.125 +
9.126 + d = closest(raddeg(direction), SKY_GRID_STEP);
9.127 +
9.128 + /* Plot points between elevations -90 and 90 degrees. */
9.129 +
9.130 + estart = max(-90, closest(raddeg(elevation), SKY_GRID_STEP) - (SKY_GRID_STEP * 3));
9.131 + efinish = min(90, closest(raddeg(elevation), SKY_GRID_STEP) + (SKY_GRID_STEP * 3));
9.132 +
9.133 + for (e = estart; e <= efinish; e += SKY_GRID_STEP)
9.134 + {
9.135 + /* Start from the most central point and work outwards until
9.136 + the points are no longer visible. */
9.137 +
9.138 + for (dcount = 0; dcount <= 180; dcount += SKY_GRID_STEP)
9.139 + {
9.140 + /* Check point visibility. */
9.141 +
9.142 + vectorf_polar(degrad(d - dcount), degrad(e), &point);
9.143 + if (vectorf_dot(viewz, &point) <= 0)
9.144 + break;
9.145 +
9.146 + /* Plot points at the given extremes. */
9.147 +
9.148 + colour = e < 0 ? SKY_LOWER_COLOUR : SKY_UPPER_COLOUR;
9.149 +
9.150 + /* Current minimum extent of the direction. */
9.151 +
9.152 + thisminx = vectorf_dot(viewx, &point) * PROJECTION_FACTOR + SCREEN_WIDTH / 2;
9.153 + thisminy = vectorf_dot(viewy, &point) * -PROJECTION_FACTOR + SCREEN_HEIGHT / 2;
9.154 +
9.155 + /* Draw from the last point at this elevation. */
9.156 +
9.157 + if (dcount != 0)
9.158 + {
9.159 + lineColor(screen,
9.160 + lastminx, lastminy,
9.161 + thisminx, thisminy,
9.162 + colour
9.163 + );
9.164 + }
9.165 +
9.166 + /* Draw to the next elevation. */
9.167 +
9.168 + gui_sky_vertical(viewx, viewy, viewz, d - dcount, e, thisminx, thisminy, colour);
9.169 +
9.170 + /* Current maximum extent of the direction. */
9.171 +
9.172 + vectorf_polar(degrad(d + dcount), degrad(e), &point);
9.173 + thismaxx = vectorf_dot(viewx, &point) * PROJECTION_FACTOR + SCREEN_WIDTH / 2;
9.174 + thismaxy = vectorf_dot(viewy, &point) * -PROJECTION_FACTOR + SCREEN_HEIGHT / 2;
9.175 +
9.176 + /* Draw from the last point at this elevation. */
9.177 +
9.178 + if (dcount != 0)
9.179 + {
9.180 + lineColor(screen,
9.181 + lastmaxx, lastmaxy,
9.182 + thismaxx, thismaxy,
9.183 + colour
9.184 + );
9.185 + }
9.186 +
9.187 + /* Draw to the next elevation. */
9.188 +
9.189 + gui_sky_vertical(viewx, viewy, viewz, d + dcount, e, thismaxx, thismaxy, colour);
9.190 +
9.191 + /* Stop after one point at -90 or 90 degree elevations. */
9.192 +
9.193 + if (abs(e) == 90)
9.194 + {
9.195 + pixelColor(screen,
9.196 + thisminx, thisminy,
9.197 + colour
9.198 + );
9.199 + break;
9.200 + }
9.201 +
9.202 + lastminx = thisminx; lastminy = thisminy;
9.203 + lastmaxx = thismaxx; lastmaxy = thismaxy;
9.204 + }
9.205 + }
9.206 +}
9.207 +
9.208 +void gui_sky_vertical(vectorf *viewx, vectorf *viewy, vectorf *viewz, int direction, int elevation, int16_t x, int16_t y, uint32_t colour)
9.209 +{
9.210 + vectorf point;
9.211 +
9.212 + if ((elevation >= -(90 - SKY_GRID_STEP)) || (elevation <= (90 - SKY_GRID_STEP)))
9.213 + {
9.214 + vectorf_polar(degrad(direction), degrad(elevation + SKY_GRID_STEP), &point);
9.215 + lineColor(screen,
9.216 + x, y,
9.217 + vectorf_dot(viewx, &point) * PROJECTION_FACTOR + SCREEN_WIDTH / 2,
9.218 + vectorf_dot(viewy, &point) * -PROJECTION_FACTOR + SCREEN_HEIGHT / 2,
9.219 + colour
9.220 + );
9.221 +
9.222 + if (elevation == -(90 - SKY_GRID_STEP))
9.223 + {
9.224 + vectorf_polar(degrad(direction), degrad(elevation - SKY_GRID_STEP), &point);
9.225 + lineColor(screen,
9.226 + x, y,
9.227 + vectorf_dot(viewx, &point) * PROJECTION_FACTOR + SCREEN_WIDTH / 2,
9.228 + vectorf_dot(viewy, &point) * -PROJECTION_FACTOR + SCREEN_HEIGHT / 2,
9.229 + colour
9.230 + );
9.231 + }
9.232 + }
9.233 +}
9.234 +
9.235 +/**
9.236 + * Generate motion vector.
9.237 + */
9.238 +void gui_motion(vectorf *viewx, vectorf *viewy, vectorf *viewz, vectorf *accelerationD)
9.239 +{
9.240 + vectorf point;
9.241 + uint32_t colour;
9.242 +
9.243 + if (vectorf_dot(viewz, accelerationD) <= 0)
9.244 + {
9.245 + colour = MOTION_REVERSE_COLOUR;
9.246 + vectorf_negate(accelerationD, &point);
9.247 + }
9.248 + else
9.249 + {
9.250 + colour = MOTION_FORWARD_COLOUR;
9.251 + point = *accelerationD;
9.252 + }
9.253 +
9.254 + lineColor(screen,
9.255 + SCREEN_WIDTH / 2,
9.256 + SCREEN_HEIGHT / 2,
9.257 + vectorf_dot(viewx, &point) * PROJECTION_FACTOR + SCREEN_WIDTH / 2,
9.258 + vectorf_dot(viewy, &point) * -PROJECTION_FACTOR + SCREEN_HEIGHT / 2,
9.259 + colour
9.260 + );
9.261 +}
9.262 +
9.263 +/**
9.264 + * Show the given point in space relative to the origin.
9.265 + */
9.266 +void gui_point(vectorf *viewx, vectorf *viewy, vectorf *viewz, vectorf *point, uint8_t r, uint8_t g, uint8_t b, uint8_t a)
9.267 +{
9.268 + int16_t x, y, radius;
9.269 + double z;
9.270 +
9.271 + if (fabs(vectorf_dot(viewz, point)) < 1)
9.272 + return;
9.273 +
9.274 + z = vectorf_dot(viewz, point);
9.275 + x = vectorf_dot(viewx, point) * PROJECTION_FACTOR / z + SCREEN_WIDTH / 2;
9.276 + y = vectorf_dot(viewy, point) * -PROJECTION_FACTOR / z + SCREEN_HEIGHT / 2;
9.277 + radius = abs(PROJECTION_FACTOR / z);
9.278 +
9.279 + if (vectorf_dot(viewz, point) < 0)
9.280 + circleRGBA(screen,
9.281 + x, y, radius,
9.282 + r, g, b, a
9.283 + );
9.284 + else
9.285 + filledCircleRGBA(screen,
9.286 + x, y, radius,
9.287 + r, g, b, a
9.288 + );
9.289 +}
9.290 +
9.291 +/**
9.292 + * Generate a view from the given point in space of the origin.
9.293 + */
9.294 +void gui_origin(vectorf *viewx, vectorf *viewy, vectorf *viewz, vectorf *point)
9.295 +{
9.296 + vectorf origin;
9.297 +
9.298 + vectorf_negate(point, &origin);
9.299 + gui_point(viewx, viewy, viewz, &origin, 255, 255, 255, 127);
9.300 +}
9.301 +
9.302 +/**
9.303 + * Draw a simple bar representation of the given vector.
9.304 + */
9.305 +void gui_bar(vectorf *value)
9.306 +{
9.307 + lineRGBA(screen,
9.308 + SCREEN_WIDTH / 2,
9.309 + SCREEN_HEIGHT / 2,
9.310 + SCREEN_WIDTH / 2 + value->x * SCREEN_WIDTH / 2,
9.311 + SCREEN_HEIGHT / 2 - value->y * SCREEN_HEIGHT / 2,
9.312 + 255, 255, 255, 255);
9.313 +
9.314 + circleRGBA(screen,
9.315 + SCREEN_WIDTH / 2 + value->x * SCREEN_WIDTH / 2,
9.316 + SCREEN_HEIGHT / 2 - value->y * SCREEN_HEIGHT / 2,
9.317 + fabs(value->z) * SCREEN_WIDTH / 2,
9.318 + 255, 255, 255, 255);
9.319 +}
9.320 +
9.321 +void gui_plot(uint16_t x, uint16_t y, uint8_t r, uint8_t g, uint8_t b, uint8_t a)
9.322 +{
9.323 + pixelRGBA(screen, x, y, r, g, b, a);
9.324 +}
9.325 +
9.326 +void gui_flush()
9.327 +{
9.328 + SDL_Flip(screen);
9.329 +}
9.330 +
9.331 +imu_ui_op gui_handle_events()
9.332 +{
9.333 + SDL_Event event;
9.334 +
9.335 + while (SDL_PollEvent(&event))
9.336 + {
9.337 + switch (event.type)
9.338 + {
9.339 + case SDL_KEYDOWN:
9.340 + switch (event.key.keysym.sym)
9.341 + {
9.342 + case SDLK_ESCAPE:
9.343 + case SDLK_q:
9.344 + return IMU_UI_OP_QUIT;
9.345 +
9.346 + case SDLK_c:
9.347 + return IMU_UI_OP_CALIBRATE;
9.348 +
9.349 + case SDLK_r:
9.350 + return IMU_UI_OP_RESET;
9.351 +
9.352 + case SDLK_p:
9.353 + return IMU_UI_OP_PAUSE;
9.354 +
9.355 + default:
9.356 + break;
9.357 + }
9.358 + break;
9.359 +
9.360 + case SDL_QUIT:
9.361 + return IMU_UI_OP_QUIT;
9.362 +
9.363 + default:
9.364 + break;
9.365 + }
9.366 + }
9.367 +
9.368 + return IMU_UI_OP_NULL;
9.369 +}
10.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
10.2 +++ b/gui.h Mon Oct 14 13:02:19 2013 +0000
10.3 @@ -0,0 +1,68 @@
10.4 +/*
10.5 + * Ben NanoNote graphical user interface utilities.
10.6 + *
10.7 + * Copyright (C) 2013 Paul Boddie
10.8 + *
10.9 + * This program is free software; you can redistribute it and/or modify
10.10 + * it under the terms of the GNU General Public License as published by
10.11 + * the Free Software Foundation; either version 2 of the License, or
10.12 + * (at your option) any later version.
10.13 + */
10.14 +
10.15 +#ifndef __GUI_H__
10.16 +#define __GUI_H__
10.17 +
10.18 +#include "SDL.h"
10.19 +#include "SDL_gfxPrimitives.h"
10.20 +#include "ui.h"
10.21 +#include "geo.h"
10.22 +
10.23 +typedef struct
10.24 +{
10.25 + uint16_t x, y;
10.26 + uint8_t r, g, b, a;
10.27 +} gui_printer;
10.28 +
10.29 +/* Device properties. */
10.30 +
10.31 +#define SCREEN_WIDTH 320
10.32 +#define SCREEN_HEIGHT 240
10.33 +
10.34 +#define SCREEN_COLUMN_WIDTH 8
10.35 +#define SCREEN_ROW_HEIGHT 8
10.36 +
10.37 +#define GUI_BUFSIZE 256
10.38 +
10.39 +#define PROJECTION_FACTOR 320
10.40 +#define SKY_GRID_STEP 15
10.41 +
10.42 +/* Colours in RGBA format. */
10.43 +
10.44 +#define SKY_UPPER_COLOUR 0xff0000ff
10.45 +#define SKY_LOWER_COLOUR 0x0000ffff
10.46 +#define MOTION_REVERSE_COLOUR 0xff00ffff
10.47 +#define MOTION_FORWARD_COLOUR 0x00ff00ff
10.48 +
10.49 +/* Access functions. */
10.50 +
10.51 +void gui_init();
10.52 +void gui_display_init();
10.53 +void gui_shutdown(int signum);
10.54 +void gui_shutdown_threaded(int signum);
10.55 +void gui_quit();
10.56 +void gui_clear();
10.57 +void gui_fill(uint8_t r, uint8_t g, uint8_t b);
10.58 +void gui_next_row(gui_printer *printer, uint16_t rows);
10.59 +void gui_next_column(gui_printer *printer, uint16_t columns);
10.60 +int gui_printf(const char *format, ...);
10.61 +void gui_sky(vectorf *viewx, vectorf *viewy, vectorf *viewz);
10.62 +void gui_sky_vertical(vectorf *viewx, vectorf *viewy, vectorf *viewz, int direction, int elevation, int16_t x, int16_t y, uint32_t colour);
10.63 +void gui_motion(vectorf *viewx, vectorf *viewy, vectorf *viewz, vectorf *accelerationD);
10.64 +void gui_point(vectorf *viewx, vectorf *viewy, vectorf *viewz, vectorf *point, uint8_t r, uint8_t g, uint8_t b, uint8_t a);
10.65 +void gui_origin(vectorf *viewx, vectorf *viewy, vectorf *viewz, vectorf *point);
10.66 +void gui_bar(vectorf *value);
10.67 +void gui_plot(uint16_t x, uint16_t y, uint8_t r, uint8_t g, uint8_t b, uint8_t a);
10.68 +void gui_flush();
10.69 +imu_ui_op gui_handle_events();
10.70 +
10.71 +#endif /* __GUI_H__ */
11.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
11.2 +++ b/i2c.c Mon Oct 14 13:02:19 2013 +0000
11.3 @@ -0,0 +1,183 @@
11.4 +/*
11.5 + * Ben NanoNote I2C communication.
11.6 + *
11.7 + * Copyright (C) 2013 Paul Boddie
11.8 + *
11.9 + * This program is free software; you can redistribute it and/or modify
11.10 + * it under the terms of the GNU General Public License as published by
11.11 + * the Free Software Foundation; either version 2 of the License, or
11.12 + * (at your option) any later version.
11.13 + */
11.14 +
11.15 +#include <time.h>
11.16 +#include "i2c.h"
11.17 +
11.18 +extern uint32_t I2C_SCL, I2C_SDA;
11.19 +
11.20 +void wait()
11.21 +{
11.22 + /* NOTE: Would want 600ns or 1300ns, but nanosleep appears too slow.
11.23 + struct timespec ts = {0, 600};
11.24 + nanosleep(&ts, NULL); */
11.25 +
11.26 + uint8_t i;
11.27 + for (i = 0; i < 200; i++);
11.28 +}
11.29 +
11.30 +/**
11.31 + * Declare the pins for initial I2C communications.
11.32 + */
11.33 +void i2c_init()
11.34 +{
11.35 + OUT(I2C_SCL);
11.36 + OUT(I2C_SDA);
11.37 +}
11.38 +
11.39 +/**
11.40 + * Initiate an I2C transaction.
11.41 + */
11.42 +void i2c_start()
11.43 +{
11.44 + OUT(I2C_SDA);
11.45 + CLR(I2C_SCL);
11.46 + wait();
11.47 + SET(I2C_SDA);
11.48 + SET(I2C_SCL);
11.49 + wait();
11.50 + CLR(I2C_SDA);
11.51 +}
11.52 +
11.53 +/**
11.54 + * Terminate an I2C transaction.
11.55 + */
11.56 +void i2c_stop()
11.57 +{
11.58 + OUT(I2C_SDA);
11.59 + CLR(I2C_SCL);
11.60 + wait();
11.61 + CLR(I2C_SDA);
11.62 + SET(I2C_SCL);
11.63 + wait();
11.64 + SET(I2C_SDA);
11.65 +}
11.66 +
11.67 +/**
11.68 + * Send an I2C acknowledgement to a transmitting device.
11.69 + */
11.70 +void i2c_ack(bool ack)
11.71 +{
11.72 + OUT(I2C_SDA);
11.73 +
11.74 + if (ack)
11.75 + CLR(I2C_SDA);
11.76 + else
11.77 + SET(I2C_SDA);
11.78 +
11.79 + SET(I2C_SCL);
11.80 + wait();
11.81 + CLR(I2C_SCL);
11.82 +
11.83 + IN(I2C_SDA);
11.84 +}
11.85 +
11.86 +/**
11.87 + * Receive a single byte from an I2C device as part of a transaction.
11.88 + */
11.89 +uint8_t i2c_recv()
11.90 +{
11.91 + uint8_t mask, result = 0;
11.92 +
11.93 + IN(I2C_SDA);
11.94 + CLR(I2C_SCL);
11.95 +
11.96 + for (mask = 0x80; mask; mask >>= 1)
11.97 + {
11.98 + SET(I2C_SCL);
11.99 + wait();
11.100 +
11.101 + if (PIN(I2C_SDA))
11.102 + result |= mask;
11.103 +
11.104 + CLR(I2C_SCL);
11.105 + wait();
11.106 + }
11.107 +
11.108 + return result;
11.109 +}
11.110 +
11.111 +/**
11.112 + * Receive into a buffer a transmission of the given length in bytes.
11.113 + */
11.114 +void i2c_recvmany(uint8_t *data, uint8_t len)
11.115 +{
11.116 + uint8_t *end = data + len;
11.117 +
11.118 + for (; data != end; data++, len--)
11.119 + {
11.120 + *data = i2c_recv();
11.121 + i2c_ack(len > 1);
11.122 + }
11.123 +}
11.124 +
11.125 +void i2c_wait()
11.126 +{
11.127 + IN(I2C_SCL);
11.128 + while (!PIN(I2C_SCL));
11.129 + OUT(I2C_SCL);
11.130 +}
11.131 +
11.132 +/**
11.133 + * Send a single byte of data to an I2C device as part of a transaction,
11.134 + * returning whether the transmission succeeded.
11.135 + */
11.136 +bool i2c_send(uint8_t data)
11.137 +{
11.138 + uint8_t mask;
11.139 + bool status;
11.140 +
11.141 + OUT(I2C_SDA);
11.142 + CLR(I2C_SCL);
11.143 +
11.144 + for (mask = 0x80; mask; mask >>= 1)
11.145 + {
11.146 + wait();
11.147 +
11.148 + if (data & mask)
11.149 + SET(I2C_SDA);
11.150 + else
11.151 + CLR(I2C_SDA);
11.152 +
11.153 + SET(I2C_SCL);
11.154 + wait();
11.155 + CLR(I2C_SCL);
11.156 + }
11.157 +
11.158 + /* Wait for acknowledgement, failing if none is given. */
11.159 +
11.160 + IN(I2C_SDA);
11.161 + SET(I2C_SCL);
11.162 + wait();
11.163 +
11.164 + status = PIN(I2C_SDA);
11.165 + CLR(I2C_SCL);
11.166 + return !status;
11.167 +}
11.168 +
11.169 +/**
11.170 + * Send from the buffer provided a transmission with the given length to an I2C
11.171 + * device.
11.172 + */
11.173 +bool i2c_sendmany(uint8_t *data, uint8_t len)
11.174 +{
11.175 + uint8_t *end = data + len;
11.176 +
11.177 + for (; data != end; data++)
11.178 + {
11.179 + if (!i2c_send(*data))
11.180 + return false;
11.181 +
11.182 + /* NOTE: Should test for the slave holding the clock signal low. */
11.183 + }
11.184 +
11.185 + return true;
11.186 +}
12.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
12.2 +++ b/i2c.h Mon Oct 14 13:02:19 2013 +0000
12.3 @@ -0,0 +1,39 @@
12.4 +/*
12.5 + * Ben NanoNote I2C communication.
12.6 + *
12.7 + * Copyright (C) 2013 Paul Boddie
12.8 + *
12.9 + * This program is free software; you can redistribute it and/or modify
12.10 + * it under the terms of the GNU General Public License as published by
12.11 + * the Free Software Foundation; either version 2 of the License, or
12.12 + * (at your option) any later version.
12.13 + */
12.14 +
12.15 +#ifndef __I2C_H__
12.16 +#define __I2C_H__
12.17 +
12.18 +#include <ubb/ubb.h>
12.19 +#include "bool.h"
12.20 +
12.21 +/* I2C modifiers. */
12.22 +
12.23 +#define I2C_READ 1
12.24 +#define I2C_WRITE 0
12.25 +
12.26 +/* Functions. */
12.27 +
12.28 +void i2c_init();
12.29 +void i2c_start();
12.30 +void i2c_stop();
12.31 +void i2c_ack(bool ack);
12.32 +uint8_t i2c_recv();
12.33 +void i2c_recvmany(uint8_t *data, uint8_t len);
12.34 +void i2c_wait();
12.35 +bool i2c_send(uint8_t data);
12.36 +bool i2c_sendmany(uint8_t *data, uint8_t len);
12.37 +
12.38 +/* Function aliases. */
12.39 +
12.40 +#define i2c_repeated_start i2c_start
12.41 +
12.42 +#endif /* __I2C_H__ */
13.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
13.2 +++ b/imu.c Mon Oct 14 13:02:19 2013 +0000
13.3 @@ -0,0 +1,318 @@
13.4 +/*
13.5 + * Ben NanoNote communication with the Pololu MinIMU-9 with the L3G4200D 3-axis
13.6 + * gyroscope and LSM303DLM accelerometer/magnetometer.
13.7 + *
13.8 + * http://www.pololu.com/catalog/product/1265
13.9 + *
13.10 + * Copyright (C) 2013 Paul Boddie
13.11 + *
13.12 + * This program is free software; you can redistribute it and/or modify
13.13 + * it under the terms of the GNU General Public License as published by
13.14 + * the Free Software Foundation; either version 2 of the License, or
13.15 + * (at your option) any later version.
13.16 + */
13.17 +
13.18 +#include <string.h>
13.19 +#include <unistd.h>
13.20 +#include "imu.h"
13.21 +
13.22 +const uint32_t I2C_SCL = IMU_SCL, I2C_SDA = IMU_SDA;
13.23 +
13.24 +/**
13.25 + * Receive data from the indicated device, using the given device register, and
13.26 + * storing the result in the buffer provided, expecting a transmission of the
13.27 + * specified length in bytes. Return whether the transaction was successful.
13.28 + */
13.29 +bool imu_recv(uint8_t device, uint8_t reg, uint8_t *result, uint8_t len)
13.30 +{
13.31 + uint8_t data[] = {device | I2C_WRITE, reg};
13.32 +
13.33 + memset(result, 0, len);
13.34 +
13.35 + /* Select the register. */
13.36 +
13.37 + i2c_start();
13.38 + if (!i2c_sendmany(data, 2))
13.39 + {
13.40 + i2c_stop();
13.41 + return false;
13.42 + }
13.43 +
13.44 + /* Continue with a read from the device. */
13.45 +
13.46 + i2c_repeated_start();
13.47 + if (!i2c_send(device | I2C_READ))
13.48 + {
13.49 + i2c_stop();
13.50 + return false;
13.51 + }
13.52 + i2c_recvmany(result, len);
13.53 +
13.54 + /* Stop the communication. */
13.55 +
13.56 + i2c_stop();
13.57 + return true;
13.58 +}
13.59 +
13.60 +/**
13.61 + * Send a single byte of data to the indicated device, using the given device
13.62 + * register. Return whether the transaction was successful.
13.63 + */
13.64 +bool imu_sendone(uint8_t device, uint8_t reg, uint8_t value)
13.65 +{
13.66 + uint8_t data[] = {device | I2C_WRITE, reg, value};
13.67 +
13.68 + /* Select the register. */
13.69 +
13.70 + i2c_start();
13.71 + if (!i2c_sendmany(data, 3))
13.72 + {
13.73 + i2c_stop();
13.74 + return false;
13.75 + }
13.76 +
13.77 + /* Stop the communication. */
13.78 +
13.79 + i2c_stop();
13.80 + return true;
13.81 +}
13.82 +
13.83 +/**
13.84 + * Convert the first two bytes of the given raw data to a signed integer.
13.85 + */
13.86 +int16_t convert(uint8_t raw[])
13.87 +{
13.88 + uint16_t raw16 = raw[0] | raw[1] << 8;
13.89 + if (raw16 & 0x8000)
13.90 + return -(raw16 ^ 0xffff) - 1;
13.91 + else
13.92 + return raw16;
13.93 +}
13.94 +
13.95 +/**
13.96 + * Convert the first two bytes of the given raw data to a signed integer,
13.97 + * discarding the least significant four bits of the raw data.
13.98 + */
13.99 +int16_t convert12(uint8_t raw[])
13.100 +{
13.101 + uint16_t raw16 = ((raw[0] >> 4) & 0x0f) | (raw[1] << 4);
13.102 + if (raw16 & 0x0800)
13.103 + return -(raw16 ^ 0x0fff) - 1;
13.104 + else
13.105 + return raw16;
13.106 +}
13.107 +
13.108 +/**
13.109 + * Convert the first two bytes of the given raw data to a signed integer,
13.110 + * discarding the most significant four bits of the raw data.
13.111 + */
13.112 +int16_t convertBE12L(uint8_t raw[])
13.113 +{
13.114 + uint16_t raw16 = ((raw[0] & 0x0f) << 8) | raw[1];
13.115 + if (raw16 & 0x0800)
13.116 + return -(raw16 ^ 0x0fff) - 1;
13.117 + else
13.118 + return raw16;
13.119 +}
13.120 +
13.121 +/**
13.122 + * Receive data from the indicated device, using the given device register, and
13.123 + * storing the result in the output parameters provided. The given converter
13.124 + * function will convert the raw data to the output form. Return whether the
13.125 + * transaction was successful.
13.126 + */
13.127 +bool imu_read_vector(uint8_t device, uint8_t reg, vectorf *out, int16_t (*converter)(uint8_t *))
13.128 +{
13.129 + uint8_t result[6];
13.130 +
13.131 + if (!imu_recv(device, reg, result, 6))
13.132 + return false;
13.133 +
13.134 + out->x = converter(result);
13.135 + out->y = converter(result + 2);
13.136 + out->z = converter(result + 4);
13.137 +
13.138 + return true;
13.139 +}
13.140 +
13.141 +bool imu_read_vector_xzy(uint8_t device, uint8_t reg, vectorf *out, int16_t (*converter)(uint8_t *))
13.142 +{
13.143 + double tmp;
13.144 +
13.145 + if (imu_read_vector(device, reg, out, converter))
13.146 + {
13.147 + tmp = out->y;
13.148 + out->y = out->z;
13.149 + out->z = tmp;
13.150 + return true;
13.151 + }
13.152 +
13.153 + return false;
13.154 +}
13.155 +
13.156 +/**
13.157 + * Return the given value scaled to the range defined by lower, middle and upper
13.158 + * points.
13.159 + */
13.160 +double scale(double value, double lower, double middle, double upper)
13.161 +{
13.162 + if (value < middle)
13.163 + return (value - middle) / (middle - lower);
13.164 + else
13.165 + return (value - middle) / (upper - middle);
13.166 +}
13.167 +
13.168 +/**
13.169 + * Normalise the given vector based on the minimum and maximum values.
13.170 + */
13.171 +void normalise(vectorf *in, vectorf *vmin, vectorf *vmax, vectorf *out)
13.172 +{
13.173 + out->x = scale(in->x, vmin->x, (vmin->x + vmax->x) / 2, vmax->x);
13.174 + out->y = scale(in->y, vmin->y, (vmin->y + vmax->y) / 2, vmax->y);
13.175 + out->z = scale(in->z, vmin->z, (vmin->z + vmax->z) / 2, vmax->z);
13.176 +}
13.177 +
13.178 +/**
13.179 + * Add a measurement to a circular buffer usable for filtering.
13.180 + */
13.181 +void queue(vectorf values[], int *oldest, int length, vectorf *value)
13.182 +{
13.183 + values[*oldest] = *value;
13.184 + *oldest = (*oldest + 1) % length;
13.185 +}
13.186 +
13.187 +/**
13.188 + * Return a filtered measurement using the given values from a circular buffer
13.189 + * with the specified oldest measurement index, and with the buffer having the
13.190 + * given length. The filtered measurement is placed in the result vector.
13.191 + */
13.192 +void filter(vectorf values[], int oldest, int length, vectorf *result)
13.193 +{
13.194 + int i;
13.195 +
13.196 + result->x = 0;
13.197 + result->y = 0;
13.198 + result->z = 0;
13.199 +
13.200 + /* NOTE: For now, a plain average is used. */
13.201 +
13.202 + for (i = 0; i < length; i++)
13.203 + {
13.204 + result->x += values[i].x;
13.205 + result->y += values[i].y;
13.206 + result->z += values[i].z;
13.207 + }
13.208 +
13.209 + result->x /= length;
13.210 + result->y /= length;
13.211 + result->z /= length;
13.212 +}
13.213 +
13.214 +/**
13.215 + * Populate the given zero base/levels for the device with the given address,
13.216 + * using the specified register to obtain measurements, performing the given
13.217 + * number of warm-up measurements and then proper readings, with the specified
13.218 + * delay between each one.
13.219 + */
13.220 +void calibrate(vectorf *zerolevel, vectorf zerolevels[], int length, uint8_t address, uint8_t reg, uint16_t delay, int16_t (*converter)(uint8_t *))
13.221 +{
13.222 + int reading = 0, index = 0;
13.223 + vectorf level;
13.224 +
13.225 + zerolevel->x = 0;
13.226 + zerolevel->y = 0;
13.227 + zerolevel->z = 0;
13.228 +
13.229 + while (reading < IMU_CALIBRATION_WARMUP + IMU_CALIBRATION_READINGS)
13.230 + {
13.231 + if (imu_read_vector(address, reg, &zerolevels[index], converter))
13.232 + {
13.233 + index = (index + 1) % length;
13.234 +
13.235 + /* Ignore the first few readings. */
13.236 +
13.237 + if (reading >= IMU_CALIBRATION_WARMUP)
13.238 + {
13.239 + filter(zerolevels, index, length, &level);
13.240 + zerolevel->x += level.x;
13.241 + zerolevel->y += level.y;
13.242 + zerolevel->z += level.z;
13.243 + }
13.244 +
13.245 + reading += 1;
13.246 + }
13.247 +
13.248 + usleep(delay);
13.249 + }
13.250 +
13.251 + zerolevel->x /= IMU_CALIBRATION_READINGS;
13.252 + zerolevel->y /= IMU_CALIBRATION_READINGS;
13.253 + zerolevel->z /= IMU_CALIBRATION_READINGS;
13.254 +}
13.255 +
13.256 +/* Accelerometer-specific functions. */
13.257 +
13.258 +void average_filter(vectorf *value, vectorf buffer[], int *index, int length)
13.259 +{
13.260 + queue(buffer, index, length, value);
13.261 + filter(buffer, *index, length, value);
13.262 +}
13.263 +
13.264 +void noise_filter(vectorf *value, double threshold)
13.265 +{
13.266 + value->x = noise(value->x, threshold);
13.267 + value->y = noise(value->y, threshold);
13.268 + value->z = noise(value->z, threshold);
13.269 +}
13.270 +
13.271 +void apply_acceleration(double acc, double acc_old, double *pos, double *neg, double seconds)
13.272 +{
13.273 + if (acc > 0)
13.274 + *pos += (acc + acc_old) / 2 * seconds;
13.275 + else
13.276 + *neg += (acc + acc_old) / 2 * seconds;
13.277 +}
13.278 +
13.279 +void apply_decay(double acc, double* pos, double* neg)
13.280 +{
13.281 + double decay = 1.0;
13.282 +
13.283 + if (*pos < -*neg)
13.284 + {
13.285 + if ((acc < -ACCEL_THRESHOLD) && (*pos > ACCEL_SUM_THRESHOLD))
13.286 + decay = VELOCITY_DECAY_SEVERE;
13.287 + if (fabs(acc) <= ACCEL_THRESHOLD)
13.288 + decay = VELOCITY_DECAY_GRADUAL;
13.289 + *neg *= decay;
13.290 + }
13.291 + if (*pos > -*neg)
13.292 + {
13.293 + if ((acc > ACCEL_THRESHOLD) && (*neg < -ACCEL_SUM_THRESHOLD))
13.294 + decay = VELOCITY_DECAY_SEVERE;
13.295 + if (fabs(acc) <= ACCEL_THRESHOLD)
13.296 + decay = VELOCITY_DECAY_GRADUAL;
13.297 + *pos *= decay;
13.298 + }
13.299 +}
13.300 +
13.301 +void update_velocity(vectorf *velocity, vectorf *acceleration, vectorf *acceleration_old, vectorf *apos, vectorf *aneg, double seconds)
13.302 +{
13.303 + apply_acceleration(acceleration->x, acceleration_old->x, &(apos->x), &(aneg->x), seconds);
13.304 + apply_acceleration(acceleration->y, acceleration_old->y, &(apos->y), &(aneg->y), seconds);
13.305 + apply_acceleration(acceleration->z, acceleration_old->z, &(apos->z), &(aneg->z), seconds);
13.306 +
13.307 + apply_decay(acceleration->x, &(apos->x), &(aneg->x));
13.308 + apply_decay(acceleration->y, &(apos->y), &(aneg->y));
13.309 + apply_decay(acceleration->z, &(apos->z), &(aneg->z));
13.310 +
13.311 + velocity->x = apos->x + aneg->x;
13.312 + velocity->y = apos->y + aneg->y;
13.313 + velocity->z = apos->z + aneg->z;
13.314 +}
13.315 +
13.316 +void update_displacement(vectorf *displacement, vectorf *velocity, vectorf *velocity_old, double seconds)
13.317 +{
13.318 + displacement->x += (velocity->x + velocity_old->x) / 2 * seconds;
13.319 + displacement->y += (velocity->y + velocity_old->y) / 2 * seconds;
13.320 + displacement->z += (velocity->z + velocity_old->z) / 2 * seconds;
13.321 +}
14.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
14.2 +++ b/imu.h Mon Oct 14 13:02:19 2013 +0000
14.3 @@ -0,0 +1,260 @@
14.4 +/*
14.5 + * Ben NanoNote communication with the Pololu MinIMU-9 with the L3G4200D 3-axis
14.6 + * gyroscope and LSM303DLM accelerometer/magnetometer.
14.7 + *
14.8 + * http://www.pololu.com/catalog/product/1265
14.9 + *
14.10 + * Copyright (C) 2013 Paul Boddie
14.11 + *
14.12 + * This program is free software; you can redistribute it and/or modify
14.13 + * it under the terms of the GNU General Public License as published by
14.14 + * the Free Software Foundation; either version 2 of the License, or
14.15 + * (at your option) any later version.
14.16 + */
14.17 +
14.18 +#ifndef __IMU_H__
14.19 +#define __IMU_H__
14.20 +
14.21 +#include <ubb/ubb.h>
14.22 +#include "i2c.h"
14.23 +#include "geo.h"
14.24 +
14.25 +/* Pin assignments:
14.26 + *
14.27 + * Sniffer UBB MinIMU-9
14.28 + * ------- ---- --------
14.29 + * DAT2 DAT2
14.30 + * CD DAT3 SCL
14.31 + * CMD CMD SDA
14.32 + * VCC VDD VIN
14.33 + * CLK CLK (3V)
14.34 + * GND GND GND
14.35 + * DAT0 DAT0 (1V8)
14.36 + * DAT1 DAT1
14.37 + *
14.38 + * The 3V and 1V8 pins play no role in the communication, but a six pin header
14.39 + * exposing these pins on the IMU can be used to connect the IMU with the
14.40 + * Sniffer/UBB without any complications.
14.41 + */
14.42 +
14.43 +#define IMU_SCL UBB_DAT3
14.44 +#define IMU_SDA UBB_CMD
14.45 +#define IMU_3V UBB_CLK
14.46 +#define IMU_1V8 UBB_DAT0
14.47 +
14.48 +/* I2C addresses and operations. */
14.49 +
14.50 +#define IMU_GYRO_ADDRESS 0xd2 /* 1101001x */
14.51 +#define IMU_ACCEL_ADDRESS 0x30 /* 0011000x */
14.52 +#define IMU_MAGNET_ADDRESS 0x3c /* 0011110x */
14.53 +
14.54 +/* Gyroscope registers. */
14.55 +
14.56 +#define IMU_GYRO_WHO_AM_I 0x0f
14.57 +#define IMU_GYRO_CTRL_REG1 0x20
14.58 +#define IMU_GYRO_CTRL_REG4 0x23
14.59 +#define IMU_GYRO_CTRL_REG5 0x24
14.60 +#define IMU_GYRO_OUT_TEMP 0x26
14.61 +#define IMU_GYRO_STATUS_REG 0x27
14.62 +
14.63 +/* Gyroscope register values. */
14.64 +
14.65 +#define IMU_GYRO_CTRL_REG1_PD 0x08
14.66 +#define IMU_GYRO_CTRL_REG1_ZEN 0x04
14.67 +#define IMU_GYRO_CTRL_REG1_YEN 0x02
14.68 +#define IMU_GYRO_CTRL_REG1_XEN 0x01
14.69 +#define IMU_GYRO_CTRL_REG1_ALL 0x0f /* composite of power and all axes */
14.70 +#define IMU_GYRO_CTRL_REG4_BDU 0x80
14.71 +#define IMU_GYRO_CTRL_REG4_250DPS 0x00 /* FS1 = 0; FS0 = 0 */
14.72 +#define IMU_GYRO_CTRL_REG4_500DPS 0x10 /* FS1 = 0; FS0 = 1 */
14.73 +#define IMU_GYRO_CTRL_REG4_2000DPS 0x20 /* FS1 = 1; FS0 = 0 */
14.74 +#define IMU_GYRO_CTRL_REG5_BOOT 0x80
14.75 +#define IMU_GYRO_CTRL_REG5_HPEN 0x10
14.76 +#define IMU_GYRO_CTRL_REG5_OUTSEL1 0x02
14.77 +#define IMU_GYRO_CTRL_REG5_OUTSEL0 0x01
14.78 +#define IMU_GYRO_OUT_X_L 0x28
14.79 +#define IMU_GYRO_OUT_X_H 0x29
14.80 +#define IMU_GYRO_OUT_Y_L 0x2a
14.81 +#define IMU_GYRO_OUT_Y_H 0x2b
14.82 +#define IMU_GYRO_OUT_Z_L 0x2c
14.83 +#define IMU_GYRO_OUT_Z_H 0x2d
14.84 +#define IMU_GYRO_READ_MANY 0x80 /* gyroscope register increment */
14.85 +
14.86 +/* Accelerometer registers. */
14.87 +
14.88 +#define IMU_ACCEL_CTRL_REG1_A 0x20
14.89 +#define IMU_ACCEL_CTRL_REG2_A 0x21
14.90 +#define IMU_ACCEL_CTRL_REG4_A 0x23
14.91 +#define IMU_ACCEL_HP_FILTER_RESET_A 0x25
14.92 +#define IMU_ACCEL_OUT_X_L_A 0x28
14.93 +#define IMU_ACCEL_OUT_X_H_A 0x29
14.94 +#define IMU_ACCEL_OUT_Y_L_A 0x2a
14.95 +#define IMU_ACCEL_OUT_Y_H_A 0x2b
14.96 +#define IMU_ACCEL_OUT_Z_L_A 0x2c
14.97 +#define IMU_ACCEL_OUT_Z_H_A 0x2d
14.98 +#define IMU_ACCEL_READ_MANY 0x80 /* accelerometer register increment */
14.99 +
14.100 +/* Accelerometer register values. */
14.101 +
14.102 +#define IMU_ACCEL_CTRL_REG1_50HZ 0x20 /* normal power */
14.103 +#define IMU_ACCEL_CTRL_REG1_100HZ 0x28 /* normal power */
14.104 +#define IMU_ACCEL_CTRL_REG1_400HZ 0x30 /* normal power */
14.105 +#define IMU_ACCEL_CTRL_REG1_1000HZ 0x38 /* normal power */
14.106 +#define IMU_ACCEL_CTRL_REG1_05HZ 0x40 /* low power, 0.5Hz */
14.107 +#define IMU_ACCEL_CTRL_REG1_1HZ 0x60 /* low power, 1Hz */
14.108 +#define IMU_ACCEL_CTRL_REG1_2HZ 0x80 /* low power, 2Hz */
14.109 +#define IMU_ACCEL_CTRL_REG1_5HZ 0x90 /* low power, 5Hz */
14.110 +#define IMU_ACCEL_CTRL_REG1_10HZ 0xa0 /* low power, 10Hz */
14.111 +#define IMU_ACCEL_CTRL_REG1_ZEN 0x04
14.112 +#define IMU_ACCEL_CTRL_REG1_YEN 0x02
14.113 +#define IMU_ACCEL_CTRL_REG1_XEN 0x01
14.114 +#define IMU_ACCEL_CTRL_REG1_ALL 0x07 /* composite of all axes */
14.115 +#define IMU_ACCEL_CTRL_REG2_FDS 0x10
14.116 +#define IMU_ACCEL_CTRL_REG2_50 0x00 /* filter cut off is rate/50 */
14.117 +#define IMU_ACCEL_CTRL_REG2_100 0x01 /* filter cut off is rate/100 */
14.118 +#define IMU_ACCEL_CTRL_REG2_200 0x02 /* filter cut off is rate/200 */
14.119 +#define IMU_ACCEL_CTRL_REG2_400 0x03 /* filter cut off is rate/400 */
14.120 +#define IMU_ACCEL_CTRL_REG2_HPCF1 0x02
14.121 +#define IMU_ACCEL_CTRL_REG4_BDU 0x80
14.122 +#define IMU_ACCEL_CTRL_REG4_2G 0x00 /* FS1 = 0; FS0 = 0 */
14.123 +#define IMU_ACCEL_CTRL_REG4_4G 0x10 /* FS1 = 0; FS0 = 1 */
14.124 +#define IMU_ACCEL_CTRL_REG4_8G 0x30 /* FS1 = 1; FS0 = 1 */
14.125 +
14.126 +/* Magnetometer registers. */
14.127 +
14.128 +#define IMU_MAGNET_CRA_REG_M 0x00
14.129 +#define IMU_MAGNET_CRB_REG_M 0x01
14.130 +#define IMU_MAGNET_MR_REG_M 0x02
14.131 +#define IMU_MAGNET_OUT_X_H_M 0x03
14.132 +#define IMU_MAGNET_OUT_X_L_M 0x04
14.133 +#define IMU_MAGNET_OUT_Z_H_M 0x05
14.134 +#define IMU_MAGNET_OUT_Z_L_M 0x06
14.135 +#define IMU_MAGNET_OUT_Y_H_M 0x07
14.136 +#define IMU_MAGNET_OUT_Y_L_M 0x08
14.137 +#define IMU_MAGNET_SR_REG_M 0x09
14.138 +#define IMU_MAGNET_IRA_REG_M 0x0a
14.139 +#define IMU_MAGNET_IRB_REG_M 0x0b
14.140 +#define IMU_MAGNET_IRC_REG_M 0x0c
14.141 +#define IMU_MAGNET_WHO_AM_I_M 0x0f
14.142 +
14.143 +/* Magnetometer register values. */
14.144 +
14.145 +#define IMU_MAGNET_CRA_REG_0_75HZ 0x00 /* 0.75Hz */
14.146 +#define IMU_MAGNET_CRA_REG_1_5HZ 0x04 /* 1.5Hz */
14.147 +#define IMU_MAGNET_CRA_REG_3HZ 0x08 /* 3Hz */
14.148 +#define IMU_MAGNET_CRA_REG_7_5HZ 0x0c /* 7.5Hz */
14.149 +#define IMU_MAGNET_CRA_REG_15HZ 0x10 /* 15Hz */
14.150 +#define IMU_MAGNET_CRA_REG_30HZ 0x14 /* 30Hz */
14.151 +#define IMU_MAGNET_CRA_REG_75HZ 0x18 /* 75Hz */
14.152 +#define IMU_MAGNET_CRA_REG_220HZ 0x1c /* 220Hz */
14.153 +#define IMU_MAGNET_CRB_REG_1_3G 0x20 /* 1.3G */
14.154 +#define IMU_MAGNET_CRB_REG_1_9G 0x40 /* 1.9G */
14.155 +#define IMU_MAGNET_CRB_REG_2_5G 0x60 /* 2.5G */
14.156 +#define IMU_MAGNET_CRB_REG_4_0G 0x80 /* 4.0G */
14.157 +#define IMU_MAGNET_CRB_REG_4_7G 0xa0 /* 4.7G */
14.158 +#define IMU_MAGNET_CRB_REG_5_6G 0xc0 /* 5.6G */
14.159 +#define IMU_MAGNET_CRB_REG_8_1G 0xe0 /* 8.1G */
14.160 +#define IMU_MAGNET_MR_REG_CONT 0x00
14.161 +#define IMU_MAGNET_MR_REG_SINGLE 0x01
14.162 +#define IMU_MAGNET_MR_REG_SLEEP 0x02
14.163 +
14.164 +/* Common values. */
14.165 +
14.166 +#define IMU_250DPS_UNIT 8750 /* µdps -> 8.75mdps */
14.167 +#define IMU_500DPS_UNIT 17500 /* µdps -> 17.5mdps */
14.168 +#define IMU_2000DPS_UNIT 70000 /* µdps -> 70mdps */
14.169 +
14.170 +#define IMU_2G_UNIT 1000 /* µg -> 1mg */
14.171 +#define IMU_4G_UNIT 2000 /* µg -> 2mg */
14.172 +#define IMU_8G_UNIT 3900 /* µg -> 3.9mg */
14.173 +
14.174 +#define IMU_1_3G_UNIT 909 /* µG (1G / 1100) */
14.175 +#define IMU_1_9G_UNIT 1169 /* µG (1G / 855) */
14.176 +#define IMU_2_5G_UNIT 1492 /* µG (1G / 670) */
14.177 +#define IMU_4_0G_UNIT 2222 /* µG (1G / 450) */
14.178 +#define IMU_4_7G_UNIT 2500 /* µG (1G / 400) */
14.179 +#define IMU_5_6G_UNIT 3030 /* µG (1G / 330) */
14.180 +#define IMU_8_1G_UNIT 4347 /* µG (1G / 230) */
14.181 +
14.182 +#define IMU_MAGNET_Z_XY_RATIO (9.0 / 8.0) /* adjustments to the above for the smaller Z scale */
14.183 +
14.184 +/* Measurement parameters. */
14.185 +
14.186 +#define IMU_UPDATE_PERIOD 10000 /* µs -> 10ms == 0.01s (100Hz) */
14.187 +#define IMU_MAGNET_UPDATE_PERIOD 30000 /* µs -> 30ms == 0.03s (33Hz) */
14.188 +#define TEXT_UPDATE_PERIOD 500000 /* µs */
14.189 +#define GUI_UPDATE_PERIOD 100000 /* µs */
14.190 +
14.191 +#define IMU_CALIBRATION_WARMUP 100
14.192 +#define IMU_CALIBRATION_READINGS 100
14.193 +
14.194 +#define IMU_ACCEL_BUFFER_SIZE 10
14.195 +
14.196 +#define IMU_UDPS_FACTOR IMU_2000DPS_UNIT
14.197 +#define IMU_GYRO_DPS_SCALE IMU_GYRO_CTRL_REG4_2000DPS
14.198 +
14.199 +#define IMU_UG_FACTOR IMU_2G_UNIT
14.200 +#define IMU_ACCEL_SCALE IMU_ACCEL_CTRL_REG4_2G
14.201 +#define IMU_ACCEL_FREQ IMU_ACCEL_CTRL_REG1_50HZ
14.202 +#define IMU_ACCEL_FILTER_FREQ IMU_ACCEL_CTRL_REG2_50
14.203 +
14.204 +#define IMU_UGAUSS_FACTOR IMU_4_0G_UNIT
14.205 +#define IMU_MAGNET_SCALE IMU_MAGNET_CRB_REG_4_0G
14.206 +#define IMU_MAGNET_FREQ IMU_MAGNET_CRA_REG_30HZ
14.207 +
14.208 +#define ACCEL_G 9.81 /* ms**-2 */
14.209 +
14.210 +#define ACCEL_THRESHOLD 0.1 /* g */
14.211 +#define ACCEL_SUM_THRESHOLD 0.01 /* gs */
14.212 +#define VELOCITY_DECAY_SEVERE 0.5
14.213 +#define VELOCITY_DECAY_GRADUAL 0.9
14.214 +
14.215 +#define ACCEL_REST_MAG_LOWER 0.995
14.216 +#define ACCEL_REST_MAG_UPPER 1.005
14.217 +#define ROTATION_ADJUSTMENT_FACTOR 0.1
14.218 +
14.219 +/* Convert microdegrees * microseconds to degrees. */
14.220 +
14.221 +#define to_angle(x) ((double) (x) / ((uint64_t) 1000000000000))
14.222 +
14.223 +/* Convert microg to g. */
14.224 +
14.225 +#define to_accel(x) ((double) (x) / 1000000)
14.226 +
14.227 +/* Convert microgauss to gauss. */
14.228 +
14.229 +#define to_field(x) to_accel(x)
14.230 +
14.231 +/* Get a period in microseconds. */
14.232 +
14.233 +#define get_period(now, then) ((now.tv_sec - then.tv_sec) * 1000000 + (now.tv_usec - then.tv_usec))
14.234 +
14.235 +/* Function definitions. */
14.236 +
14.237 +bool imu_recv(uint8_t device, uint8_t reg, uint8_t *result, uint8_t len);
14.238 +bool imu_sendone(uint8_t device, uint8_t reg, uint8_t value);
14.239 +int16_t convert(uint8_t raw[]);
14.240 +int16_t convert12(uint8_t raw[]);
14.241 +int16_t convertBE12L(uint8_t raw[]);
14.242 +bool imu_read_vector(uint8_t device, uint8_t reg, vectorf *out, int16_t (*converter)(uint8_t *));
14.243 +bool imu_read_vector_xzy(uint8_t device, uint8_t reg, vectorf *out, int16_t (*converter)(uint8_t *));
14.244 +double scale(double value, double lower, double middle, double upper);
14.245 +void normalise(vectorf *in, vectorf *vmin, vectorf *vmax, vectorf *out);
14.246 +void queue(vectorf values[], int *oldest, int length, vectorf *value);
14.247 +void filter(vectorf values[], int oldest, int length, vectorf *result);
14.248 +void calibrate(vectorf *zerolevel, vectorf zerolevels[], int length, uint8_t address, uint8_t reg, uint16_t delay, int16_t (*converter)(uint8_t *));
14.249 +
14.250 +/* Accelerometer-specific functions. */
14.251 +
14.252 +void average_filter(vectorf *value, vectorf buffer[], int *index, int length);
14.253 +void noise_filter(vectorf *value, double threshold);
14.254 +void apply_acceleration(double acc, double acc_old, double *pos, double *neg, double seconds);
14.255 +void apply_decay(double acc, double* pos, double* neg);
14.256 +void update_velocity(vectorf *velocity, vectorf *acceleration, vectorf *acceleration_old, vectorf *apos, vectorf *aneg, double seconds);
14.257 +void update_displacement(vectorf *displacement, vectorf *velocity, vectorf *velocity_old, double seconds);
14.258 +
14.259 +/* Function aliases. */
14.260 +
14.261 +#define imu_init i2c_init
14.262 +
14.263 +#endif /* __IMU_H__ */
15.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
15.2 +++ b/itest.c Mon Oct 14 13:02:19 2013 +0000
15.3 @@ -0,0 +1,176 @@
15.4 +/*
15.5 + * Test Pololu MinIMU-9 measurements.
15.6 + *
15.7 + * Copyright (C) 2013 Paul Boddie
15.8 + *
15.9 + * This program is free software; you can redistribute it and/or modify
15.10 + * it under the terms of the GNU General Public License as published by
15.11 + * the Free Software Foundation; either version 2 of the License, or
15.12 + * (at your option) any later version.
15.13 + */
15.14 +
15.15 +#include <stdio.h>
15.16 +#include <stdlib.h>
15.17 +#include <signal.h>
15.18 +#include <string.h>
15.19 +#include <unistd.h>
15.20 +#include <sys/time.h>
15.21 +#include <pthread.h>
15.22 +#include "imu.h"
15.23 +#include "shutdown.h"
15.24 +#include "gui.h"
15.25 +#include "measure.h"
15.26 +
15.27 +int crosses_axis(double current, double previous)
15.28 +{
15.29 + if ((current == 0) && (previous != 0))
15.30 + return -sign(previous);
15.31 + if ((current != previous) && (sign(current) == -sign(previous)))
15.32 + return sign(current);
15.33 + return 0;
15.34 +}
15.35 +
15.36 +/* Main program. */
15.37 +
15.38 +int main(int argc, char *argv[])
15.39 +{
15.40 + void *threadresult;
15.41 + uint8_t result[6];
15.42 + int readings;
15.43 + uint32_t period;
15.44 + struct timeval now, ui_updated;
15.45 + bool paused = false, using_filter = false;
15.46 +
15.47 + signal(SIGINT, init_shutdown);
15.48 +
15.49 + /* Access the 8:10 port. */
15.50 +
15.51 + if (ubb_open(0) < 0) {
15.52 + perror("ubb_open");
15.53 + return 1;
15.54 + }
15.55 +
15.56 + ubb_power(1);
15.57 + printf("Power on.\n");
15.58 +
15.59 + /* Bring the IMU up. */
15.60 +
15.61 + imu_init();
15.62 +
15.63 + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG1, IMU_GYRO_CTRL_REG1_ALL);
15.64 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG1_A, IMU_ACCEL_CTRL_REG1_ALL | IMU_ACCEL_CTRL_REG1_50HZ);
15.65 +
15.66 + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG5, 0);
15.67 +
15.68 + if ((argc > 1) && (strcmp(argv[1], "-f") == 0))
15.69 + {
15.70 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, IMU_ACCEL_CTRL_REG2_FDS | IMU_ACCEL_FILTER_FREQ);
15.71 + imu_recv(IMU_ACCEL_ADDRESS, IMU_ACCEL_HP_FILTER_RESET_A, result, 1);
15.72 + using_filter = true;
15.73 + }
15.74 + else
15.75 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, 0);
15.76 +
15.77 + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG4, IMU_GYRO_CTRL_REG4_BDU | IMU_GYRO_DPS_SCALE);
15.78 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG4_A, IMU_ACCEL_CTRL_REG4_BDU | IMU_ACCEL_SCALE);
15.79 +
15.80 + if (imu_recv(IMU_GYRO_ADDRESS, IMU_GYRO_WHO_AM_I, result, 1))
15.81 + printf("Who am I? %x\n", result[0]);
15.82 +
15.83 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_WHO_AM_I_M, result, 1))
15.84 + printf("Who am I? %x\n", result[0]);
15.85 +
15.86 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRA_REG_M, result, 1))
15.87 + printf("Identification A? %x\n", result[0]);
15.88 +
15.89 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRB_REG_M, result, 1))
15.90 + printf("Identification B? %x\n", result[0]);
15.91 +
15.92 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRC_REG_M, result, 1))
15.93 + printf("Identification C? %x\n", result[0]);
15.94 +
15.95 + gui_init();
15.96 + gui_display_init();
15.97 +
15.98 + signal(SIGINT, gui_shutdown);
15.99 +
15.100 + memset(accelerationB, 0, sizeof(accelerationB));
15.101 +
15.102 + ui_calibrate(using_filter, gui_printf, gui_flush);
15.103 +
15.104 + memset(accelerationB, 0, sizeof(accelerationB));
15.105 +
15.106 + readings = 0;
15.107 + gui_clear();
15.108 + gui_printf("A0: % 8.1f, % 8.1f, % 8.1f\n", acceleration0.x, acceleration0.y, acceleration0.z);
15.109 +
15.110 + /* Create a measurement thread. */
15.111 +
15.112 + if (pthread_create(&thread, NULL, get_measurements, &using_filter) != 0)
15.113 + {
15.114 + perror("pthread_create");
15.115 + gui_shutdown(0);
15.116 + }
15.117 +
15.118 + signal(SIGINT, gui_shutdown_threaded);
15.119 +
15.120 + pthread_mutex_init(&mutex, NULL);
15.121 +
15.122 + gettimeofday(&ui_updated, NULL);
15.123 +
15.124 + while (1)
15.125 + {
15.126 + gettimeofday(&now, NULL);
15.127 +
15.128 + period = get_period(now, ui_updated);
15.129 +
15.130 + if ((period >= IMU_UPDATE_PERIOD) && (!paused))
15.131 + {
15.132 + ui_updated = now;
15.133 + readings = (readings + 1) % SCREEN_WIDTH;
15.134 +
15.135 + if (readings == 0)
15.136 + gui_clear();
15.137 +
15.138 + pthread_mutex_lock(&mutex);
15.139 +
15.140 + gui_plot(readings % SCREEN_WIDTH, SCREEN_HEIGHT / 2 * (1 - acceleration.x), 255, 0, 0, 255);
15.141 + gui_plot(readings % SCREEN_WIDTH, SCREEN_HEIGHT / 2 * (1 - acceleration.y), 0, 255, 0, 255);
15.142 + gui_plot(readings % SCREEN_WIDTH, SCREEN_HEIGHT / 2 * (1 - acceleration.z), 0, 0, 255, 255);
15.143 +
15.144 + gui_flush();
15.145 +
15.146 + printf("A: % 6.4f, % 6.4f, % 6.4f ",
15.147 + acceleration.x, acceleration.y, acceleration.z);
15.148 +
15.149 + printf("DET: % 6.4f, % 6.4f, % 6.4f\n", direction, elevation, tilt);
15.150 +
15.151 + pthread_mutex_unlock(&mutex);
15.152 + }
15.153 +
15.154 + switch (gui_handle_events())
15.155 + {
15.156 + case IMU_UI_OP_QUIT:
15.157 + pthread_cancel(thread);
15.158 + pthread_join(thread, &threadresult);
15.159 + gui_quit();
15.160 + return 0;
15.161 +
15.162 + case IMU_UI_OP_PAUSE:
15.163 + paused = !paused;
15.164 + break;
15.165 +
15.166 + default:
15.167 + break;
15.168 + }
15.169 +
15.170 + usleep(IMU_UPDATE_PERIOD);
15.171 + }
15.172 +
15.173 + /* This should be unreachable. */
15.174 +
15.175 + pthread_cancel(thread);
15.176 + pthread_join(thread, &threadresult);
15.177 + gui_quit();
15.178 + return 0;
15.179 +}
16.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
16.2 +++ b/main.c Mon Oct 14 13:02:19 2013 +0000
16.3 @@ -0,0 +1,274 @@
16.4 +/*
16.5 + * Ben NanoNote communication with the Pololu MinIMU-9 with the L3G4200D 3-axis
16.6 + * gyroscope and LSM303DLM accelerometer/magnetometer.
16.7 + *
16.8 + * http://www.pololu.com/catalog/product/1265
16.9 + *
16.10 + * Copyright (C) 2013 Paul Boddie
16.11 + *
16.12 + * This program is free software; you can redistribute it and/or modify
16.13 + * it under the terms of the GNU General Public License as published by
16.14 + * the Free Software Foundation; either version 2 of the License, or
16.15 + * (at your option) any later version.
16.16 + */
16.17 +
16.18 +#include <stdio.h>
16.19 +#include <signal.h>
16.20 +#include <string.h>
16.21 +#include <sys/time.h>
16.22 +#include <unistd.h>
16.23 +#include <pthread.h>
16.24 +#include "imu.h"
16.25 +#include "shutdown.h"
16.26 +#include "gui.h"
16.27 +#include "ui.h"
16.28 +#include "measure.h"
16.29 +
16.30 +/* Main program. */
16.31 +
16.32 +int main(int argc, char *argv[])
16.33 +{
16.34 + int argno = 1;
16.35 + void *threadresult;
16.36 + uint8_t result[6];
16.37 + bool using_filter = false;
16.38 +
16.39 + /* Local view state. */
16.40 +
16.41 + vectorf _viewx, _viewy, _viewz,
16.42 + _accelerationD, _accelerationRD,
16.43 + _fieldD;
16.44 +
16.45 + /* Timekeeping. */
16.46 +
16.47 + struct timeval now, text_updated, gui_updated;
16.48 +
16.49 + /* Graphical mode variables. */
16.50 +
16.51 + bool graphical = false;
16.52 + int (*print)(const char *, ...) = printf;
16.53 + void (*flush)() = text_flush;
16.54 + void (*clear)() = text_clear;
16.55 + void (*quit)() = text_quit;
16.56 + void (*shutdown)(int) = text_shutdown;
16.57 + void (*shutdown_threaded)(int) = text_shutdown_threaded;
16.58 + imu_ui_op (*handle_events)() = text_handle_events;
16.59 +
16.60 + memset(accelerationB, 0, sizeof(accelerationB));
16.61 +
16.62 + /* Enable a high-pass filter if requested. */
16.63 +
16.64 + if ((argc > argno) && (strcmp(argv[argno], "-f") == 0))
16.65 + {
16.66 + argno++;
16.67 + using_filter = true;
16.68 + }
16.69 +
16.70 + /* Initialise graphical or textual mode. */
16.71 +
16.72 + graphical = (argc > argno) && (strcmp(argv[argno], "-g") == 0);
16.73 +
16.74 + if (graphical)
16.75 + {
16.76 + argno++;
16.77 +
16.78 + print = gui_printf;
16.79 + flush = gui_flush;
16.80 + clear = gui_clear;
16.81 + quit = gui_quit;
16.82 + shutdown = gui_shutdown;
16.83 + shutdown_threaded = gui_shutdown_threaded;
16.84 + handle_events = gui_handle_events;
16.85 +
16.86 + gui_init();
16.87 + gui_display_init();
16.88 + }
16.89 +
16.90 + signal(SIGINT, init_shutdown);
16.91 +
16.92 + /* Access the 8:10 port. */
16.93 +
16.94 + if (ubb_open(0) < 0) {
16.95 + perror("ubb_open");
16.96 + return 1;
16.97 + }
16.98 +
16.99 + ubb_power(1);
16.100 + printf("Power on.\n");
16.101 +
16.102 + /* Bring the IMU up. */
16.103 +
16.104 + imu_init();
16.105 +
16.106 + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG1, IMU_GYRO_CTRL_REG1_ALL);
16.107 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG1_A, IMU_ACCEL_CTRL_REG1_ALL | IMU_ACCEL_FREQ);
16.108 +
16.109 + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG5, 0);
16.110 +
16.111 + if (using_filter)
16.112 + {
16.113 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, IMU_ACCEL_CTRL_REG2_FDS | IMU_ACCEL_FILTER_FREQ);
16.114 + imu_recv(IMU_ACCEL_ADDRESS, IMU_ACCEL_HP_FILTER_RESET_A, result, 1);
16.115 + }
16.116 + else
16.117 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, 0);
16.118 +
16.119 + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG4, IMU_GYRO_CTRL_REG4_BDU | IMU_GYRO_DPS_SCALE);
16.120 + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG4_A, IMU_ACCEL_CTRL_REG4_BDU | IMU_ACCEL_SCALE);
16.121 +
16.122 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRA_REG_M, IMU_MAGNET_FREQ);
16.123 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRB_REG_M, IMU_MAGNET_SCALE);
16.124 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_SINGLE);
16.125 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_CONT);
16.126 +
16.127 + usleep(IMU_MAGNET_UPDATE_PERIOD);
16.128 +
16.129 + if (imu_recv(IMU_GYRO_ADDRESS, IMU_GYRO_WHO_AM_I, result, 1))
16.130 + printf("Who am I? %x\n", result[0]);
16.131 +
16.132 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_WHO_AM_I_M, result, 1))
16.133 + printf("Who am I? %x\n", result[0]);
16.134 +
16.135 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRA_REG_M, result, 1))
16.136 + printf("Identification A? %x\n", result[0]);
16.137 +
16.138 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRB_REG_M, result, 1))
16.139 + printf("Identification B? %x\n", result[0]);
16.140 +
16.141 + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRC_REG_M, result, 1))
16.142 + printf("Identification C? %x\n", result[0]);
16.143 +
16.144 + /* Get average values for the gyroscope and accelerometer. */
16.145 +
16.146 + if ((argc > argno) && (strcmp(argv[argno], "-c") == 0))
16.147 + {
16.148 + argno++;
16.149 + ui_calibrate(using_filter, print, flush);
16.150 + }
16.151 +
16.152 + /* Reset the acceleration buffer. */
16.153 +
16.154 + memset(accelerationB, 0, sizeof(accelerationB));
16.155 +
16.156 + /* Create a measurement thread. */
16.157 +
16.158 + if (pthread_create(&thread, NULL, get_measurements, &using_filter) != 0)
16.159 + {
16.160 + perror("pthread_create");
16.161 + shutdown(0);
16.162 + }
16.163 +
16.164 + signal(SIGINT, shutdown_threaded);
16.165 +
16.166 + pthread_mutex_init(&mutex, NULL);
16.167 +
16.168 + gettimeofday(&now, NULL);
16.169 + text_updated = now; gui_updated = now;
16.170 +
16.171 + /* Refresh the display by obtaining measurements made in the measurement
16.172 + thread. */
16.173 +
16.174 + while (1)
16.175 + {
16.176 + gettimeofday(&now, NULL);
16.177 +
16.178 + if (get_period(now, text_updated) >= TEXT_UPDATE_PERIOD)
16.179 + {
16.180 + pthread_mutex_lock(&mutex);
16.181 +
16.182 + /* Show textual details. */
16.183 +
16.184 + if (!graphical)
16.185 + {
16.186 + clear();
16.187 + print("Rotation? %.4f, %.4f, %.4f\n", rotation.x, rotation.y, rotation.z);
16.188 + print("Vector? %.4f, %.4f, %.4f\n", viewx.x, viewx.y, viewx.z);
16.189 + print("Vector? %.4f, %.4f, %.4f\n", viewy.x, viewy.y, viewy.z);
16.190 + print("Vector? %.4f, %.4f, %.4f\n", viewz.x, viewz.y, viewz.z);
16.191 + print("Acceleration? %.4f, %.4f, %.4f\n", accelerationD.x, accelerationD.y, accelerationD.z);
16.192 + print("Field vector? %.4f, %.4f, %.4f\n", fieldD.x, fieldD.y, fieldD.z);
16.193 + flush();
16.194 + }
16.195 +
16.196 + printf("Period? %d\n", imu_period);
16.197 + printf("Direction? %.4f\n", raddeg(direction));
16.198 + printf("Elevation? %.4f (%.4f)\n", raddeg(elevation), raddeg(elevationA));
16.199 + printf("Tilt? %.4f (%.4f)\n", raddeg(tilt), raddeg(tiltA));
16.200 + printf("Acceleration? %.4f, %.4f, %.4f\n", accelerationD.x, accelerationD.y, accelerationD.z);
16.201 +
16.202 + pthread_mutex_unlock(&mutex);
16.203 +
16.204 + text_updated = now;
16.205 + }
16.206 +
16.207 + if (graphical && (get_period(now, gui_updated) >= GUI_UPDATE_PERIOD))
16.208 + {
16.209 + pthread_mutex_lock(&mutex);
16.210 + _viewx = viewx;
16.211 + _viewy = viewy;
16.212 + _viewz = viewz;
16.213 + _accelerationD = accelerationD;
16.214 + _accelerationRD = accelerationRD;
16.215 + _fieldD = fieldD;
16.216 + pthread_mutex_unlock(&mutex);
16.217 +
16.218 + clear();
16.219 + gui_sky(&_viewx, &_viewy, &_viewz);
16.220 +
16.221 + _accelerationRD.x *= 10;
16.222 + _accelerationRD.y *= 10;
16.223 + _accelerationRD.z *= 10;
16.224 + gui_point(&_viewx, &_viewy, &_viewz, &_accelerationRD, 0, 255, 0, 127);
16.225 + gui_bar(&_accelerationD);
16.226 +
16.227 + _fieldD.x *= 10;
16.228 + _fieldD.y *= 10;
16.229 + _fieldD.z *= 10;
16.230 + gui_point(&_viewx, &_viewy, &_viewz, &_fieldD, 0, 0, 255, 127);
16.231 +
16.232 + flush();
16.233 +
16.234 + gui_updated = now;
16.235 + }
16.236 +
16.237 + /* Respond to instructions from the UI. */
16.238 +
16.239 + switch (handle_events())
16.240 + {
16.241 + case IMU_UI_OP_QUIT:
16.242 + pthread_cancel(thread);
16.243 + pthread_join(thread, &threadresult);
16.244 + quit();
16.245 + return 0;
16.246 +
16.247 + case IMU_UI_OP_RESET:
16.248 + pthread_mutex_lock(&mutex);
16.249 + devicex = devicex0;
16.250 + devicey = devicey0;
16.251 + devicez = devicez0;
16.252 + vectorf_reset(&accelerationD);
16.253 + pthread_mutex_unlock(&mutex);
16.254 + break;
16.255 +
16.256 + case IMU_UI_OP_CALIBRATE:
16.257 + pthread_mutex_lock(&mutex);
16.258 + clear();
16.259 + ui_calibrate(using_filter, print, flush);
16.260 + flush();
16.261 + memset(accelerationB, 0, sizeof(accelerationB));
16.262 + gettimeofday(&imu_updated, NULL);
16.263 + pthread_mutex_unlock(&mutex);
16.264 + break;
16.265 +
16.266 + default:
16.267 + break;
16.268 + }
16.269 + }
16.270 +
16.271 + /* This should be unreachable. */
16.272 +
16.273 + pthread_cancel(thread);
16.274 + pthread_join(thread, &threadresult);
16.275 + quit();
16.276 + return 0;
16.277 +}
17.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
17.2 +++ b/measure.c Mon Oct 14 13:02:19 2013 +0000
17.3 @@ -0,0 +1,255 @@
17.4 +/*
17.5 + * Ben NanoNote communication with the Pololu MinIMU-9 with the L3G4200D 3-axis
17.6 + * gyroscope and LSM303DLM accelerometer/magnetometer.
17.7 + *
17.8 + * http://www.pololu.com/catalog/product/1265
17.9 + *
17.10 + * Copyright (C) 2013 Paul Boddie
17.11 + *
17.12 + * This program is free software; you can redistribute it and/or modify
17.13 + * it under the terms of the GNU General Public License as published by
17.14 + * the Free Software Foundation; either version 2 of the License, or
17.15 + * (at your option) any later version.
17.16 + */
17.17 +
17.18 +#include <sys/time.h>
17.19 +#include <unistd.h>
17.20 +#include <pthread.h>
17.21 +#include "imu.h"
17.22 +#include "geo.h"
17.23 +
17.24 +#define __MEASURE_H_PRIVATE__
17.25 +#include "measure.h"
17.26 +#undef __MEASURE_H_PRIVATE__
17.27 +
17.28 +static bool setF0 = false;
17.29 +
17.30 +/**
17.31 + * Perform calibration with feedback given in the user interface.
17.32 + */
17.33 +void ui_calibrate(bool using_filter, int (*print)(const char *, ...), void (*flush)())
17.34 +{
17.35 + vectorf tmpB[1];
17.36 +
17.37 + print("Calibrating...\n");
17.38 + flush();
17.39 +
17.40 + /* Calibrate without a filter for rotation. */
17.41 +
17.42 + calibrate(&rotation0, tmpB, 1, IMU_GYRO_ADDRESS, IMU_GYRO_OUT_X_L | IMU_GYRO_READ_MANY, IMU_UPDATE_PERIOD, convert);
17.43 +
17.44 + print("Calibrated using (%.4f, %.4f, %.4f).\n", rotation0.x, rotation0.y, rotation0.z);
17.45 + flush();
17.46 +
17.47 + /* Calibrate using a filter for acceleration. */
17.48 +
17.49 + calibrate(&acceleration0, accelerationB, IMU_ACCEL_BUFFER_SIZE,
17.50 + IMU_ACCEL_ADDRESS, IMU_ACCEL_OUT_X_L_A | IMU_ACCEL_READ_MANY, IMU_UPDATE_PERIOD, convert12);
17.51 +
17.52 + /* Filter out the expected 1g measurement on the z axis. */
17.53 +
17.54 + if (!using_filter)
17.55 + acceleration0.z += acceleration1g;
17.56 +
17.57 + print("Calibrated using (%.4f, %.4f, %.4f).\n", acceleration0.x, acceleration0.y, acceleration0.z);
17.58 + flush();
17.59 +}
17.60 +
17.61 +void *get_measurements(void *arg)
17.62 +{
17.63 + struct timeval now;
17.64 + uint32_t period;
17.65 + bool using_filter = false;
17.66 + double accelerationM;
17.67 + bool set_reference = false;
17.68 +
17.69 + pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
17.70 +
17.71 + if (arg != NULL)
17.72 + using_filter = *((bool *) arg);
17.73 +
17.74 + /* Initialise the default device orientation. */
17.75 +
17.76 + devicex = devicex0;
17.77 + devicey = devicey0;
17.78 + devicez = devicez0;
17.79 +
17.80 + /* Note the time to schedule the next update. */
17.81 +
17.82 + gettimeofday(&imu_updated, NULL);
17.83 + imu_magnet_updated = imu_updated;
17.84 +
17.85 + /* NOTE: Wake up the stupid magnetometer. */
17.86 +
17.87 + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_CONT);
17.88 +
17.89 + /* Actual readings. */
17.90 +
17.91 + while (1)
17.92 + {
17.93 + gettimeofday(&now, NULL);
17.94 +
17.95 + period = get_period(now, imu_magnet_updated);
17.96 +
17.97 + if (period >= IMU_MAGNET_UPDATE_PERIOD)
17.98 + {
17.99 + imu_magnet_updated = now;
17.100 +
17.101 + pthread_mutex_lock(&mutex);
17.102 +
17.103 + if (imu_read_vector_xzy(IMU_MAGNET_ADDRESS, IMU_MAGNET_OUT_X_H_M,
17.104 + &field, convertBE12L))
17.105 + {
17.106 + /* NOTE: Handle stupid magnetometer readings. */
17.107 +
17.108 + if (!setF0 && (field.x == 0))
17.109 + {
17.110 + field.y = 0; field.z = 0;
17.111 + }
17.112 + else
17.113 + {
17.114 + normalise(&field, &fieldmin, &fieldmax, &field);
17.115 + field.x = to_field(field.x * IMU_UGAUSS_FACTOR);
17.116 + field.y = to_field(field.y * IMU_UGAUSS_FACTOR);
17.117 + field.z = to_field(field.z * IMU_UGAUSS_FACTOR * IMU_MAGNET_Z_XY_RATIO);
17.118 + vectorf_normalise(&field, &field);
17.119 + }
17.120 + }
17.121 +
17.122 + pthread_mutex_unlock(&mutex);
17.123 + }
17.124 +
17.125 + period = get_period(now, imu_updated);
17.126 +
17.127 + if (period >= IMU_UPDATE_PERIOD)
17.128 + {
17.129 + imu_updated = now;
17.130 +
17.131 + pthread_mutex_lock(&mutex);
17.132 +
17.133 + imu_period = period;
17.134 +
17.135 + if (imu_read_vector(IMU_GYRO_ADDRESS, IMU_GYRO_OUT_X_L | IMU_GYRO_READ_MANY,
17.136 + &rotation, convert))
17.137 + {
17.138 + rotation.x -= rotation0.x;
17.139 + rotation.y -= rotation0.y;
17.140 + rotation.z -= rotation0.z;
17.141 +
17.142 + rotation.x = to_angle(rotation.x * IMU_UDPS_FACTOR * period);
17.143 + rotation.y = to_angle(rotation.y * IMU_UDPS_FACTOR * period);
17.144 + rotation.z = to_angle(rotation.z * IMU_UDPS_FACTOR * period);
17.145 +
17.146 + plane_rotate(&devicey, &devicez, degrad(rotation.x));
17.147 + plane_rotate(&devicez, &devicex, degrad(rotation.y));
17.148 + plane_rotate(&devicex, &devicey, degrad(rotation.z));
17.149 + }
17.150 +
17.151 + if (imu_read_vector(IMU_ACCEL_ADDRESS, IMU_ACCEL_OUT_X_L_A | IMU_ACCEL_READ_MANY,
17.152 + &acceleration, convert12))
17.153 + {
17.154 + acceleration.x -= acceleration0.x;
17.155 + acceleration.y -= acceleration0.y;
17.156 + acceleration.z -= acceleration0.z;
17.157 +
17.158 + /* Convert to g. */
17.159 +
17.160 + acceleration.x /= acceleration1g;
17.161 + acceleration.y /= acceleration1g;
17.162 + acceleration.z /= acceleration1g;
17.163 +
17.164 + /* Detect gravitational acceleration. */
17.165 +
17.166 + accelerationM = vectorf_mag(&acceleration);
17.167 + set_reference = (accelerationM > ACCEL_REST_MAG_LOWER) && (accelerationM < ACCEL_REST_MAG_UPPER);
17.168 +
17.169 + /* Obtain the acceleration in the global space. */
17.170 +
17.171 + vectorf_convert(&acceleration, &devicex, &devicey, &devicez, &accelerationD);
17.172 + }
17.173 +
17.174 + /* Obtain the view axes and device orientation. */
17.175 +
17.176 + vectorf_negate(&devicey, &viewx);
17.177 + vectorf_negate(&devicez, &viewy);
17.178 + vectorf_negate(&devicex, &viewz);
17.179 +
17.180 + direction = vectorf_direction(&viewz);
17.181 + elevation = vectorf_elevation(&viewz);
17.182 + tilt = within(-(vectorf_tilt_in_plane(&viewy0, &viewx, &viewy) - M_PI / 2), M_PI);
17.183 +
17.184 + /* Reset or update the reference acceleration. */
17.185 +
17.186 + if (set_reference)
17.187 + {
17.188 + accelerationRD = accelerationD;
17.189 + }
17.190 + else
17.191 + {
17.192 + vectorf_rotate_in_space(&accelerationRD, &viewz, &viewy, &viewx, degrad(rotation.y), &accelerationRD);
17.193 + vectorf_rotate_in_space(&accelerationRD, &viewx, &viewy, &viewz, degrad(-rotation.x), &accelerationRD);
17.194 + }
17.195 +
17.196 + /* Define the tilt and elevation of the reference acceleration. */
17.197 +
17.198 + elevationA = vectorf_tilt_in_plane(&accelerationRD, &viewy0, &viewz);
17.199 + tiltA = vectorf_tilt_in_plane_with_axis(&accelerationRD, &viewy0, &viewx, &viewz);
17.200 +
17.201 + /* Adjust according to elevation. */
17.202 +
17.203 + if (set_reference && (fabs(elevation) < degrad(60)))
17.204 + {
17.205 + plane_rotate(&devicey, &devicez, -tiltA * ROTATION_ADJUSTMENT_FACTOR);
17.206 + plane_rotate(&devicez, &devicex, -elevationA * ROTATION_ADJUSTMENT_FACTOR);
17.207 +
17.208 + vectorf_negate(&devicey, &viewx);
17.209 + vectorf_negate(&devicez, &viewy);
17.210 + vectorf_negate(&devicex, &viewz);
17.211 + }
17.212 +
17.213 + /* Obtain the magnetic field in the global space. */
17.214 +
17.215 + if (!vectorf_null(&field))
17.216 + {
17.217 + directionF = vectorf_direction(&field);
17.218 + elevationF = vectorf_elevation(&field);
17.219 +
17.220 + /* Define the global vector, remembering the initial value. */
17.221 +
17.222 + vectorf_convert(&field, &devicex, &devicey, &devicez, &fieldD);
17.223 +
17.224 + if (!setF0)
17.225 + {
17.226 + fieldD0 = fieldD;
17.227 + setF0 = true;
17.228 + }
17.229 + }
17.230 +
17.231 + /* Determine the initial field vector in the current device space. */
17.232 +
17.233 + if (setF0)
17.234 + {
17.235 + vectorf_convert_into(&fieldD0, &devicex, &devicey, &devicez, &field0);
17.236 + directionF0 = vectorf_direction(&field0);
17.237 + elevationF0 = vectorf_elevation(&field0);
17.238 +
17.239 + /* Determine the east and north vectors using static field information. */
17.240 +
17.241 + vectorf_cross(&accelerationRD, &fieldD0, &fieldE);
17.242 + vectorf_normalise(&fieldE, &fieldE);
17.243 + vectorf_cross(&fieldE, &accelerationRD, &fieldN);
17.244 + }
17.245 +
17.246 + /* Subtract the constant background acceleration. */
17.247 +
17.248 + if (!using_filter)
17.249 + accelerationD.y -= 1;
17.250 +
17.251 + pthread_mutex_unlock(&mutex);
17.252 + }
17.253 +
17.254 + usleep(IMU_UPDATE_PERIOD);
17.255 + }
17.256 +
17.257 + return NULL;
17.258 +}
18.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
18.2 +++ b/measure.h Mon Oct 14 13:02:19 2013 +0000
18.3 @@ -0,0 +1,108 @@
18.4 +/*
18.5 + * Ben NanoNote communication with the Pololu MinIMU-9 with the L3G4200D 3-axis
18.6 + * gyroscope and LSM303DLM accelerometer/magnetometer.
18.7 + *
18.8 + * http://www.pololu.com/catalog/product/1265
18.9 + *
18.10 + * Copyright (C) 2013 Paul Boddie
18.11 + *
18.12 + * This program is free software; you can redistribute it and/or modify
18.13 + * it under the terms of the GNU General Public License as published by
18.14 + * the Free Software Foundation; either version 2 of the License, or
18.15 + * (at your option) any later version.
18.16 + */
18.17 +
18.18 +#ifndef __MEASURE_H__
18.19 +#define __MEASURE_H__
18.20 +
18.21 +#include "geo.h"
18.22 +
18.23 +/* Function definitions. */
18.24 +
18.25 +void ui_calibrate(bool using_filter, int (*print)(const char *, ...), void (*flush)());
18.26 +void *get_measurements(void *arg);
18.27 +
18.28 +#ifdef __MEASURE_H_PRIVATE__
18.29 +
18.30 +/* The raw accelerometer value of 1g. */
18.31 +
18.32 +const double acceleration1g = 1000000.0 / IMU_UG_FACTOR;
18.33 +
18.34 +/* The device vectors defined in the chosen coordinate system, together with
18.35 + the corresponding initial view axes. */
18.36 +
18.37 +const vectorf devicex0 = {{0, 0, -1}}, viewz0 = {{0, 0, 1}},
18.38 + devicey0 = {{-1, 0, 0}}, viewx0 = {{1, 0, 0}},
18.39 + devicez0 = {{0, -1, 0}}, viewy0 = {{0, 1, 0}};
18.40 +
18.41 +/* Measurements. */
18.42 +
18.43 +vectorf accelerationB[IMU_ACCEL_BUFFER_SIZE],
18.44 + acceleration,
18.45 + acceleration0 = {{0, 0, 0}},
18.46 + rotation,
18.47 + rotation0 = {{0, 0, 0}},
18.48 + field,
18.49 + fieldmin = {{-327.0, -355.0, -338.0}},
18.50 + fieldmax = {{107.0, 150.0, 205.0}};
18.51 +
18.52 +/* Timekeeping. */
18.53 +
18.54 +struct timeval imu_updated, imu_magnet_updated;
18.55 +uint32_t imu_period = 0;
18.56 +
18.57 +/* Orientation. */
18.58 +
18.59 +vectorf devicex, devicey, devicez,
18.60 + viewx, viewy, viewz,
18.61 + accelerationD,
18.62 + accelerationR = {{0, 0, -1}},
18.63 + accelerationRD = {{0, 1, 0}},
18.64 + fieldD = {{0, 0, 0}},
18.65 + fieldD0 = {{0, 0, 0}},
18.66 + field0 = {{0, 0, 0}},
18.67 + fieldN = {{0, 0, 0}},
18.68 + fieldE = {{0, 0, 0}};
18.69 +double direction, elevation, tilt,
18.70 + elevationA = 0, tiltA = 0,
18.71 + directionF, elevationF,
18.72 + directionF0, elevationF0;
18.73 +
18.74 +/* Measurement thread. */
18.75 +
18.76 +pthread_t thread;
18.77 +pthread_mutex_t mutex;
18.78 +
18.79 +#else
18.80 +
18.81 +extern const double acceleration1g;
18.82 +extern vectorf devicex0, devicey0, devicez0,
18.83 + devicex, devicey, devicez,
18.84 + viewx0, viewy0, viewz0,
18.85 + viewx, viewy, viewz,
18.86 + accelerationB[IMU_ACCEL_BUFFER_SIZE],
18.87 + acceleration,
18.88 + acceleration0,
18.89 + accelerationD,
18.90 + accelerationR,
18.91 + accelerationRD,
18.92 + rotation,
18.93 + rotation0,
18.94 + field,
18.95 + fieldmin, fieldmax,
18.96 + fieldD, fieldD0, field0, fieldN, fieldE;
18.97 +extern struct timeval imu_updated;
18.98 +extern uint32_t imu_period;
18.99 +extern double direction, elevation, tilt,
18.100 + elevationA, tiltA,
18.101 + directionF, elevationF,
18.102 + directionF0, elevationF0;
18.103 +
18.104 +/* Measurement thread. */
18.105 +
18.106 +extern pthread_t thread;
18.107 +extern pthread_mutex_t mutex;
18.108 +
18.109 +#endif /* __MEASURE_H_PRIVATE__ */
18.110 +
18.111 +#endif /* __MEASURE_H__ */
19.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
19.2 +++ b/shutdown.c Mon Oct 14 13:02:19 2013 +0000
19.3 @@ -0,0 +1,24 @@
19.4 +/*
19.5 + * Common shutdown functionality.
19.6 + *
19.7 + * Copyright (C) 2013 Paul Boddie
19.8 + *
19.9 + * This program is free software; you can redistribute it and/or modify
19.10 + * it under the terms of the GNU General Public License as published by
19.11 + * the Free Software Foundation; either version 2 of the License, or
19.12 + * (at your option) any later version.
19.13 + */
19.14 +
19.15 +#include <stdio.h>
19.16 +#include <stdlib.h>
19.17 +#include <ubb/ubb.h>
19.18 +
19.19 +/**
19.20 + * Handle termination of the process.
19.21 + */
19.22 +void init_shutdown(int signum)
19.23 +{
19.24 + printf("Closing...\n");
19.25 + ubb_close(0);
19.26 + exit(1);
19.27 +}
20.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
20.2 +++ b/shutdown.h Mon Oct 14 13:02:19 2013 +0000
20.3 @@ -0,0 +1,17 @@
20.4 +/*
20.5 + * Common shutdown functionality.
20.6 + *
20.7 + * Copyright (C) 2013 Paul Boddie
20.8 + *
20.9 + * This program is free software; you can redistribute it and/or modify
20.10 + * it under the terms of the GNU General Public License as published by
20.11 + * the Free Software Foundation; either version 2 of the License, or
20.12 + * (at your option) any later version.
20.13 + */
20.14 +
20.15 +#ifndef __SHUTDOWN_H__
20.16 +#define __SHUTDOWN_H__
20.17 +
20.18 +void init_shutdown(int signum);
20.19 +
20.20 +#endif /* __SHUTDOWN_H__ */
21.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
21.2 +++ b/ui.c Mon Oct 14 13:02:19 2013 +0000
21.3 @@ -0,0 +1,55 @@
21.4 +/*
21.5 + * Common user interface functionality.
21.6 + *
21.7 + * Copyright (C) 2013 Paul Boddie
21.8 + *
21.9 + * This program is free software; you can redistribute it and/or modify
21.10 + * it under the terms of the GNU General Public License as published by
21.11 + * the Free Software Foundation; either version 2 of the License, or
21.12 + * (at your option) any later version.
21.13 + */
21.14 +
21.15 +#include <stdio.h>
21.16 +#include <stdlib.h>
21.17 +#include <ubb/ubb.h>
21.18 +#include <pthread.h>
21.19 +#include "ui.h"
21.20 +#include "shutdown.h"
21.21 +
21.22 +void text_shutdown(int signum)
21.23 +{
21.24 + init_shutdown(signum);
21.25 +}
21.26 +
21.27 +void text_shutdown_threaded(int signum)
21.28 +{
21.29 + extern pthread_t thread;
21.30 + void *threadresult;
21.31 +
21.32 + if (pthread_cancel(thread) == 0)
21.33 + pthread_join(thread, &threadresult);
21.34 +
21.35 + text_shutdown(signum);
21.36 +}
21.37 +
21.38 +/* Textual interface functions. */
21.39 +
21.40 +imu_ui_op text_handle_events()
21.41 +{
21.42 + return IMU_UI_OP_NULL;
21.43 +}
21.44 +
21.45 +void text_flush()
21.46 +{
21.47 + fflush(stdout);
21.48 +}
21.49 +
21.50 +void text_clear()
21.51 +{
21.52 +}
21.53 +
21.54 +void text_quit()
21.55 +{
21.56 + printf("Closing...\n");
21.57 + ubb_close(0);
21.58 +}
22.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
22.2 +++ b/ui.h Mon Oct 14 13:02:19 2013 +0000
22.3 @@ -0,0 +1,34 @@
22.4 +/*
22.5 + * Common user interface functionality.
22.6 + *
22.7 + * Copyright (C) 2013 Paul Boddie
22.8 + *
22.9 + * This program is free software; you can redistribute it and/or modify
22.10 + * it under the terms of the GNU General Public License as published by
22.11 + * the Free Software Foundation; either version 2 of the License, or
22.12 + * (at your option) any later version.
22.13 + */
22.14 +
22.15 +#ifndef __UI_H__
22.16 +#define __UI_H__
22.17 +
22.18 +/* Common types. */
22.19 +
22.20 +typedef enum
22.21 +{
22.22 + IMU_UI_OP_NULL, IMU_UI_OP_CALIBRATE, IMU_UI_OP_RESET, IMU_UI_OP_PAUSE,
22.23 + IMU_UI_OP_QUIT
22.24 +} imu_ui_op;
22.25 +
22.26 +void init_shutdown(int signum);
22.27 +void text_shutdown(int signum);
22.28 +void text_shutdown_threaded(int signum);
22.29 +
22.30 +/* Textual interface functions. */
22.31 +
22.32 +imu_ui_op text_handle_events();
22.33 +void text_flush();
22.34 +void text_clear();
22.35 +void text_quit();
22.36 +
22.37 +#endif /* __UI_H__ */