# HG changeset patch # User Paul Boddie # Date 1381755739 0 # Node ID 631f88781de8960ffd72a1984ca6a4accc47fe81 Ben NanoNote and Pololu MinIMU-9 interfacing. diff -r 000000000000 -r 631f88781de8 Makefile --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Makefile Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,60 @@ +# Makefile - Build the Pololu IMU software +# +# Copyright (C) 2013 Paul Boddie +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. + +LIBUBB = ../ben-blinkenlights/libubb +SYSROOT = ../openwrt-xburst/staging_dir/target-mipsel_eglibc-2.15 +TOOLBIN = ../openwrt-xburst/staging_dir/toolchain-mipsel_gcc-4.6-linaro_eglibc-2.15/bin + +ARCH = mipsel-openwrt-linux +CC = $(TOOLBIN)/$(ARCH)-gcc + +CFLAGS = -g -Wall -fPIC -march=mips32 -I$(LIBUBB)/include $(shell $(SYSROOT)/usr/bin/sdl-config --cflags) # -DDEBUG=1 +LDFLAGS = -lm -lubb -L$(LIBUBB) $(shell $(SYSROOT)/usr/bin/sdl-config --libs) -Wl,-rpath-link,$(SYSROOT)/usr/lib -lSDL_gfx + +IMU = imu +TEST = itest +CALIBRATE = calibrate +TARGETS = $(IMU) $(TEST) $(CALIBRATE) + +BASICSRC = imu.c i2c.c shutdown.c geo.c +UISRC = measure.c ui.c gui.c + +IMUSRC = $(BASICSRC) $(UISRC) main.c +IMUOBJ = $(IMUSRC:.c=.o) + +TESTSRC = $(BASICSRC) $(UISRC) itest.c +TESTOBJ = $(TESTSRC:.c=.o) + +CALIBRATESRC = $(BASICSRC) calibrate.c +CALIBRATEOBJ = $(CALIBRATESRC:.c=.o) + +ALLSRC = $(BASICSRC) $(UISRC) main.c itest.c calibrate.c +ALLOBJ = $(ALLSRC:.c=.o) + +.PHONY: all clean distclean + +all: $(TARGETS) + +clean: + rm -f $(ALLOBJ) $(TARGETS) + +distclean: clean + echo "Nothing else to clean." + +$(IMU): $(IMUOBJ) + $(CC) $(LDFLAGS) $(IMUOBJ) -o $@ + +$(TEST): $(TESTOBJ) + $(CC) $(LDFLAGS) $(TESTOBJ) -o $@ + +$(CALIBRATE): $(CALIBRATEOBJ) + $(CC) $(LDFLAGS) $(CALIBRATEOBJ) -o $@ + +.c.o: + $(CC) -c $(CFLAGS) $< -o $@ diff -r 000000000000 -r 631f88781de8 bool.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/bool.h Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,12 @@ +/* + * A simple boolean definition found in Python's asdl.h. + */ + +#ifndef __BOOL_H__ +#define __BOOL_H__ + +#ifndef __cplusplus +typedef enum {false, true} bool; +#endif + +#endif /* __BOOL_H__ */ diff -r 000000000000 -r 631f88781de8 calibrate.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/calibrate.c Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,167 @@ +/* + * Calibrate Pololu MinIMU-9 measurements. + * + * Copyright (C) 2013 Paul Boddie + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ + +#include +#include +#include +#include +#include +#include "imu.h" +#include "shutdown.h" +#include "geo.h" + +/* Main program. */ + +int main(int argc, char *argv[]) +{ + uint8_t result[6]; + vectorf value, + vmin = {{1, 1, 1}}, + vmax = {{-1, -1, -1}}, + valueB[IMU_ACCEL_BUFFER_SIZE]; + int argno = 1, vindex = 0; + bool gyroscope = false, accelerometer = false, magnetometer = false, + averaging = false, normalised = false; + + if ((argc > argno) && (strcmp(argv[argno], "-g") == 0)) + { + argno++; + gyroscope = true; + } + else if ((argc > argno) && (strcmp(argv[argno], "-a") == 0)) + { + argno++; + accelerometer = true; + } + else if ((argc > argno) && (strcmp(argv[argno], "-m") == 0)) + { + argno++; + magnetometer = true; + } + else + { + printf("Need -g, -a or -m to select gyroscope, accelerometer or magnetometer respectively.\n"); + exit(1); + } + + if ((argc > argno) && (strcmp(argv[argno], "-v") == 0)) + { + argno++; + averaging = true; + } + + if ((argc > argno) && (strcmp(argv[argno], "-n") == 0)) + { + argno++; + normalised = true; + } + + signal(SIGINT, init_shutdown); + + /* Access the 8:10 port. */ + + if (ubb_open(0) < 0) { + perror("ubb_open"); + return 1; + } + + ubb_power(1); + printf("Power on.\n"); + + /* Bring the IMU up. */ + + imu_init(); + + if (gyroscope) + { + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG1, IMU_GYRO_CTRL_REG1_ALL); + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG5, 0); + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG4, IMU_GYRO_CTRL_REG4_BDU | IMU_GYRO_DPS_SCALE); + } + else if (accelerometer) + { + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG1_A, IMU_ACCEL_CTRL_REG1_ALL | IMU_ACCEL_CTRL_REG1_50HZ); + + if ((argc > argno) && (strcmp(argv[argno], "-f") == 0)) + { + argno++; + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, IMU_ACCEL_CTRL_REG2_FDS | IMU_ACCEL_FILTER_FREQ); + imu_recv(IMU_ACCEL_ADDRESS, IMU_ACCEL_HP_FILTER_RESET_A, result, 1); + } + else + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, 0); + + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG4_A, IMU_ACCEL_CTRL_REG4_BDU | IMU_ACCEL_SCALE); + } + else if (magnetometer) + { + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRA_REG_M, IMU_MAGNET_CRA_REG_30HZ); + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRB_REG_M, IMU_MAGNET_CRB_REG_4_0G); + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_SINGLE); + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_CONT); + } + + if (imu_recv(IMU_GYRO_ADDRESS, IMU_GYRO_WHO_AM_I, result, 1)) + printf("Who am I? %x\n", result[0]); + + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_WHO_AM_I_M, result, 1)) + printf("Who am I? %x\n", result[0]); + + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRA_REG_M, result, 1)) + printf("Identification A? %x\n", result[0]); + + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRB_REG_M, result, 1)) + printf("Identification B? %x\n", result[0]); + + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRC_REG_M, result, 1)) + printf("Identification C? %x\n", result[0]); + + memset(valueB, 0, sizeof(valueB)); + + while (1) + { + if (accelerometer) + imu_read_vector(IMU_ACCEL_ADDRESS, IMU_ACCEL_OUT_X_L_A | IMU_ACCEL_READ_MANY, &value, convert12); + else if (gyroscope) + imu_read_vector(IMU_GYRO_ADDRESS, IMU_GYRO_OUT_X_L | IMU_GYRO_READ_MANY, &value, convert); + else if (magnetometer) + imu_read_vector_xzy(IMU_MAGNET_ADDRESS, IMU_MAGNET_OUT_X_H_M, &value, convertBE12L); + + if (averaging) + { + queue(valueB, &vindex, IMU_ACCEL_BUFFER_SIZE, &value); + filter(valueB, vindex, IMU_ACCEL_BUFFER_SIZE, &value); + } + + vmin.x = min(value.x, vmin.x); + vmin.y = min(value.y, vmin.y); + vmin.z = min(value.z, vmin.z); + vmax.x = max(value.x, vmax.x); + vmax.y = max(value.y, vmax.y); + vmax.z = max(value.z, vmax.z); + + printf("V: % 6.1f, % 6.1f, % 6.1f " + "-: % 6.1f, % 6.1f, % 6.1f " + "+: % 6.1f, % 6.1f, % 6.1f\n", + normalised ? scale(value.x, vmin.x, (vmin.x + vmax.x) / 2, vmax.x) : value.x, + normalised ? scale(value.y, vmin.y, (vmin.y + vmax.y) / 2, vmax.y) : value.y, + normalised ? scale(value.z, vmin.z, (vmin.z + vmax.z) / 2, vmax.z) : value.z, + vmin.x, vmin.y, vmin.z, + vmax.x, vmax.y, vmax.z); + + usleep(magnetometer ? IMU_MAGNET_UPDATE_PERIOD : IMU_UPDATE_PERIOD); + } + + /* This should be unreachable. */ + + ubb_close(0); + return 0; +} diff -r 000000000000 -r 631f88781de8 docs/Ben_Pololu_IMU --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/docs/Ben_Pololu_IMU Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,23 @@ +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]. + +== Goals == + +* Experiment with I2C communication using the 8:10 port (see [[UBB]]) +* Evaluate the accuracy of digital gyroscope and accelerometer components +* Consider applications of orientation and positioning data +* Refresh mostly dormant vector mathematics knowledge + +== Progress == + +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. + +Communication with the board is done using the I2C protocol which requires only two additional wires from the 8:10 port. Although Linux kernel support could probably be employed to conduct I2C 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. + +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. + + +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) +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) + + +[[Category:Ben NanoNote]] diff -r 000000000000 -r 631f88781de8 docs/COPYING.txt --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/docs/COPYING.txt Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,17 @@ +Licence Agreement for ben-pololu-imu +------------------------------------ + +Copyright (C) 2013 Paul Boddie + +This program is free software; you can redistribute it and/or modify it under +the terms of the GNU General Public License as published by the Free Software +Foundation; either version 3 of the License, or (at your option) any later +version. + +This program is distributed in the hope that it will be useful, but WITHOUT +ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +FOR A PARTICULAR PURPOSE. See the GNU General Public License for more +details. + +You should have received a copy of the GNU General Public License along with +this program. If not, see . diff -r 000000000000 -r 631f88781de8 docs/gpl-3.0.txt --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/docs/gpl-3.0.txt Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,674 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff -r 000000000000 -r 631f88781de8 geo.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/geo.c Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,232 @@ +/* + * Geometric operations. + * + * Copyright (C) 2013 Paul Boddie + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ + +#include "geo.h" + +/** + * Return a value within the given magnitude. + */ +double within(double value, double mag) +{ + double offset = fmod(value, mag); + int num = (int) fdiv(value, mag); + + if (num % 2 == 0) + return offset; + else if (offset > 0) + return offset - mag; + else + return offset + mag; +} + +/** + * Return whether the given vector is null. + */ +bool vectorf_null(const vectorf *in) +{ + return ((in->x == 0) && (in->y == 0) && (in->z == 0)); +} + +/** + * Reset the given vector to have zero magnitude. + */ +void vectorf_reset(vectorf *in) +{ + in->x = 0; in->y = 0; in->z = 0; +} + +/** + * Return the cross product of the two input vectors in the output vector + * provided. + */ +void vectorf_cross(const vectorf *in1, const vectorf *in2, vectorf *out) +{ + out->x = in1->y * in2->z - in1->z * in2->y; + out->y = in1->z * in2->x - in1->x * in2->z; + out->z = in1->x * in2->y - in1->y * in2->x; +} + +/** + * Return the dot product of the two input vectors. + */ +double vectorf_dot(const vectorf *in1, const vectorf *in2) +{ + return in1->x * in2->x + in1->y * in2->y + in1->z * in2->z; +} + +/** + * Return the magnitude of the given vector. + */ +double vectorf_mag(const vectorf *in) +{ + return sqrt(pow(in->x, 2) + pow(in->y, 2) + pow(in->z, 2)); +} + +/** + * Normalise the given vector. + */ +void vectorf_normalise(vectorf *in, vectorf *out) +{ + double mag = vectorf_mag(in); + + out->x = in->x / mag; + out->y = in->y / mag; + out->z = in->z / mag; +} + +/** + * Rotate the plane defined by the given input vectors by the given angle, + * in radians, modifying the input vectors to define the resulting plane. + */ +void plane_rotate(vectorf *in1, vectorf *in2, double angle) +{ + double c = cos(angle), s = sin(angle); + vectorf out1, out2; + + /* Rotation around the plane normal with contributions from both plane + vectors. */ + + out1.x = c * in1->x + s * in2->x; + out1.y = c * in1->y + s * in2->y; + out1.z = c * in1->z + s * in2->z; + + /* The second plane vector is 90 degrees (pi/2) ahead of the first: + cos(angle + pi/2) == -sin(angle) + sin(angle + pi/2) == cos(angle) */ + + out2.x = c * in2->x - s * in1->x; + out2.y = c * in2->y - s * in1->y; + out2.z = c * in2->z - s * in1->z; + + *in1 = out1; + *in2 = out2; +} + +/** + * Populate the output vector with the negated input vector. + */ +void vectorf_negate(vectorf *in, vectorf *out) +{ + out->x = -in->x; + out->y = -in->y; + out->z = -in->z; +} + +/** + * Return the direction of the given vector in radians. + */ +double vectorf_direction(vectorf *in) +{ + return atan2(in->x, in->z); +} + +/** + * Return the elevation of the given unit vector in radians. + */ +double vectorf_elevation(vectorf *in) +{ + return asin(in->y); +} + +/** + * Obtain the direction vector for the given direction and elevation. + */ +void vectorf_polar(double direction, double elevation, vectorf *out) +{ + out->x = sin(direction) * cos(elevation); + out->y = sin(elevation); + out->z = cos(direction) * cos(elevation); +} + +/** + * Convert the input vector expressed in terms of the given axis vectors into + * the global coordinate space. + */ +void vectorf_convert(const vectorf *in, const vectorf *devicex, const vectorf *devicey, const vectorf *devicez, vectorf *out) +{ + out->x = in->x * devicex->x + in->y * devicey->x + in->z * devicez->x; + out->y = in->x * devicex->y + in->y * devicey->y + in->z * devicez->y; + out->z = in->x * devicex->z + in->y * devicey->z + in->z * devicez->z; +} + +/** + * Convert the input vector expressed in terms of the global coordinate space + * into a vector expressed in terms of the given axis vectors. + */ +void vectorf_convert_into(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z, vectorf *out) +{ + out->x = vectorf_dot(in, x); + out->y = vectorf_dot(in, y); + out->z = vectorf_dot(in, z); +} + +/** + * Return the tilt/roll of the given vector in radians projected into the given + * plane. + */ +double vectorf_tilt_in_plane(const vectorf *in, const vectorf *x, const vectorf *y) +{ + vectorf v, z, y2; + + vectorf_cross(x, y, &z); + vectorf_normalise(&z, &z); + vectorf_cross(&z, x, &y2); + vectorf_convert_into(in, x, &y2, &z, &v); + return atan2(v.y, v.x); +} + +/** + * Return the tilt/roll of the given vector in radians projected into the given + * space. + */ +double vectorf_tilt_in_plane_with_axis(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z) +{ + vectorf v, z2, y2; + + vectorf_cross(x, y, &z2); + if (vectorf_dot(z, &z2) < 0) + vectorf_negate(&z2, &z2); + vectorf_cross(&z2, x, &y2); + vectorf_convert_into(in, x, &y2, &z2, &v); + return atan2(v.y, v.x); +} + +/** + * Rotate a vector around the given z axis by the given angle in radians. + */ +void vectorf_rotate_in_space(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z, double angle, vectorf *out) +{ + vectorf v; + double tilt, c, s, vz, mag; + + /* Project the vector into the xy plane. */ + + vectorf_convert_into(in, x, y, z, &v); + + /* Remember the z value before obtaining a two dimensional vector. */ + + vz = v.z; + v.z = 0; + mag = vectorf_mag(&v); + + /* Get the current angle. */ + + tilt = atan2(v.y, v.x); + + /* Calculate the rotated vector. */ + + c = cos(tilt + angle); + s = sin(tilt + angle); + + out->x = mag * (c * x->x + s * y->x) + vz * z->x; + out->y = mag * (c * x->y + s * y->y) + vz * z->y; + out->z = mag * (c * x->z + s * y->z) + vz * z->z; +} diff -r 000000000000 -r 631f88781de8 geo.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/geo.h Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,64 @@ +/* + * Geometric operations. + * + * Copyright (C) 2013 Paul Boddie + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ + +#ifndef __GEO_H__ +#define __GEO_H__ + +#include +#include "bool.h" + +typedef union +{ + struct { double x, y, z; }; + double axis[3]; +} vectorf; + +double within(double value, double mag); +bool vectorf_null(const vectorf *in); +void vectorf_reset(vectorf *in); +void vectorf_cross(const vectorf *in1, const vectorf *in2, vectorf *out); +double vectorf_dot(const vectorf *in1, const vectorf *in2); +double vectorf_mag(const vectorf *in); +void vectorf_normalise(vectorf *in, vectorf *out); +void plane_rotate(vectorf *in1, vectorf *in2, double angle); +void vectorf_negate(vectorf *in, vectorf *out); +double vectorf_direction(vectorf *in); +double vectorf_elevation(vectorf *in); +void vectorf_polar(double direction, double elevation, vectorf *out); +void vectorf_convert(const vectorf *in, const vectorf *devicex, const vectorf *devicey, const vectorf *devicez, vectorf *out); +void vectorf_convert_into(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z, vectorf *out); +double vectorf_tilt_in_plane(const vectorf *in, const vectorf *x, const vectorf *y); +double vectorf_tilt_in_plane_with_axis(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z); +void vectorf_rotate_in_space(const vectorf *in, const vectorf *x, const vectorf *y, const vectorf *z, double angle, vectorf *out); + +#define degrad(x) ((x) * M_PI / 180.0) +#define raddeg(x) ((x) * 180.0 / M_PI) + +#define fdiv(x, y) (floor((int) ((x) / (y)))) + +/* Get the closest number at the given resolution. */ + +#define closest(x, res) (((int) (x) / (res)) * (res)) + +/* Get the maximum and minimum of the given numbers. */ + +#define max(x, y) ((x) >= (y) ? (x) : (y)) +#define min(x, y) ((x) <= (y) ? (x) : (y)) + +/* Get the sign of a number. */ + +#define sign(x) ((x) == 0 ? 0 : (x) < 0 ? -1 : 1) + +/* Eliminate noise using the given threshold. */ + +#define noise(x, y) (fabs(x) < (y) ? 0 : (x)) + +#endif /* __GEO_H__ */ diff -r 000000000000 -r 631f88781de8 gui.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/gui.c Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,366 @@ +/* + * Ben NanoNote graphical user interface utilities. + * + * Copyright (C) 2013 Paul Boddie + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ + +#include +#include "gui.h" + +SDL_Surface *screen; +gui_printer printer = {0, 0, 255, 255, 255, 255}; + +void gui_init() +{ + if (SDL_Init(SDL_INIT_VIDEO) < 0) + { + fprintf(stderr, "SDL error: %s\n", SDL_GetError()); + exit(1); + } +} + +void gui_display_init() +{ + screen = SDL_SetVideoMode(SCREEN_WIDTH, SCREEN_HEIGHT, 0, 0); + SDL_ShowCursor(SDL_DISABLE); +} + +void gui_shutdown(int signum) +{ + SDL_Quit(); + text_shutdown(signum); +} + +void gui_shutdown_threaded(int signum) +{ + SDL_Quit(); + text_shutdown_threaded(signum); +} + +void gui_quit() +{ + SDL_Quit(); + text_quit(); +} + +void gui_clear() +{ + gui_fill(0, 0, 0); + printer.x = 0; + printer.y = 0; +} + +void gui_fill(uint8_t r, uint8_t g, uint8_t b) +{ + SDL_FillRect(screen, NULL, SDL_MapRGB(screen->format, r, g, b)); +} + +void gui_next_row(gui_printer *printer, uint16_t rows) +{ + printer->x = 0; + printer->y = (printer->y + rows * SCREEN_ROW_HEIGHT) % SCREEN_HEIGHT; +} + +void gui_next_column(gui_printer *printer, uint16_t columns) +{ + printer->x += columns * SCREEN_COLUMN_WIDTH; + if (printer->x >= SCREEN_WIDTH) + { + printer->x = 0; + printer->y = (printer->y + SCREEN_ROW_HEIGHT) % SCREEN_HEIGHT; + } +} + +int gui_printf(const char *format, ...) +{ + va_list args; + static char buffer[GUI_BUFSIZE]; + char *this = buffer, *next; + int printed = 0; + + va_start(args, format); + vsnprintf(buffer, GUI_BUFSIZE, format, args); + va_end(args); + + do + { + next = strchr(this, (int) '\n'); + if (next != NULL) + *next = '\0'; + stringRGBA(screen, printer.x, printer.y, this, printer.r, printer.g, printer.b, printer.a); + if (next != NULL) + gui_next_row(&printer, 1); + else + gui_next_column(&printer, strlen(this)); + printed += strlen(this); + this = next; + } + while (next != NULL); + + return printed; +} + +/** + * Generate sky points. + */ +void gui_sky(vectorf *viewx, vectorf *viewy, vectorf *viewz) +{ + double direction, elevation; + int d, e, estart, efinish, dcount; + int16_t lastminx, lastminy, thisminx, thisminy, + lastmaxx, lastmaxy, thismaxx, thismaxy; + uint32_t colour; + vectorf point; + + direction = vectorf_direction(viewz); + elevation = vectorf_elevation(viewz); + + d = closest(raddeg(direction), SKY_GRID_STEP); + + /* Plot points between elevations -90 and 90 degrees. */ + + estart = max(-90, closest(raddeg(elevation), SKY_GRID_STEP) - (SKY_GRID_STEP * 3)); + efinish = min(90, closest(raddeg(elevation), SKY_GRID_STEP) + (SKY_GRID_STEP * 3)); + + for (e = estart; e <= efinish; e += SKY_GRID_STEP) + { + /* Start from the most central point and work outwards until + the points are no longer visible. */ + + for (dcount = 0; dcount <= 180; dcount += SKY_GRID_STEP) + { + /* Check point visibility. */ + + vectorf_polar(degrad(d - dcount), degrad(e), &point); + if (vectorf_dot(viewz, &point) <= 0) + break; + + /* Plot points at the given extremes. */ + + colour = e < 0 ? SKY_LOWER_COLOUR : SKY_UPPER_COLOUR; + + /* Current minimum extent of the direction. */ + + thisminx = vectorf_dot(viewx, &point) * PROJECTION_FACTOR + SCREEN_WIDTH / 2; + thisminy = vectorf_dot(viewy, &point) * -PROJECTION_FACTOR + SCREEN_HEIGHT / 2; + + /* Draw from the last point at this elevation. */ + + if (dcount != 0) + { + lineColor(screen, + lastminx, lastminy, + thisminx, thisminy, + colour + ); + } + + /* Draw to the next elevation. */ + + gui_sky_vertical(viewx, viewy, viewz, d - dcount, e, thisminx, thisminy, colour); + + /* Current maximum extent of the direction. */ + + vectorf_polar(degrad(d + dcount), degrad(e), &point); + thismaxx = vectorf_dot(viewx, &point) * PROJECTION_FACTOR + SCREEN_WIDTH / 2; + thismaxy = vectorf_dot(viewy, &point) * -PROJECTION_FACTOR + SCREEN_HEIGHT / 2; + + /* Draw from the last point at this elevation. */ + + if (dcount != 0) + { + lineColor(screen, + lastmaxx, lastmaxy, + thismaxx, thismaxy, + colour + ); + } + + /* Draw to the next elevation. */ + + gui_sky_vertical(viewx, viewy, viewz, d + dcount, e, thismaxx, thismaxy, colour); + + /* Stop after one point at -90 or 90 degree elevations. */ + + if (abs(e) == 90) + { + pixelColor(screen, + thisminx, thisminy, + colour + ); + break; + } + + lastminx = thisminx; lastminy = thisminy; + lastmaxx = thismaxx; lastmaxy = thismaxy; + } + } +} + +void gui_sky_vertical(vectorf *viewx, vectorf *viewy, vectorf *viewz, int direction, int elevation, int16_t x, int16_t y, uint32_t colour) +{ + vectorf point; + + if ((elevation >= -(90 - SKY_GRID_STEP)) || (elevation <= (90 - SKY_GRID_STEP))) + { + vectorf_polar(degrad(direction), degrad(elevation + SKY_GRID_STEP), &point); + lineColor(screen, + x, y, + vectorf_dot(viewx, &point) * PROJECTION_FACTOR + SCREEN_WIDTH / 2, + vectorf_dot(viewy, &point) * -PROJECTION_FACTOR + SCREEN_HEIGHT / 2, + colour + ); + + if (elevation == -(90 - SKY_GRID_STEP)) + { + vectorf_polar(degrad(direction), degrad(elevation - SKY_GRID_STEP), &point); + lineColor(screen, + x, y, + vectorf_dot(viewx, &point) * PROJECTION_FACTOR + SCREEN_WIDTH / 2, + vectorf_dot(viewy, &point) * -PROJECTION_FACTOR + SCREEN_HEIGHT / 2, + colour + ); + } + } +} + +/** + * Generate motion vector. + */ +void gui_motion(vectorf *viewx, vectorf *viewy, vectorf *viewz, vectorf *accelerationD) +{ + vectorf point; + uint32_t colour; + + if (vectorf_dot(viewz, accelerationD) <= 0) + { + colour = MOTION_REVERSE_COLOUR; + vectorf_negate(accelerationD, &point); + } + else + { + colour = MOTION_FORWARD_COLOUR; + point = *accelerationD; + } + + lineColor(screen, + SCREEN_WIDTH / 2, + SCREEN_HEIGHT / 2, + vectorf_dot(viewx, &point) * PROJECTION_FACTOR + SCREEN_WIDTH / 2, + vectorf_dot(viewy, &point) * -PROJECTION_FACTOR + SCREEN_HEIGHT / 2, + colour + ); +} + +/** + * Show the given point in space relative to the origin. + */ +void gui_point(vectorf *viewx, vectorf *viewy, vectorf *viewz, vectorf *point, uint8_t r, uint8_t g, uint8_t b, uint8_t a) +{ + int16_t x, y, radius; + double z; + + if (fabs(vectorf_dot(viewz, point)) < 1) + return; + + z = vectorf_dot(viewz, point); + x = vectorf_dot(viewx, point) * PROJECTION_FACTOR / z + SCREEN_WIDTH / 2; + y = vectorf_dot(viewy, point) * -PROJECTION_FACTOR / z + SCREEN_HEIGHT / 2; + radius = abs(PROJECTION_FACTOR / z); + + if (vectorf_dot(viewz, point) < 0) + circleRGBA(screen, + x, y, radius, + r, g, b, a + ); + else + filledCircleRGBA(screen, + x, y, radius, + r, g, b, a + ); +} + +/** + * Generate a view from the given point in space of the origin. + */ +void gui_origin(vectorf *viewx, vectorf *viewy, vectorf *viewz, vectorf *point) +{ + vectorf origin; + + vectorf_negate(point, &origin); + gui_point(viewx, viewy, viewz, &origin, 255, 255, 255, 127); +} + +/** + * Draw a simple bar representation of the given vector. + */ +void gui_bar(vectorf *value) +{ + lineRGBA(screen, + SCREEN_WIDTH / 2, + SCREEN_HEIGHT / 2, + SCREEN_WIDTH / 2 + value->x * SCREEN_WIDTH / 2, + SCREEN_HEIGHT / 2 - value->y * SCREEN_HEIGHT / 2, + 255, 255, 255, 255); + + circleRGBA(screen, + SCREEN_WIDTH / 2 + value->x * SCREEN_WIDTH / 2, + SCREEN_HEIGHT / 2 - value->y * SCREEN_HEIGHT / 2, + fabs(value->z) * SCREEN_WIDTH / 2, + 255, 255, 255, 255); +} + +void gui_plot(uint16_t x, uint16_t y, uint8_t r, uint8_t g, uint8_t b, uint8_t a) +{ + pixelRGBA(screen, x, y, r, g, b, a); +} + +void gui_flush() +{ + SDL_Flip(screen); +} + +imu_ui_op gui_handle_events() +{ + SDL_Event event; + + while (SDL_PollEvent(&event)) + { + switch (event.type) + { + case SDL_KEYDOWN: + switch (event.key.keysym.sym) + { + case SDLK_ESCAPE: + case SDLK_q: + return IMU_UI_OP_QUIT; + + case SDLK_c: + return IMU_UI_OP_CALIBRATE; + + case SDLK_r: + return IMU_UI_OP_RESET; + + case SDLK_p: + return IMU_UI_OP_PAUSE; + + default: + break; + } + break; + + case SDL_QUIT: + return IMU_UI_OP_QUIT; + + default: + break; + } + } + + return IMU_UI_OP_NULL; +} diff -r 000000000000 -r 631f88781de8 gui.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/gui.h Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,68 @@ +/* + * Ben NanoNote graphical user interface utilities. + * + * Copyright (C) 2013 Paul Boddie + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ + +#ifndef __GUI_H__ +#define __GUI_H__ + +#include "SDL.h" +#include "SDL_gfxPrimitives.h" +#include "ui.h" +#include "geo.h" + +typedef struct +{ + uint16_t x, y; + uint8_t r, g, b, a; +} gui_printer; + +/* Device properties. */ + +#define SCREEN_WIDTH 320 +#define SCREEN_HEIGHT 240 + +#define SCREEN_COLUMN_WIDTH 8 +#define SCREEN_ROW_HEIGHT 8 + +#define GUI_BUFSIZE 256 + +#define PROJECTION_FACTOR 320 +#define SKY_GRID_STEP 15 + +/* Colours in RGBA format. */ + +#define SKY_UPPER_COLOUR 0xff0000ff +#define SKY_LOWER_COLOUR 0x0000ffff +#define MOTION_REVERSE_COLOUR 0xff00ffff +#define MOTION_FORWARD_COLOUR 0x00ff00ff + +/* Access functions. */ + +void gui_init(); +void gui_display_init(); +void gui_shutdown(int signum); +void gui_shutdown_threaded(int signum); +void gui_quit(); +void gui_clear(); +void gui_fill(uint8_t r, uint8_t g, uint8_t b); +void gui_next_row(gui_printer *printer, uint16_t rows); +void gui_next_column(gui_printer *printer, uint16_t columns); +int gui_printf(const char *format, ...); +void gui_sky(vectorf *viewx, vectorf *viewy, vectorf *viewz); +void gui_sky_vertical(vectorf *viewx, vectorf *viewy, vectorf *viewz, int direction, int elevation, int16_t x, int16_t y, uint32_t colour); +void gui_motion(vectorf *viewx, vectorf *viewy, vectorf *viewz, vectorf *accelerationD); +void gui_point(vectorf *viewx, vectorf *viewy, vectorf *viewz, vectorf *point, uint8_t r, uint8_t g, uint8_t b, uint8_t a); +void gui_origin(vectorf *viewx, vectorf *viewy, vectorf *viewz, vectorf *point); +void gui_bar(vectorf *value); +void gui_plot(uint16_t x, uint16_t y, uint8_t r, uint8_t g, uint8_t b, uint8_t a); +void gui_flush(); +imu_ui_op gui_handle_events(); + +#endif /* __GUI_H__ */ diff -r 000000000000 -r 631f88781de8 i2c.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/i2c.c Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,183 @@ +/* + * Ben NanoNote I2C communication. + * + * Copyright (C) 2013 Paul Boddie + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ + +#include +#include "i2c.h" + +extern uint32_t I2C_SCL, I2C_SDA; + +void wait() +{ + /* NOTE: Would want 600ns or 1300ns, but nanosleep appears too slow. + struct timespec ts = {0, 600}; + nanosleep(&ts, NULL); */ + + uint8_t i; + for (i = 0; i < 200; i++); +} + +/** + * Declare the pins for initial I2C communications. + */ +void i2c_init() +{ + OUT(I2C_SCL); + OUT(I2C_SDA); +} + +/** + * Initiate an I2C transaction. + */ +void i2c_start() +{ + OUT(I2C_SDA); + CLR(I2C_SCL); + wait(); + SET(I2C_SDA); + SET(I2C_SCL); + wait(); + CLR(I2C_SDA); +} + +/** + * Terminate an I2C transaction. + */ +void i2c_stop() +{ + OUT(I2C_SDA); + CLR(I2C_SCL); + wait(); + CLR(I2C_SDA); + SET(I2C_SCL); + wait(); + SET(I2C_SDA); +} + +/** + * Send an I2C acknowledgement to a transmitting device. + */ +void i2c_ack(bool ack) +{ + OUT(I2C_SDA); + + if (ack) + CLR(I2C_SDA); + else + SET(I2C_SDA); + + SET(I2C_SCL); + wait(); + CLR(I2C_SCL); + + IN(I2C_SDA); +} + +/** + * Receive a single byte from an I2C device as part of a transaction. + */ +uint8_t i2c_recv() +{ + uint8_t mask, result = 0; + + IN(I2C_SDA); + CLR(I2C_SCL); + + for (mask = 0x80; mask; mask >>= 1) + { + SET(I2C_SCL); + wait(); + + if (PIN(I2C_SDA)) + result |= mask; + + CLR(I2C_SCL); + wait(); + } + + return result; +} + +/** + * Receive into a buffer a transmission of the given length in bytes. + */ +void i2c_recvmany(uint8_t *data, uint8_t len) +{ + uint8_t *end = data + len; + + for (; data != end; data++, len--) + { + *data = i2c_recv(); + i2c_ack(len > 1); + } +} + +void i2c_wait() +{ + IN(I2C_SCL); + while (!PIN(I2C_SCL)); + OUT(I2C_SCL); +} + +/** + * Send a single byte of data to an I2C device as part of a transaction, + * returning whether the transmission succeeded. + */ +bool i2c_send(uint8_t data) +{ + uint8_t mask; + bool status; + + OUT(I2C_SDA); + CLR(I2C_SCL); + + for (mask = 0x80; mask; mask >>= 1) + { + wait(); + + if (data & mask) + SET(I2C_SDA); + else + CLR(I2C_SDA); + + SET(I2C_SCL); + wait(); + CLR(I2C_SCL); + } + + /* Wait for acknowledgement, failing if none is given. */ + + IN(I2C_SDA); + SET(I2C_SCL); + wait(); + + status = PIN(I2C_SDA); + CLR(I2C_SCL); + return !status; +} + +/** + * Send from the buffer provided a transmission with the given length to an I2C + * device. + */ +bool i2c_sendmany(uint8_t *data, uint8_t len) +{ + uint8_t *end = data + len; + + for (; data != end; data++) + { + if (!i2c_send(*data)) + return false; + + /* NOTE: Should test for the slave holding the clock signal low. */ + } + + return true; +} diff -r 000000000000 -r 631f88781de8 i2c.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/i2c.h Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,39 @@ +/* + * Ben NanoNote I2C communication. + * + * Copyright (C) 2013 Paul Boddie + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ + +#ifndef __I2C_H__ +#define __I2C_H__ + +#include +#include "bool.h" + +/* I2C modifiers. */ + +#define I2C_READ 1 +#define I2C_WRITE 0 + +/* Functions. */ + +void i2c_init(); +void i2c_start(); +void i2c_stop(); +void i2c_ack(bool ack); +uint8_t i2c_recv(); +void i2c_recvmany(uint8_t *data, uint8_t len); +void i2c_wait(); +bool i2c_send(uint8_t data); +bool i2c_sendmany(uint8_t *data, uint8_t len); + +/* Function aliases. */ + +#define i2c_repeated_start i2c_start + +#endif /* __I2C_H__ */ diff -r 000000000000 -r 631f88781de8 imu.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/imu.c Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,318 @@ +/* + * Ben NanoNote communication with the Pololu MinIMU-9 with the L3G4200D 3-axis + * gyroscope and LSM303DLM accelerometer/magnetometer. + * + * http://www.pololu.com/catalog/product/1265 + * + * Copyright (C) 2013 Paul Boddie + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ + +#include +#include +#include "imu.h" + +const uint32_t I2C_SCL = IMU_SCL, I2C_SDA = IMU_SDA; + +/** + * Receive data from the indicated device, using the given device register, and + * storing the result in the buffer provided, expecting a transmission of the + * specified length in bytes. Return whether the transaction was successful. + */ +bool imu_recv(uint8_t device, uint8_t reg, uint8_t *result, uint8_t len) +{ + uint8_t data[] = {device | I2C_WRITE, reg}; + + memset(result, 0, len); + + /* Select the register. */ + + i2c_start(); + if (!i2c_sendmany(data, 2)) + { + i2c_stop(); + return false; + } + + /* Continue with a read from the device. */ + + i2c_repeated_start(); + if (!i2c_send(device | I2C_READ)) + { + i2c_stop(); + return false; + } + i2c_recvmany(result, len); + + /* Stop the communication. */ + + i2c_stop(); + return true; +} + +/** + * Send a single byte of data to the indicated device, using the given device + * register. Return whether the transaction was successful. + */ +bool imu_sendone(uint8_t device, uint8_t reg, uint8_t value) +{ + uint8_t data[] = {device | I2C_WRITE, reg, value}; + + /* Select the register. */ + + i2c_start(); + if (!i2c_sendmany(data, 3)) + { + i2c_stop(); + return false; + } + + /* Stop the communication. */ + + i2c_stop(); + return true; +} + +/** + * Convert the first two bytes of the given raw data to a signed integer. + */ +int16_t convert(uint8_t raw[]) +{ + uint16_t raw16 = raw[0] | raw[1] << 8; + if (raw16 & 0x8000) + return -(raw16 ^ 0xffff) - 1; + else + return raw16; +} + +/** + * Convert the first two bytes of the given raw data to a signed integer, + * discarding the least significant four bits of the raw data. + */ +int16_t convert12(uint8_t raw[]) +{ + uint16_t raw16 = ((raw[0] >> 4) & 0x0f) | (raw[1] << 4); + if (raw16 & 0x0800) + return -(raw16 ^ 0x0fff) - 1; + else + return raw16; +} + +/** + * Convert the first two bytes of the given raw data to a signed integer, + * discarding the most significant four bits of the raw data. + */ +int16_t convertBE12L(uint8_t raw[]) +{ + uint16_t raw16 = ((raw[0] & 0x0f) << 8) | raw[1]; + if (raw16 & 0x0800) + return -(raw16 ^ 0x0fff) - 1; + else + return raw16; +} + +/** + * Receive data from the indicated device, using the given device register, and + * storing the result in the output parameters provided. The given converter + * function will convert the raw data to the output form. Return whether the + * transaction was successful. + */ +bool imu_read_vector(uint8_t device, uint8_t reg, vectorf *out, int16_t (*converter)(uint8_t *)) +{ + uint8_t result[6]; + + if (!imu_recv(device, reg, result, 6)) + return false; + + out->x = converter(result); + out->y = converter(result + 2); + out->z = converter(result + 4); + + return true; +} + +bool imu_read_vector_xzy(uint8_t device, uint8_t reg, vectorf *out, int16_t (*converter)(uint8_t *)) +{ + double tmp; + + if (imu_read_vector(device, reg, out, converter)) + { + tmp = out->y; + out->y = out->z; + out->z = tmp; + return true; + } + + return false; +} + +/** + * Return the given value scaled to the range defined by lower, middle and upper + * points. + */ +double scale(double value, double lower, double middle, double upper) +{ + if (value < middle) + return (value - middle) / (middle - lower); + else + return (value - middle) / (upper - middle); +} + +/** + * Normalise the given vector based on the minimum and maximum values. + */ +void normalise(vectorf *in, vectorf *vmin, vectorf *vmax, vectorf *out) +{ + out->x = scale(in->x, vmin->x, (vmin->x + vmax->x) / 2, vmax->x); + out->y = scale(in->y, vmin->y, (vmin->y + vmax->y) / 2, vmax->y); + out->z = scale(in->z, vmin->z, (vmin->z + vmax->z) / 2, vmax->z); +} + +/** + * Add a measurement to a circular buffer usable for filtering. + */ +void queue(vectorf values[], int *oldest, int length, vectorf *value) +{ + values[*oldest] = *value; + *oldest = (*oldest + 1) % length; +} + +/** + * Return a filtered measurement using the given values from a circular buffer + * with the specified oldest measurement index, and with the buffer having the + * given length. The filtered measurement is placed in the result vector. + */ +void filter(vectorf values[], int oldest, int length, vectorf *result) +{ + int i; + + result->x = 0; + result->y = 0; + result->z = 0; + + /* NOTE: For now, a plain average is used. */ + + for (i = 0; i < length; i++) + { + result->x += values[i].x; + result->y += values[i].y; + result->z += values[i].z; + } + + result->x /= length; + result->y /= length; + result->z /= length; +} + +/** + * Populate the given zero base/levels for the device with the given address, + * using the specified register to obtain measurements, performing the given + * number of warm-up measurements and then proper readings, with the specified + * delay between each one. + */ +void calibrate(vectorf *zerolevel, vectorf zerolevels[], int length, uint8_t address, uint8_t reg, uint16_t delay, int16_t (*converter)(uint8_t *)) +{ + int reading = 0, index = 0; + vectorf level; + + zerolevel->x = 0; + zerolevel->y = 0; + zerolevel->z = 0; + + while (reading < IMU_CALIBRATION_WARMUP + IMU_CALIBRATION_READINGS) + { + if (imu_read_vector(address, reg, &zerolevels[index], converter)) + { + index = (index + 1) % length; + + /* Ignore the first few readings. */ + + if (reading >= IMU_CALIBRATION_WARMUP) + { + filter(zerolevels, index, length, &level); + zerolevel->x += level.x; + zerolevel->y += level.y; + zerolevel->z += level.z; + } + + reading += 1; + } + + usleep(delay); + } + + zerolevel->x /= IMU_CALIBRATION_READINGS; + zerolevel->y /= IMU_CALIBRATION_READINGS; + zerolevel->z /= IMU_CALIBRATION_READINGS; +} + +/* Accelerometer-specific functions. */ + +void average_filter(vectorf *value, vectorf buffer[], int *index, int length) +{ + queue(buffer, index, length, value); + filter(buffer, *index, length, value); +} + +void noise_filter(vectorf *value, double threshold) +{ + value->x = noise(value->x, threshold); + value->y = noise(value->y, threshold); + value->z = noise(value->z, threshold); +} + +void apply_acceleration(double acc, double acc_old, double *pos, double *neg, double seconds) +{ + if (acc > 0) + *pos += (acc + acc_old) / 2 * seconds; + else + *neg += (acc + acc_old) / 2 * seconds; +} + +void apply_decay(double acc, double* pos, double* neg) +{ + double decay = 1.0; + + if (*pos < -*neg) + { + if ((acc < -ACCEL_THRESHOLD) && (*pos > ACCEL_SUM_THRESHOLD)) + decay = VELOCITY_DECAY_SEVERE; + if (fabs(acc) <= ACCEL_THRESHOLD) + decay = VELOCITY_DECAY_GRADUAL; + *neg *= decay; + } + if (*pos > -*neg) + { + if ((acc > ACCEL_THRESHOLD) && (*neg < -ACCEL_SUM_THRESHOLD)) + decay = VELOCITY_DECAY_SEVERE; + if (fabs(acc) <= ACCEL_THRESHOLD) + decay = VELOCITY_DECAY_GRADUAL; + *pos *= decay; + } +} + +void update_velocity(vectorf *velocity, vectorf *acceleration, vectorf *acceleration_old, vectorf *apos, vectorf *aneg, double seconds) +{ + apply_acceleration(acceleration->x, acceleration_old->x, &(apos->x), &(aneg->x), seconds); + apply_acceleration(acceleration->y, acceleration_old->y, &(apos->y), &(aneg->y), seconds); + apply_acceleration(acceleration->z, acceleration_old->z, &(apos->z), &(aneg->z), seconds); + + apply_decay(acceleration->x, &(apos->x), &(aneg->x)); + apply_decay(acceleration->y, &(apos->y), &(aneg->y)); + apply_decay(acceleration->z, &(apos->z), &(aneg->z)); + + velocity->x = apos->x + aneg->x; + velocity->y = apos->y + aneg->y; + velocity->z = apos->z + aneg->z; +} + +void update_displacement(vectorf *displacement, vectorf *velocity, vectorf *velocity_old, double seconds) +{ + displacement->x += (velocity->x + velocity_old->x) / 2 * seconds; + displacement->y += (velocity->y + velocity_old->y) / 2 * seconds; + displacement->z += (velocity->z + velocity_old->z) / 2 * seconds; +} diff -r 000000000000 -r 631f88781de8 imu.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/imu.h Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,260 @@ +/* + * Ben NanoNote communication with the Pololu MinIMU-9 with the L3G4200D 3-axis + * gyroscope and LSM303DLM accelerometer/magnetometer. + * + * http://www.pololu.com/catalog/product/1265 + * + * Copyright (C) 2013 Paul Boddie + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ + +#ifndef __IMU_H__ +#define __IMU_H__ + +#include +#include "i2c.h" +#include "geo.h" + +/* Pin assignments: + * + * Sniffer UBB MinIMU-9 + * ------- ---- -------- + * DAT2 DAT2 + * CD DAT3 SCL + * CMD CMD SDA + * VCC VDD VIN + * CLK CLK (3V) + * GND GND GND + * DAT0 DAT0 (1V8) + * DAT1 DAT1 + * + * The 3V and 1V8 pins play no role in the communication, but a six pin header + * exposing these pins on the IMU can be used to connect the IMU with the + * Sniffer/UBB without any complications. + */ + +#define IMU_SCL UBB_DAT3 +#define IMU_SDA UBB_CMD +#define IMU_3V UBB_CLK +#define IMU_1V8 UBB_DAT0 + +/* I2C addresses and operations. */ + +#define IMU_GYRO_ADDRESS 0xd2 /* 1101001x */ +#define IMU_ACCEL_ADDRESS 0x30 /* 0011000x */ +#define IMU_MAGNET_ADDRESS 0x3c /* 0011110x */ + +/* Gyroscope registers. */ + +#define IMU_GYRO_WHO_AM_I 0x0f +#define IMU_GYRO_CTRL_REG1 0x20 +#define IMU_GYRO_CTRL_REG4 0x23 +#define IMU_GYRO_CTRL_REG5 0x24 +#define IMU_GYRO_OUT_TEMP 0x26 +#define IMU_GYRO_STATUS_REG 0x27 + +/* Gyroscope register values. */ + +#define IMU_GYRO_CTRL_REG1_PD 0x08 +#define IMU_GYRO_CTRL_REG1_ZEN 0x04 +#define IMU_GYRO_CTRL_REG1_YEN 0x02 +#define IMU_GYRO_CTRL_REG1_XEN 0x01 +#define IMU_GYRO_CTRL_REG1_ALL 0x0f /* composite of power and all axes */ +#define IMU_GYRO_CTRL_REG4_BDU 0x80 +#define IMU_GYRO_CTRL_REG4_250DPS 0x00 /* FS1 = 0; FS0 = 0 */ +#define IMU_GYRO_CTRL_REG4_500DPS 0x10 /* FS1 = 0; FS0 = 1 */ +#define IMU_GYRO_CTRL_REG4_2000DPS 0x20 /* FS1 = 1; FS0 = 0 */ +#define IMU_GYRO_CTRL_REG5_BOOT 0x80 +#define IMU_GYRO_CTRL_REG5_HPEN 0x10 +#define IMU_GYRO_CTRL_REG5_OUTSEL1 0x02 +#define IMU_GYRO_CTRL_REG5_OUTSEL0 0x01 +#define IMU_GYRO_OUT_X_L 0x28 +#define IMU_GYRO_OUT_X_H 0x29 +#define IMU_GYRO_OUT_Y_L 0x2a +#define IMU_GYRO_OUT_Y_H 0x2b +#define IMU_GYRO_OUT_Z_L 0x2c +#define IMU_GYRO_OUT_Z_H 0x2d +#define IMU_GYRO_READ_MANY 0x80 /* gyroscope register increment */ + +/* Accelerometer registers. */ + +#define IMU_ACCEL_CTRL_REG1_A 0x20 +#define IMU_ACCEL_CTRL_REG2_A 0x21 +#define IMU_ACCEL_CTRL_REG4_A 0x23 +#define IMU_ACCEL_HP_FILTER_RESET_A 0x25 +#define IMU_ACCEL_OUT_X_L_A 0x28 +#define IMU_ACCEL_OUT_X_H_A 0x29 +#define IMU_ACCEL_OUT_Y_L_A 0x2a +#define IMU_ACCEL_OUT_Y_H_A 0x2b +#define IMU_ACCEL_OUT_Z_L_A 0x2c +#define IMU_ACCEL_OUT_Z_H_A 0x2d +#define IMU_ACCEL_READ_MANY 0x80 /* accelerometer register increment */ + +/* Accelerometer register values. */ + +#define IMU_ACCEL_CTRL_REG1_50HZ 0x20 /* normal power */ +#define IMU_ACCEL_CTRL_REG1_100HZ 0x28 /* normal power */ +#define IMU_ACCEL_CTRL_REG1_400HZ 0x30 /* normal power */ +#define IMU_ACCEL_CTRL_REG1_1000HZ 0x38 /* normal power */ +#define IMU_ACCEL_CTRL_REG1_05HZ 0x40 /* low power, 0.5Hz */ +#define IMU_ACCEL_CTRL_REG1_1HZ 0x60 /* low power, 1Hz */ +#define IMU_ACCEL_CTRL_REG1_2HZ 0x80 /* low power, 2Hz */ +#define IMU_ACCEL_CTRL_REG1_5HZ 0x90 /* low power, 5Hz */ +#define IMU_ACCEL_CTRL_REG1_10HZ 0xa0 /* low power, 10Hz */ +#define IMU_ACCEL_CTRL_REG1_ZEN 0x04 +#define IMU_ACCEL_CTRL_REG1_YEN 0x02 +#define IMU_ACCEL_CTRL_REG1_XEN 0x01 +#define IMU_ACCEL_CTRL_REG1_ALL 0x07 /* composite of all axes */ +#define IMU_ACCEL_CTRL_REG2_FDS 0x10 +#define IMU_ACCEL_CTRL_REG2_50 0x00 /* filter cut off is rate/50 */ +#define IMU_ACCEL_CTRL_REG2_100 0x01 /* filter cut off is rate/100 */ +#define IMU_ACCEL_CTRL_REG2_200 0x02 /* filter cut off is rate/200 */ +#define IMU_ACCEL_CTRL_REG2_400 0x03 /* filter cut off is rate/400 */ +#define IMU_ACCEL_CTRL_REG2_HPCF1 0x02 +#define IMU_ACCEL_CTRL_REG4_BDU 0x80 +#define IMU_ACCEL_CTRL_REG4_2G 0x00 /* FS1 = 0; FS0 = 0 */ +#define IMU_ACCEL_CTRL_REG4_4G 0x10 /* FS1 = 0; FS0 = 1 */ +#define IMU_ACCEL_CTRL_REG4_8G 0x30 /* FS1 = 1; FS0 = 1 */ + +/* Magnetometer registers. */ + +#define IMU_MAGNET_CRA_REG_M 0x00 +#define IMU_MAGNET_CRB_REG_M 0x01 +#define IMU_MAGNET_MR_REG_M 0x02 +#define IMU_MAGNET_OUT_X_H_M 0x03 +#define IMU_MAGNET_OUT_X_L_M 0x04 +#define IMU_MAGNET_OUT_Z_H_M 0x05 +#define IMU_MAGNET_OUT_Z_L_M 0x06 +#define IMU_MAGNET_OUT_Y_H_M 0x07 +#define IMU_MAGNET_OUT_Y_L_M 0x08 +#define IMU_MAGNET_SR_REG_M 0x09 +#define IMU_MAGNET_IRA_REG_M 0x0a +#define IMU_MAGNET_IRB_REG_M 0x0b +#define IMU_MAGNET_IRC_REG_M 0x0c +#define IMU_MAGNET_WHO_AM_I_M 0x0f + +/* Magnetometer register values. */ + +#define IMU_MAGNET_CRA_REG_0_75HZ 0x00 /* 0.75Hz */ +#define IMU_MAGNET_CRA_REG_1_5HZ 0x04 /* 1.5Hz */ +#define IMU_MAGNET_CRA_REG_3HZ 0x08 /* 3Hz */ +#define IMU_MAGNET_CRA_REG_7_5HZ 0x0c /* 7.5Hz */ +#define IMU_MAGNET_CRA_REG_15HZ 0x10 /* 15Hz */ +#define IMU_MAGNET_CRA_REG_30HZ 0x14 /* 30Hz */ +#define IMU_MAGNET_CRA_REG_75HZ 0x18 /* 75Hz */ +#define IMU_MAGNET_CRA_REG_220HZ 0x1c /* 220Hz */ +#define IMU_MAGNET_CRB_REG_1_3G 0x20 /* 1.3G */ +#define IMU_MAGNET_CRB_REG_1_9G 0x40 /* 1.9G */ +#define IMU_MAGNET_CRB_REG_2_5G 0x60 /* 2.5G */ +#define IMU_MAGNET_CRB_REG_4_0G 0x80 /* 4.0G */ +#define IMU_MAGNET_CRB_REG_4_7G 0xa0 /* 4.7G */ +#define IMU_MAGNET_CRB_REG_5_6G 0xc0 /* 5.6G */ +#define IMU_MAGNET_CRB_REG_8_1G 0xe0 /* 8.1G */ +#define IMU_MAGNET_MR_REG_CONT 0x00 +#define IMU_MAGNET_MR_REG_SINGLE 0x01 +#define IMU_MAGNET_MR_REG_SLEEP 0x02 + +/* Common values. */ + +#define IMU_250DPS_UNIT 8750 /* µdps -> 8.75mdps */ +#define IMU_500DPS_UNIT 17500 /* µdps -> 17.5mdps */ +#define IMU_2000DPS_UNIT 70000 /* µdps -> 70mdps */ + +#define IMU_2G_UNIT 1000 /* µg -> 1mg */ +#define IMU_4G_UNIT 2000 /* µg -> 2mg */ +#define IMU_8G_UNIT 3900 /* µg -> 3.9mg */ + +#define IMU_1_3G_UNIT 909 /* µG (1G / 1100) */ +#define IMU_1_9G_UNIT 1169 /* µG (1G / 855) */ +#define IMU_2_5G_UNIT 1492 /* µG (1G / 670) */ +#define IMU_4_0G_UNIT 2222 /* µG (1G / 450) */ +#define IMU_4_7G_UNIT 2500 /* µG (1G / 400) */ +#define IMU_5_6G_UNIT 3030 /* µG (1G / 330) */ +#define IMU_8_1G_UNIT 4347 /* µG (1G / 230) */ + +#define IMU_MAGNET_Z_XY_RATIO (9.0 / 8.0) /* adjustments to the above for the smaller Z scale */ + +/* Measurement parameters. */ + +#define IMU_UPDATE_PERIOD 10000 /* µs -> 10ms == 0.01s (100Hz) */ +#define IMU_MAGNET_UPDATE_PERIOD 30000 /* µs -> 30ms == 0.03s (33Hz) */ +#define TEXT_UPDATE_PERIOD 500000 /* µs */ +#define GUI_UPDATE_PERIOD 100000 /* µs */ + +#define IMU_CALIBRATION_WARMUP 100 +#define IMU_CALIBRATION_READINGS 100 + +#define IMU_ACCEL_BUFFER_SIZE 10 + +#define IMU_UDPS_FACTOR IMU_2000DPS_UNIT +#define IMU_GYRO_DPS_SCALE IMU_GYRO_CTRL_REG4_2000DPS + +#define IMU_UG_FACTOR IMU_2G_UNIT +#define IMU_ACCEL_SCALE IMU_ACCEL_CTRL_REG4_2G +#define IMU_ACCEL_FREQ IMU_ACCEL_CTRL_REG1_50HZ +#define IMU_ACCEL_FILTER_FREQ IMU_ACCEL_CTRL_REG2_50 + +#define IMU_UGAUSS_FACTOR IMU_4_0G_UNIT +#define IMU_MAGNET_SCALE IMU_MAGNET_CRB_REG_4_0G +#define IMU_MAGNET_FREQ IMU_MAGNET_CRA_REG_30HZ + +#define ACCEL_G 9.81 /* ms**-2 */ + +#define ACCEL_THRESHOLD 0.1 /* g */ +#define ACCEL_SUM_THRESHOLD 0.01 /* gs */ +#define VELOCITY_DECAY_SEVERE 0.5 +#define VELOCITY_DECAY_GRADUAL 0.9 + +#define ACCEL_REST_MAG_LOWER 0.995 +#define ACCEL_REST_MAG_UPPER 1.005 +#define ROTATION_ADJUSTMENT_FACTOR 0.1 + +/* Convert microdegrees * microseconds to degrees. */ + +#define to_angle(x) ((double) (x) / ((uint64_t) 1000000000000)) + +/* Convert microg to g. */ + +#define to_accel(x) ((double) (x) / 1000000) + +/* Convert microgauss to gauss. */ + +#define to_field(x) to_accel(x) + +/* Get a period in microseconds. */ + +#define get_period(now, then) ((now.tv_sec - then.tv_sec) * 1000000 + (now.tv_usec - then.tv_usec)) + +/* Function definitions. */ + +bool imu_recv(uint8_t device, uint8_t reg, uint8_t *result, uint8_t len); +bool imu_sendone(uint8_t device, uint8_t reg, uint8_t value); +int16_t convert(uint8_t raw[]); +int16_t convert12(uint8_t raw[]); +int16_t convertBE12L(uint8_t raw[]); +bool imu_read_vector(uint8_t device, uint8_t reg, vectorf *out, int16_t (*converter)(uint8_t *)); +bool imu_read_vector_xzy(uint8_t device, uint8_t reg, vectorf *out, int16_t (*converter)(uint8_t *)); +double scale(double value, double lower, double middle, double upper); +void normalise(vectorf *in, vectorf *vmin, vectorf *vmax, vectorf *out); +void queue(vectorf values[], int *oldest, int length, vectorf *value); +void filter(vectorf values[], int oldest, int length, vectorf *result); +void calibrate(vectorf *zerolevel, vectorf zerolevels[], int length, uint8_t address, uint8_t reg, uint16_t delay, int16_t (*converter)(uint8_t *)); + +/* Accelerometer-specific functions. */ + +void average_filter(vectorf *value, vectorf buffer[], int *index, int length); +void noise_filter(vectorf *value, double threshold); +void apply_acceleration(double acc, double acc_old, double *pos, double *neg, double seconds); +void apply_decay(double acc, double* pos, double* neg); +void update_velocity(vectorf *velocity, vectorf *acceleration, vectorf *acceleration_old, vectorf *apos, vectorf *aneg, double seconds); +void update_displacement(vectorf *displacement, vectorf *velocity, vectorf *velocity_old, double seconds); + +/* Function aliases. */ + +#define imu_init i2c_init + +#endif /* __IMU_H__ */ diff -r 000000000000 -r 631f88781de8 itest.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/itest.c Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,176 @@ +/* + * Test Pololu MinIMU-9 measurements. + * + * Copyright (C) 2013 Paul Boddie + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ + +#include +#include +#include +#include +#include +#include +#include +#include "imu.h" +#include "shutdown.h" +#include "gui.h" +#include "measure.h" + +int crosses_axis(double current, double previous) +{ + if ((current == 0) && (previous != 0)) + return -sign(previous); + if ((current != previous) && (sign(current) == -sign(previous))) + return sign(current); + return 0; +} + +/* Main program. */ + +int main(int argc, char *argv[]) +{ + void *threadresult; + uint8_t result[6]; + int readings; + uint32_t period; + struct timeval now, ui_updated; + bool paused = false, using_filter = false; + + signal(SIGINT, init_shutdown); + + /* Access the 8:10 port. */ + + if (ubb_open(0) < 0) { + perror("ubb_open"); + return 1; + } + + ubb_power(1); + printf("Power on.\n"); + + /* Bring the IMU up. */ + + imu_init(); + + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG1, IMU_GYRO_CTRL_REG1_ALL); + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG1_A, IMU_ACCEL_CTRL_REG1_ALL | IMU_ACCEL_CTRL_REG1_50HZ); + + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG5, 0); + + if ((argc > 1) && (strcmp(argv[1], "-f") == 0)) + { + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, IMU_ACCEL_CTRL_REG2_FDS | IMU_ACCEL_FILTER_FREQ); + imu_recv(IMU_ACCEL_ADDRESS, IMU_ACCEL_HP_FILTER_RESET_A, result, 1); + using_filter = true; + } + else + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, 0); + + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG4, IMU_GYRO_CTRL_REG4_BDU | IMU_GYRO_DPS_SCALE); + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG4_A, IMU_ACCEL_CTRL_REG4_BDU | IMU_ACCEL_SCALE); + + if (imu_recv(IMU_GYRO_ADDRESS, IMU_GYRO_WHO_AM_I, result, 1)) + printf("Who am I? %x\n", result[0]); + + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_WHO_AM_I_M, result, 1)) + printf("Who am I? %x\n", result[0]); + + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRA_REG_M, result, 1)) + printf("Identification A? %x\n", result[0]); + + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRB_REG_M, result, 1)) + printf("Identification B? %x\n", result[0]); + + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRC_REG_M, result, 1)) + printf("Identification C? %x\n", result[0]); + + gui_init(); + gui_display_init(); + + signal(SIGINT, gui_shutdown); + + memset(accelerationB, 0, sizeof(accelerationB)); + + ui_calibrate(using_filter, gui_printf, gui_flush); + + memset(accelerationB, 0, sizeof(accelerationB)); + + readings = 0; + gui_clear(); + gui_printf("A0: % 8.1f, % 8.1f, % 8.1f\n", acceleration0.x, acceleration0.y, acceleration0.z); + + /* Create a measurement thread. */ + + if (pthread_create(&thread, NULL, get_measurements, &using_filter) != 0) + { + perror("pthread_create"); + gui_shutdown(0); + } + + signal(SIGINT, gui_shutdown_threaded); + + pthread_mutex_init(&mutex, NULL); + + gettimeofday(&ui_updated, NULL); + + while (1) + { + gettimeofday(&now, NULL); + + period = get_period(now, ui_updated); + + if ((period >= IMU_UPDATE_PERIOD) && (!paused)) + { + ui_updated = now; + readings = (readings + 1) % SCREEN_WIDTH; + + if (readings == 0) + gui_clear(); + + pthread_mutex_lock(&mutex); + + gui_plot(readings % SCREEN_WIDTH, SCREEN_HEIGHT / 2 * (1 - acceleration.x), 255, 0, 0, 255); + gui_plot(readings % SCREEN_WIDTH, SCREEN_HEIGHT / 2 * (1 - acceleration.y), 0, 255, 0, 255); + gui_plot(readings % SCREEN_WIDTH, SCREEN_HEIGHT / 2 * (1 - acceleration.z), 0, 0, 255, 255); + + gui_flush(); + + printf("A: % 6.4f, % 6.4f, % 6.4f ", + acceleration.x, acceleration.y, acceleration.z); + + printf("DET: % 6.4f, % 6.4f, % 6.4f\n", direction, elevation, tilt); + + pthread_mutex_unlock(&mutex); + } + + switch (gui_handle_events()) + { + case IMU_UI_OP_QUIT: + pthread_cancel(thread); + pthread_join(thread, &threadresult); + gui_quit(); + return 0; + + case IMU_UI_OP_PAUSE: + paused = !paused; + break; + + default: + break; + } + + usleep(IMU_UPDATE_PERIOD); + } + + /* This should be unreachable. */ + + pthread_cancel(thread); + pthread_join(thread, &threadresult); + gui_quit(); + return 0; +} diff -r 000000000000 -r 631f88781de8 main.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.c Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,274 @@ +/* + * Ben NanoNote communication with the Pololu MinIMU-9 with the L3G4200D 3-axis + * gyroscope and LSM303DLM accelerometer/magnetometer. + * + * http://www.pololu.com/catalog/product/1265 + * + * Copyright (C) 2013 Paul Boddie + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ + +#include +#include +#include +#include +#include +#include +#include "imu.h" +#include "shutdown.h" +#include "gui.h" +#include "ui.h" +#include "measure.h" + +/* Main program. */ + +int main(int argc, char *argv[]) +{ + int argno = 1; + void *threadresult; + uint8_t result[6]; + bool using_filter = false; + + /* Local view state. */ + + vectorf _viewx, _viewy, _viewz, + _accelerationD, _accelerationRD, + _fieldD; + + /* Timekeeping. */ + + struct timeval now, text_updated, gui_updated; + + /* Graphical mode variables. */ + + bool graphical = false; + int (*print)(const char *, ...) = printf; + void (*flush)() = text_flush; + void (*clear)() = text_clear; + void (*quit)() = text_quit; + void (*shutdown)(int) = text_shutdown; + void (*shutdown_threaded)(int) = text_shutdown_threaded; + imu_ui_op (*handle_events)() = text_handle_events; + + memset(accelerationB, 0, sizeof(accelerationB)); + + /* Enable a high-pass filter if requested. */ + + if ((argc > argno) && (strcmp(argv[argno], "-f") == 0)) + { + argno++; + using_filter = true; + } + + /* Initialise graphical or textual mode. */ + + graphical = (argc > argno) && (strcmp(argv[argno], "-g") == 0); + + if (graphical) + { + argno++; + + print = gui_printf; + flush = gui_flush; + clear = gui_clear; + quit = gui_quit; + shutdown = gui_shutdown; + shutdown_threaded = gui_shutdown_threaded; + handle_events = gui_handle_events; + + gui_init(); + gui_display_init(); + } + + signal(SIGINT, init_shutdown); + + /* Access the 8:10 port. */ + + if (ubb_open(0) < 0) { + perror("ubb_open"); + return 1; + } + + ubb_power(1); + printf("Power on.\n"); + + /* Bring the IMU up. */ + + imu_init(); + + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG1, IMU_GYRO_CTRL_REG1_ALL); + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG1_A, IMU_ACCEL_CTRL_REG1_ALL | IMU_ACCEL_FREQ); + + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG5, 0); + + if (using_filter) + { + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, IMU_ACCEL_CTRL_REG2_FDS | IMU_ACCEL_FILTER_FREQ); + imu_recv(IMU_ACCEL_ADDRESS, IMU_ACCEL_HP_FILTER_RESET_A, result, 1); + } + else + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG2_A, 0); + + imu_sendone(IMU_GYRO_ADDRESS, IMU_GYRO_CTRL_REG4, IMU_GYRO_CTRL_REG4_BDU | IMU_GYRO_DPS_SCALE); + imu_sendone(IMU_ACCEL_ADDRESS, IMU_ACCEL_CTRL_REG4_A, IMU_ACCEL_CTRL_REG4_BDU | IMU_ACCEL_SCALE); + + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRA_REG_M, IMU_MAGNET_FREQ); + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_CRB_REG_M, IMU_MAGNET_SCALE); + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_SINGLE); + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_CONT); + + usleep(IMU_MAGNET_UPDATE_PERIOD); + + if (imu_recv(IMU_GYRO_ADDRESS, IMU_GYRO_WHO_AM_I, result, 1)) + printf("Who am I? %x\n", result[0]); + + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_WHO_AM_I_M, result, 1)) + printf("Who am I? %x\n", result[0]); + + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRA_REG_M, result, 1)) + printf("Identification A? %x\n", result[0]); + + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRB_REG_M, result, 1)) + printf("Identification B? %x\n", result[0]); + + if (imu_recv(IMU_MAGNET_ADDRESS, IMU_MAGNET_IRC_REG_M, result, 1)) + printf("Identification C? %x\n", result[0]); + + /* Get average values for the gyroscope and accelerometer. */ + + if ((argc > argno) && (strcmp(argv[argno], "-c") == 0)) + { + argno++; + ui_calibrate(using_filter, print, flush); + } + + /* Reset the acceleration buffer. */ + + memset(accelerationB, 0, sizeof(accelerationB)); + + /* Create a measurement thread. */ + + if (pthread_create(&thread, NULL, get_measurements, &using_filter) != 0) + { + perror("pthread_create"); + shutdown(0); + } + + signal(SIGINT, shutdown_threaded); + + pthread_mutex_init(&mutex, NULL); + + gettimeofday(&now, NULL); + text_updated = now; gui_updated = now; + + /* Refresh the display by obtaining measurements made in the measurement + thread. */ + + while (1) + { + gettimeofday(&now, NULL); + + if (get_period(now, text_updated) >= TEXT_UPDATE_PERIOD) + { + pthread_mutex_lock(&mutex); + + /* Show textual details. */ + + if (!graphical) + { + clear(); + print("Rotation? %.4f, %.4f, %.4f\n", rotation.x, rotation.y, rotation.z); + print("Vector? %.4f, %.4f, %.4f\n", viewx.x, viewx.y, viewx.z); + print("Vector? %.4f, %.4f, %.4f\n", viewy.x, viewy.y, viewy.z); + print("Vector? %.4f, %.4f, %.4f\n", viewz.x, viewz.y, viewz.z); + print("Acceleration? %.4f, %.4f, %.4f\n", accelerationD.x, accelerationD.y, accelerationD.z); + print("Field vector? %.4f, %.4f, %.4f\n", fieldD.x, fieldD.y, fieldD.z); + flush(); + } + + printf("Period? %d\n", imu_period); + printf("Direction? %.4f\n", raddeg(direction)); + printf("Elevation? %.4f (%.4f)\n", raddeg(elevation), raddeg(elevationA)); + printf("Tilt? %.4f (%.4f)\n", raddeg(tilt), raddeg(tiltA)); + printf("Acceleration? %.4f, %.4f, %.4f\n", accelerationD.x, accelerationD.y, accelerationD.z); + + pthread_mutex_unlock(&mutex); + + text_updated = now; + } + + if (graphical && (get_period(now, gui_updated) >= GUI_UPDATE_PERIOD)) + { + pthread_mutex_lock(&mutex); + _viewx = viewx; + _viewy = viewy; + _viewz = viewz; + _accelerationD = accelerationD; + _accelerationRD = accelerationRD; + _fieldD = fieldD; + pthread_mutex_unlock(&mutex); + + clear(); + gui_sky(&_viewx, &_viewy, &_viewz); + + _accelerationRD.x *= 10; + _accelerationRD.y *= 10; + _accelerationRD.z *= 10; + gui_point(&_viewx, &_viewy, &_viewz, &_accelerationRD, 0, 255, 0, 127); + gui_bar(&_accelerationD); + + _fieldD.x *= 10; + _fieldD.y *= 10; + _fieldD.z *= 10; + gui_point(&_viewx, &_viewy, &_viewz, &_fieldD, 0, 0, 255, 127); + + flush(); + + gui_updated = now; + } + + /* Respond to instructions from the UI. */ + + switch (handle_events()) + { + case IMU_UI_OP_QUIT: + pthread_cancel(thread); + pthread_join(thread, &threadresult); + quit(); + return 0; + + case IMU_UI_OP_RESET: + pthread_mutex_lock(&mutex); + devicex = devicex0; + devicey = devicey0; + devicez = devicez0; + vectorf_reset(&accelerationD); + pthread_mutex_unlock(&mutex); + break; + + case IMU_UI_OP_CALIBRATE: + pthread_mutex_lock(&mutex); + clear(); + ui_calibrate(using_filter, print, flush); + flush(); + memset(accelerationB, 0, sizeof(accelerationB)); + gettimeofday(&imu_updated, NULL); + pthread_mutex_unlock(&mutex); + break; + + default: + break; + } + } + + /* This should be unreachable. */ + + pthread_cancel(thread); + pthread_join(thread, &threadresult); + quit(); + return 0; +} diff -r 000000000000 -r 631f88781de8 measure.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/measure.c Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,255 @@ +/* + * Ben NanoNote communication with the Pololu MinIMU-9 with the L3G4200D 3-axis + * gyroscope and LSM303DLM accelerometer/magnetometer. + * + * http://www.pololu.com/catalog/product/1265 + * + * Copyright (C) 2013 Paul Boddie + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ + +#include +#include +#include +#include "imu.h" +#include "geo.h" + +#define __MEASURE_H_PRIVATE__ +#include "measure.h" +#undef __MEASURE_H_PRIVATE__ + +static bool setF0 = false; + +/** + * Perform calibration with feedback given in the user interface. + */ +void ui_calibrate(bool using_filter, int (*print)(const char *, ...), void (*flush)()) +{ + vectorf tmpB[1]; + + print("Calibrating...\n"); + flush(); + + /* Calibrate without a filter for rotation. */ + + calibrate(&rotation0, tmpB, 1, IMU_GYRO_ADDRESS, IMU_GYRO_OUT_X_L | IMU_GYRO_READ_MANY, IMU_UPDATE_PERIOD, convert); + + print("Calibrated using (%.4f, %.4f, %.4f).\n", rotation0.x, rotation0.y, rotation0.z); + flush(); + + /* Calibrate using a filter for acceleration. */ + + calibrate(&acceleration0, accelerationB, IMU_ACCEL_BUFFER_SIZE, + IMU_ACCEL_ADDRESS, IMU_ACCEL_OUT_X_L_A | IMU_ACCEL_READ_MANY, IMU_UPDATE_PERIOD, convert12); + + /* Filter out the expected 1g measurement on the z axis. */ + + if (!using_filter) + acceleration0.z += acceleration1g; + + print("Calibrated using (%.4f, %.4f, %.4f).\n", acceleration0.x, acceleration0.y, acceleration0.z); + flush(); +} + +void *get_measurements(void *arg) +{ + struct timeval now; + uint32_t period; + bool using_filter = false; + double accelerationM; + bool set_reference = false; + + pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL); + + if (arg != NULL) + using_filter = *((bool *) arg); + + /* Initialise the default device orientation. */ + + devicex = devicex0; + devicey = devicey0; + devicez = devicez0; + + /* Note the time to schedule the next update. */ + + gettimeofday(&imu_updated, NULL); + imu_magnet_updated = imu_updated; + + /* NOTE: Wake up the stupid magnetometer. */ + + imu_sendone(IMU_MAGNET_ADDRESS, IMU_MAGNET_MR_REG_M, IMU_MAGNET_MR_REG_CONT); + + /* Actual readings. */ + + while (1) + { + gettimeofday(&now, NULL); + + period = get_period(now, imu_magnet_updated); + + if (period >= IMU_MAGNET_UPDATE_PERIOD) + { + imu_magnet_updated = now; + + pthread_mutex_lock(&mutex); + + if (imu_read_vector_xzy(IMU_MAGNET_ADDRESS, IMU_MAGNET_OUT_X_H_M, + &field, convertBE12L)) + { + /* NOTE: Handle stupid magnetometer readings. */ + + if (!setF0 && (field.x == 0)) + { + field.y = 0; field.z = 0; + } + else + { + normalise(&field, &fieldmin, &fieldmax, &field); + field.x = to_field(field.x * IMU_UGAUSS_FACTOR); + field.y = to_field(field.y * IMU_UGAUSS_FACTOR); + field.z = to_field(field.z * IMU_UGAUSS_FACTOR * IMU_MAGNET_Z_XY_RATIO); + vectorf_normalise(&field, &field); + } + } + + pthread_mutex_unlock(&mutex); + } + + period = get_period(now, imu_updated); + + if (period >= IMU_UPDATE_PERIOD) + { + imu_updated = now; + + pthread_mutex_lock(&mutex); + + imu_period = period; + + if (imu_read_vector(IMU_GYRO_ADDRESS, IMU_GYRO_OUT_X_L | IMU_GYRO_READ_MANY, + &rotation, convert)) + { + rotation.x -= rotation0.x; + rotation.y -= rotation0.y; + rotation.z -= rotation0.z; + + rotation.x = to_angle(rotation.x * IMU_UDPS_FACTOR * period); + rotation.y = to_angle(rotation.y * IMU_UDPS_FACTOR * period); + rotation.z = to_angle(rotation.z * IMU_UDPS_FACTOR * period); + + plane_rotate(&devicey, &devicez, degrad(rotation.x)); + plane_rotate(&devicez, &devicex, degrad(rotation.y)); + plane_rotate(&devicex, &devicey, degrad(rotation.z)); + } + + if (imu_read_vector(IMU_ACCEL_ADDRESS, IMU_ACCEL_OUT_X_L_A | IMU_ACCEL_READ_MANY, + &acceleration, convert12)) + { + acceleration.x -= acceleration0.x; + acceleration.y -= acceleration0.y; + acceleration.z -= acceleration0.z; + + /* Convert to g. */ + + acceleration.x /= acceleration1g; + acceleration.y /= acceleration1g; + acceleration.z /= acceleration1g; + + /* Detect gravitational acceleration. */ + + accelerationM = vectorf_mag(&acceleration); + set_reference = (accelerationM > ACCEL_REST_MAG_LOWER) && (accelerationM < ACCEL_REST_MAG_UPPER); + + /* Obtain the acceleration in the global space. */ + + vectorf_convert(&acceleration, &devicex, &devicey, &devicez, &accelerationD); + } + + /* Obtain the view axes and device orientation. */ + + vectorf_negate(&devicey, &viewx); + vectorf_negate(&devicez, &viewy); + vectorf_negate(&devicex, &viewz); + + direction = vectorf_direction(&viewz); + elevation = vectorf_elevation(&viewz); + tilt = within(-(vectorf_tilt_in_plane(&viewy0, &viewx, &viewy) - M_PI / 2), M_PI); + + /* Reset or update the reference acceleration. */ + + if (set_reference) + { + accelerationRD = accelerationD; + } + else + { + vectorf_rotate_in_space(&accelerationRD, &viewz, &viewy, &viewx, degrad(rotation.y), &accelerationRD); + vectorf_rotate_in_space(&accelerationRD, &viewx, &viewy, &viewz, degrad(-rotation.x), &accelerationRD); + } + + /* Define the tilt and elevation of the reference acceleration. */ + + elevationA = vectorf_tilt_in_plane(&accelerationRD, &viewy0, &viewz); + tiltA = vectorf_tilt_in_plane_with_axis(&accelerationRD, &viewy0, &viewx, &viewz); + + /* Adjust according to elevation. */ + + if (set_reference && (fabs(elevation) < degrad(60))) + { + plane_rotate(&devicey, &devicez, -tiltA * ROTATION_ADJUSTMENT_FACTOR); + plane_rotate(&devicez, &devicex, -elevationA * ROTATION_ADJUSTMENT_FACTOR); + + vectorf_negate(&devicey, &viewx); + vectorf_negate(&devicez, &viewy); + vectorf_negate(&devicex, &viewz); + } + + /* Obtain the magnetic field in the global space. */ + + if (!vectorf_null(&field)) + { + directionF = vectorf_direction(&field); + elevationF = vectorf_elevation(&field); + + /* Define the global vector, remembering the initial value. */ + + vectorf_convert(&field, &devicex, &devicey, &devicez, &fieldD); + + if (!setF0) + { + fieldD0 = fieldD; + setF0 = true; + } + } + + /* Determine the initial field vector in the current device space. */ + + if (setF0) + { + vectorf_convert_into(&fieldD0, &devicex, &devicey, &devicez, &field0); + directionF0 = vectorf_direction(&field0); + elevationF0 = vectorf_elevation(&field0); + + /* Determine the east and north vectors using static field information. */ + + vectorf_cross(&accelerationRD, &fieldD0, &fieldE); + vectorf_normalise(&fieldE, &fieldE); + vectorf_cross(&fieldE, &accelerationRD, &fieldN); + } + + /* Subtract the constant background acceleration. */ + + if (!using_filter) + accelerationD.y -= 1; + + pthread_mutex_unlock(&mutex); + } + + usleep(IMU_UPDATE_PERIOD); + } + + return NULL; +} diff -r 000000000000 -r 631f88781de8 measure.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/measure.h Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,108 @@ +/* + * Ben NanoNote communication with the Pololu MinIMU-9 with the L3G4200D 3-axis + * gyroscope and LSM303DLM accelerometer/magnetometer. + * + * http://www.pololu.com/catalog/product/1265 + * + * Copyright (C) 2013 Paul Boddie + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ + +#ifndef __MEASURE_H__ +#define __MEASURE_H__ + +#include "geo.h" + +/* Function definitions. */ + +void ui_calibrate(bool using_filter, int (*print)(const char *, ...), void (*flush)()); +void *get_measurements(void *arg); + +#ifdef __MEASURE_H_PRIVATE__ + +/* The raw accelerometer value of 1g. */ + +const double acceleration1g = 1000000.0 / IMU_UG_FACTOR; + +/* The device vectors defined in the chosen coordinate system, together with + the corresponding initial view axes. */ + +const vectorf devicex0 = {{0, 0, -1}}, viewz0 = {{0, 0, 1}}, + devicey0 = {{-1, 0, 0}}, viewx0 = {{1, 0, 0}}, + devicez0 = {{0, -1, 0}}, viewy0 = {{0, 1, 0}}; + +/* Measurements. */ + +vectorf accelerationB[IMU_ACCEL_BUFFER_SIZE], + acceleration, + acceleration0 = {{0, 0, 0}}, + rotation, + rotation0 = {{0, 0, 0}}, + field, + fieldmin = {{-327.0, -355.0, -338.0}}, + fieldmax = {{107.0, 150.0, 205.0}}; + +/* Timekeeping. */ + +struct timeval imu_updated, imu_magnet_updated; +uint32_t imu_period = 0; + +/* Orientation. */ + +vectorf devicex, devicey, devicez, + viewx, viewy, viewz, + accelerationD, + accelerationR = {{0, 0, -1}}, + accelerationRD = {{0, 1, 0}}, + fieldD = {{0, 0, 0}}, + fieldD0 = {{0, 0, 0}}, + field0 = {{0, 0, 0}}, + fieldN = {{0, 0, 0}}, + fieldE = {{0, 0, 0}}; +double direction, elevation, tilt, + elevationA = 0, tiltA = 0, + directionF, elevationF, + directionF0, elevationF0; + +/* Measurement thread. */ + +pthread_t thread; +pthread_mutex_t mutex; + +#else + +extern const double acceleration1g; +extern vectorf devicex0, devicey0, devicez0, + devicex, devicey, devicez, + viewx0, viewy0, viewz0, + viewx, viewy, viewz, + accelerationB[IMU_ACCEL_BUFFER_SIZE], + acceleration, + acceleration0, + accelerationD, + accelerationR, + accelerationRD, + rotation, + rotation0, + field, + fieldmin, fieldmax, + fieldD, fieldD0, field0, fieldN, fieldE; +extern struct timeval imu_updated; +extern uint32_t imu_period; +extern double direction, elevation, tilt, + elevationA, tiltA, + directionF, elevationF, + directionF0, elevationF0; + +/* Measurement thread. */ + +extern pthread_t thread; +extern pthread_mutex_t mutex; + +#endif /* __MEASURE_H_PRIVATE__ */ + +#endif /* __MEASURE_H__ */ diff -r 000000000000 -r 631f88781de8 shutdown.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/shutdown.c Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,24 @@ +/* + * Common shutdown functionality. + * + * Copyright (C) 2013 Paul Boddie + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ + +#include +#include +#include + +/** + * Handle termination of the process. + */ +void init_shutdown(int signum) +{ + printf("Closing...\n"); + ubb_close(0); + exit(1); +} diff -r 000000000000 -r 631f88781de8 shutdown.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/shutdown.h Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,17 @@ +/* + * Common shutdown functionality. + * + * Copyright (C) 2013 Paul Boddie + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ + +#ifndef __SHUTDOWN_H__ +#define __SHUTDOWN_H__ + +void init_shutdown(int signum); + +#endif /* __SHUTDOWN_H__ */ diff -r 000000000000 -r 631f88781de8 ui.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ui.c Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,55 @@ +/* + * Common user interface functionality. + * + * Copyright (C) 2013 Paul Boddie + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ + +#include +#include +#include +#include +#include "ui.h" +#include "shutdown.h" + +void text_shutdown(int signum) +{ + init_shutdown(signum); +} + +void text_shutdown_threaded(int signum) +{ + extern pthread_t thread; + void *threadresult; + + if (pthread_cancel(thread) == 0) + pthread_join(thread, &threadresult); + + text_shutdown(signum); +} + +/* Textual interface functions. */ + +imu_ui_op text_handle_events() +{ + return IMU_UI_OP_NULL; +} + +void text_flush() +{ + fflush(stdout); +} + +void text_clear() +{ +} + +void text_quit() +{ + printf("Closing...\n"); + ubb_close(0); +} diff -r 000000000000 -r 631f88781de8 ui.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ui.h Mon Oct 14 13:02:19 2013 +0000 @@ -0,0 +1,34 @@ +/* + * Common user interface functionality. + * + * Copyright (C) 2013 Paul Boddie + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + */ + +#ifndef __UI_H__ +#define __UI_H__ + +/* Common types. */ + +typedef enum +{ + IMU_UI_OP_NULL, IMU_UI_OP_CALIBRATE, IMU_UI_OP_RESET, IMU_UI_OP_PAUSE, + IMU_UI_OP_QUIT +} imu_ui_op; + +void init_shutdown(int signum); +void text_shutdown(int signum); +void text_shutdown_threaded(int signum); + +/* Textual interface functions. */ + +imu_ui_op text_handle_events(); +void text_flush(); +void text_clear(); +void text_quit(); + +#endif /* __UI_H__ */