├── .dir-locals.el ├── .gitignore ├── DESIGN_GOALS.md ├── Doxyfile ├── LICENSE.txt ├── Makefile ├── README.md ├── exceptions.h ├── i2c_bus.cpp ├── i2c_bus.h ├── imu.h ├── l3g.cpp ├── l3g.h ├── lis3mdl.cpp ├── lis3mdl.h ├── lsm303.cpp ├── lsm303.h ├── lsm6.cpp ├── lsm6.h ├── minimu9-ahrs-calibrate ├── minimu9-ahrs-calibrate.1 ├── minimu9-ahrs-calibrator ├── minimu9-ahrs-calibrator.1 ├── minimu9-ahrs.1 ├── minimu9-ahrs.cpp ├── minimu9.cpp ├── minimu9.h ├── pacer.h ├── prog_options.cpp ├── prog_options.h ├── sensor_set.h ├── vector.h └── version.h /.dir-locals.el: -------------------------------------------------------------------------------- 1 | ; This file causes the Emacs text editor to follow our indentation style. 2 | 3 | ( 4 | (c-mode . ((c-basic-offset . 2) (tab-width . 2) (indent-tabs-mode . nil) (c-file-style . "linux")) ) 5 | (c++-mode . ((c-basic-offset . 2) (tab-width . 2) (indent-tabs-mode . nil) (c-file-style . "linux")) ) 6 | (nil . ((fill-column . 80))) 7 | ) 8 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /minimu9-ahrs 2 | /minimu9-rpi-ahrs 3 | *.d 4 | *.o 5 | *~ 6 | \#*\# 7 | *.gch 8 | *.config 9 | *.creator* 10 | *.files 11 | *.includes 12 | *.autosave 13 | /docs 14 | -------------------------------------------------------------------------------- /DESIGN_GOALS.md: -------------------------------------------------------------------------------- 1 | # Design goals 2 | 3 | When I made `minimu9-ahrs`, I had several goals in mind. 4 | 5 | First of all, this AHRS isn't just for model planes. I wanted it to be 6 | generally useful, so I wanted to avoid making any assumptions about what 7 | orientation the IMU is in. Therefore, I wanted to use an algorithm that treats 8 | all three axes (X, Y, and Z) equally. I looked at existing algorithms that 9 | stored the current state as a Direction Cosine Matrix (DCM) and found that when 10 | they normalized the DCM, they usually treated one axis specially. For an 11 | example of this problem, see the algorithm presented in the "Renormalization" 12 | section of [Direction Cosine Matrix IMU Theory][1] by William Premerlani and 13 | Paul Bizard. I decided to use a quaternion instead of a DCM to store the 14 | internal state because normalizing a quaternion is straightforward and treats 15 | all axes equally. 16 | 17 | Secondly, I wanted to have "gyro only" and "compass only" modes as a tool for 18 | teaching people about how IMUs work and also to help troubleshoot systems where 19 | one of those components might be acting incorrectly. Therefore, I designed the 20 | program to have different sensor fusion algorithms in it and you can easily pick 21 | one at runtime with a command-line parameter. 22 | 23 | Third, I wanted it to be easy to integrate the output of the program into a 24 | larger system. That's why I send the output as numbers to the standard output 25 | pipe and provide several different output formats. 26 | 27 | And finally, I wanted the code to have all the usual attributes that good code 28 | has. Short functions, small classes, and avoiding lots of magic numbers and 29 | global variables are some of the things I had in mind. 30 | 31 | [1]: http://gentlenav.googlecode.com/files/DCMDraft2.pdf -------------------------------------------------------------------------------- /Doxyfile: -------------------------------------------------------------------------------- 1 | PROJECT_NAME = "minimu9-rpi-ahrs" 2 | 3 | INPUT_ENCODING = UTF-8 4 | INPUT = . 5 | FILE_PATTERNS = *.cpp *.h 6 | EXCLUDE = Eigen 7 | RECURSIVE = NO 8 | 9 | OUTPUT_DIRECTORY = docs 10 | GENERATE_HTML = YES 11 | HTML_OUTPUT = html 12 | 13 | #BRIEF_MEMBER_DESC = YES 14 | #REPEAT_BRIEF = YES 15 | #ABBREVIATE_BRIEF = "The $name class" \ 16 | # "The $name widget" \ 17 | # "The $name file" \ 18 | # is \ 19 | # provides \ 20 | # specifies \ 21 | # contains \ 22 | # represents \ 23 | # a \ 24 | # an \ 25 | # the 26 | 27 | #FULL_PATH_NAMES = YES 28 | #SHORT_NAMES = NO 29 | 30 | # Discourage people from putting tabs in their code. 31 | TAB_SIZE = 16 32 | 33 | # Document as much stuff as possible 34 | EXTRACT_ALL = YES 35 | EXTRACT_PRIVATE = YES 36 | EXTRACT_STATIC = YES 37 | EXTRACT_LOCAL_CLASSES = YES 38 | EXTRACT_ANON_NSPACES = YES 39 | 40 | # HIDE_SCOPE_NAMES = YES 41 | 42 | GENERATE_TODOLIST = YES 43 | 44 | WARNINGS = YES 45 | WARN_IF_UNDOCUMENTED = YES 46 | WARN_IF_DOC_ERROR = YES 47 | 48 | SOURCE_BROWSER = YES 49 | 50 | REFERENCED_BY_RELATION = YES 51 | REFERENCES_RELATION = YES 52 | REFERENCES_LINK_SOURCE = NO 53 | 54 | 55 | HTML_COLORSTYLE_HUE = 340 56 | HTML_COLORSTYLE_SAT = 100 57 | HTML_TIMESTAMP = YES 58 | 59 | GENERATE_LATEX = NO -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2009-2017 Pololu Corporation. For more information, see 2 | 3 | http://www.pololu.com/ 4 | http://forum.pololu.com/ 5 | 6 | Permission is hereby granted, free of charge, to any person 7 | obtaining a copy of this software and associated documentation 8 | files (the "Software"), to deal in the Software without 9 | restriction, including without limitation the rights to use, 10 | copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the 12 | Software is furnished to do so, subject to the following 13 | conditions: 14 | 15 | The above copyright notice and this permission notice shall be 16 | included in all copies or substantial portions of the Software. 17 | 18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 19 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 20 | OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 21 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 22 | HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 23 | WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 24 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 25 | OTHER DEALINGS IN THE SOFTWARE. 26 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | OBJs := $(patsubst %.cpp, %.o, $(wildcard *.cpp)) 2 | BIN := minimu9-ahrs 3 | 4 | CC := g++ 5 | 6 | CPPFLAGS += -I. -Wall -Wextra -std=gnu++11 -O2 7 | 8 | # Use the Eigen library. 9 | EIGEN_NAME = eigen3 10 | CPPFLAGS += $(shell pkg-config --cflags $(EIGEN_NAME)) 11 | 12 | # Use boost libraries 13 | LDLIBS += -lboost_program_options 14 | 15 | # Put debugging info in there so we can get stack traces. 16 | #CPPFLAGS += -g -rdynamic -O0 17 | 18 | # Generate .d files with dependency info 19 | CPPFLAGS += -MD -MP 20 | 21 | all: vector.h.gch $(BIN) 22 | 23 | $(BIN) : $(OBJs) 24 | 25 | DEPs := $(OBJs:%.o=%.d) 26 | 27 | vector.h.gch: vector.h 28 | $(CC) $(CFLAGS) $(CPPFLAGS) $< -o $@ 29 | 30 | .PHONY: clean 31 | clean: 32 | @rm -fv $(BIN) $(OBJs) $(DEPs) *.o *.gch *.d 33 | @rm -fr docs 34 | 35 | .PHONY: docs 36 | docs: 37 | doxygen 38 | 39 | prefix = $(DESTDIR)/usr 40 | bindir = $(prefix)/bin 41 | sharedir = $(prefix)/share 42 | mandir = $(sharedir)/man 43 | man1dir = $(mandir)/man1 44 | 45 | .PHONY: install 46 | install: $(BIN) 47 | install $(BIN) $(bindir) 48 | install $(BIN)-calibrate $(bindir) 49 | install $(BIN)-calibrator $(bindir) 50 | install -m 0644 $(BIN).1 $(man1dir) 51 | install -m 0644 $(BIN)-calibrate.1 $(man1dir) 52 | install -m 0644 $(BIN)-calibrator.1 $(man1dir) 53 | 54 | -include $(DEPs) vector.h.d 55 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # minimu9-ahrs 2 | 3 | **minimu9-ahrs** is a program for reading sensor data from the Pololu MinIMU-9 4 | and similar boards over I²C. It supports the following sensor chips: 5 | 6 | * LSM6DS33 accelerometer and gyro 7 | * LSM6DSO accelerometer and gyro 8 | * LIS3MDL magnetometer 9 | * LSM303D magnetometer and accelerometer 10 | * LSM303DLHC magnetometer and accelerometer 11 | * LSM303DLM magnetometer and accelerometer 12 | * LSM303DLH magnetometer and accelerometer 13 | * L3GD20H gyro 14 | * L3GD20 gyro 15 | * L3G4200D gyro 16 | 17 | This program works with any board that has a magnetometer, acceleromter, and a 18 | gyro from the list above. This includes the following Pololu products: 19 | 20 | * MinIMU-9 [v0][mv0], [v1][mv1], [v2][mv2], [v3][mv3], [v5][mv5], [v6][mv6] 21 | * AltIMU-10 [v3][av3], [v4][av4], [v5][av5], [v6][av6] 22 | * [Balboa 32U4 Balancing Robot Kit][balboa] 23 | 24 | The program can output the raw sensor data from the magnetometor, accelerometer, 25 | and gyro or it can use that raw data to compute the orientation of the IMU. 26 | This program was designed and tested on the [Raspberry Pi], but it will probably 27 | work on any embedded Linux board that supports I²C. 28 | 29 | ## Getting started 30 | 31 | ### Enabling I²C 32 | 33 | First, you need to make sure your system supports I²C. Try typing 34 | `ls /dev/i2c*`: if you don't see a device there named something like 35 | `/dev/i2c-1` then your I²C is not enabled properly. 36 | 37 | On a **Raspberry Pi running Raspbian**, you should run `sudo raspi-config` 38 | and browse its menus to find the option to enable I²C. 39 | 40 | Here are some other related resources that might be useful for you when 41 | figuring out how to enable I²C: 42 | 43 | * [Configuring Your Pi for I²C by Adafruit](https://learn.adafruit.com/adafruit-16-channel-servo-driver-with-raspberry-pi/configuring-your-pi-for-i2c/) 44 | * [Guide to interfacing a Gyro and Accelerometer with a Raspberry Pi](https://ozzmaker.com/berryimu/) 45 | 46 | ### Managing device permissions 47 | 48 | After enabling the I²C devices, you should set them up so that your user has 49 | permission to access them. That way, you won't have to run `sudo`. 50 | 51 | First, run `groups` to see what groups your user belongs to. If `i2c` is on the 52 | list, then that is good. If you are not on the `i2c` group, then you should add 53 | yourself to it by running `sudo usermod -a -G i2c $(whoami)`, logging out, and 54 | then logging in again. If your system does not have an `i2c` group, you can 55 | create one or use a different group like `plugdev`. 56 | 57 | Next, run `ls -l /dev/i2c*` to make sure that your I²C devices have their group 58 | set to `i2c`. The group name is shown in the fourth column, and will usually be 59 | `root` or `i2c`. If the devices are not in the `i2c` group, then you should fix 60 | that by making a file called `/etc/udev.d/rules.d/i2c.rules` with the following 61 | line in it: 62 | 63 | SUBSYSTEM=="i2c-dev" GROUP="i2c" 64 | 65 | After making this file, you can make it take effect by running 66 | `sudo udevadm trigger` (or rebooting). 67 | 68 | If you get an error about permission being denied, double check that you have done these steps correctly. 69 | 70 | ### Soldering 71 | 72 | The MinIMU-9 and AltIMU-9 boards come with male header pins, and you will need 73 | to solder these into the board in order to make a solid connection. 74 | 75 | ### Wiring 76 | 77 | You will need to power your IMU board and connect it to the I²C bus of your 78 | embedded computer board. The correct connections for the Raspberry Pi and a 79 | MinIMU-9 are listed below: 80 | 81 | | Raspberry Pi pin | MinIMU-9 pin | 82 | |------------------|--------------| 83 | | GND | GND | 84 | | 3V3 Power | VDD | 85 | | GPIO 2 (SDA) | SDA | 86 | | GPIO 3 (SCL) | SCL | 87 | 88 | Below is a [picture][wiring_pic] with a MinIMU-9 v2 showing how to make those 89 | connections. 90 | 91 | [![][wiring_pic_small]][wiring_pic] 92 | 93 | You will need four female-female jumper wires designed for 0.1"-spaced pins. 94 | Pololu's [Female-Female Premium Jumper Wires][ffwires] work well. 95 | 96 | ### Determining which bus to use 97 | 98 | The default I²C bus used by this program is `/dev/i2c-1`. 99 | On most Raspberry Pi boards, this bus is accessible on the GPIO connector. 100 | 101 | If you want to use a different bus, you should make a configuration file 102 | in your home directory named 103 | `~/.minimu9-ahrs` with a single line of the form `i2c-bus=BUSNAME`, where 104 | `BUSNAME` is the full path to the bus you want to use. 105 | 106 | For example, to use `/dev/i2c-0` by default, the config file should read: 107 | 108 | i2c-bus=/dev/i2c-0 109 | 110 | If you are not sure which bus to use, you could try running `i2cdetect` on each 111 | available bus as described below. 112 | 113 | ### Checking your setup 114 | 115 | After you have enabled I²C, given yourself the proper permissions, soldered the 116 | MinIMU-9, and connected it to the Raspberry Pi, you should check your setup by 117 | running `i2cdetect`. Try running `i2cdetect -y N`, where `N` is the number of 118 | the I²C bus you want to use (typically 1 or 0). The output should look similar 119 | to this: 120 | 121 | ``` 122 | $ i2cdetect -y 1 123 | 0 1 2 3 4 5 6 7 8 9 a b c d e f 124 | 00: -- -- -- -- -- -- -- -- -- -- -- -- -- 125 | 10: -- -- -- -- -- -- -- -- -- -- -- -- -- 1d -- -- 126 | 20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 127 | 30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 128 | 40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 129 | 50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 130 | 60: -- -- -- -- -- -- -- -- -- -- -- 6b -- -- -- -- 131 | 70: -- -- -- -- -- -- -- -- 132 | ``` 133 | 134 | The exact output will depend on the type of IMU 135 | you are using, but the important thing is that the body of the printed table 136 | should contain a few hex numbers representing the addresses of I²C devices that 137 | were detected on the bus. 138 | 139 | If the `i2cdetect` command is not recognized, you should install the `i2c-tools` 140 | package. On Raspbian, you can run `sudo apt-get install i2c-tools` to install 141 | it. 142 | 143 | If you do not see a few hex numbers in the body of the table, then make sure 144 | your soldering and wiring are correct and try selecting a different bus by 145 | changing the bus number argument `N`. 146 | 147 | If you get a permission denied error, make sure you have configured the device 148 | permissions properly as described above. 149 | 150 | If you get a "No such file or directory" error referring to your I²C device, 151 | make sure that you have properly enabled I²C as described above. 152 | 153 | ### Building from source 154 | 155 | To build `minimu9-ahrs`, you first need to install some libraries that it 156 | depends on. On Raspbian, you can install these dependencies by running: 157 | 158 | sudo apt-get install libi2c-dev libeigen3-dev libboost-program-options-dev 159 | 160 | Then, to build `minimu9-ahrs`, navigate to the top-level directory of the source 161 | code and then run this command: 162 | 163 | make 164 | 165 | Finally, to install minimu9-ahrs onto your system, run: 166 | 167 | sudo make install 168 | 169 | ### Looking at raw values 170 | 171 | As a first test, you should look at the raw readings from the sensors on your 172 | IMU to make sure it is OK. Run `minimu9-ahrs --mode raw`. The output should 173 | look something like this: 174 | 175 | ``` 176 | -1318 -3106 -1801 1896 1219 3679 5 18 3 177 | -1318 -3106 -1801 1898 1200 3681 0 24 -1 178 | -1318 -3106 -1801 1899 1200 3688 15 17 2 179 | -1309 -3105 -1799 1874 1201 3671 17 20 -1 180 | -1309 -3105 -1799 1898 1214 3663 11 15 -2 181 | ``` 182 | 183 | **Yes, there will be noise in all the readings, even if your IMU is not moving 184 | at all.** That is totally normal. 185 | 186 | This output consists of three vectors. From left to right they are the raw 187 | magnetometer reading, the raw accelerometer reading, and the raw gyro reading. 188 | Each vector consists of three integers, in X-Y-Z order. You should turn the 189 | device and make sure that the raw readings change correspondingly. For example, 190 | when the X axis of the board is pointing straight up, the accelerometer's X 191 | reading (the 4th number on each line) should be positive and the other two 192 | components of the acceleration should be close to zero. 193 | 194 | ### Calibrating 195 | 196 | The magnetometer will need to be calibrated to create a mapping from the 197 | ellipsoid shape of the raw readings to the unit sphere shape that we want the 198 | scaled readings to have. The calibration feature for the `minimu9-ahrs` assumes 199 | that the shape of the raw readings will be an ellipsoid that is offset from the 200 | origin and stretched along the X, Y, and Z axes. It cannot handle a rotated 201 | ellipsoid. It can be informative to run `minimu9-ahrs --mode raw > output.tsv` 202 | while moving the magnetometer and then make some scatter plots of the raw 203 | magnetometer readings in a spreadsheet program to see what shape the readings 204 | have. 205 | 206 | To calibrate the magnetometer, run `minimu9-ahrs-calibrate` and follow the 207 | on-screen instructions when they tell you to start rotating the IMU through as 208 | many different orientations as possible. Once the script has collected enough 209 | data, it saves the data to `~/.minimu9-ahrs-cal-data` and then runs a separate 210 | Python script (`minimu9-ahrs-calibrator`) to create a calibration. 211 | 212 | The Python script previously had a complicated algorithm powered by SciPy that 213 | would take about 20 minutes to run and was not reliable. Currently, the script 214 | just uses a very simple algorithm that finds the minimum and maximum values of 215 | each axis of the magnetometer and uses those as the calibration values. This is 216 | probably not the best way to calibrate your magnetometer; there are more 217 | advanced ways that might work better. 218 | 219 | The `minimu9-ahrs-calibrate` script saves the calibration file to 220 | `~/.minimu9-ahrs-cal`. The calibration file is simply a one-line file with 6 221 | numbers separated by spaces: minimum x, maximum x, minimum y, maximum y, minimum 222 | z, maximum z. These numbers specify the linear mapping from the raw ellipsoid 223 | to the unit sphere. For example, if "minimum x" is -414, it means that a 224 | magnetometer reading of -414 on the X axis will get mapped to -1.0 when the 225 | readings are scaled. 226 | 227 | ### Looking at Euler angles 228 | 229 | Run `minimu9-ahrs --output euler`. It will print a stream of floating-point 230 | numbers, nine per line. The first three numbers are the yaw, pitch, and roll 231 | angles of the board in degrees. All three Euler angles should be close zero 232 | when the board is oriented with the Z axis facing down and the X axis facing 233 | towards magnetic north. From that starting point: 234 | 235 | * A positive `yaw` corresponds to a rotation about the Z axis that is 236 | clockwise when viewed from above. 237 | * A positive `pitch` correspond to a rotation about the Y axis that would 238 | cause the X axis to aim higher into the sky. 239 | * A positive `roll` would correspond to a counter-clockwise rotation about 240 | the X axis. 241 | 242 | The way you should think about it is that board starts in the neutral position, 243 | then the yaw rotation is applied, then the pitch rotation is applied, and then 244 | the roll rotation is applied to get the board to its final position. 245 | 246 | Look at the Euler angle output as you turn the board and make sure that it looks 247 | good. 248 | 249 | 250 | ## Man page 251 | 252 | For more information about `minimu9-ahrs`, including all the options it supports 253 | and a precise description of its output format, view the man page by running 254 | `man minimu9-ahrs`. 255 | 256 | 257 | ## Related projects 258 | 259 | * [ahrs-visualizer] - another program by me that provides a 3D display of 260 | the orientation output from `minimu9-ahrs`. 261 | * [Pololu MinIMU-9 Step-by-Step][kim] - an alternative tutorial by Mike Kim 262 | that explains how to use minimu9-ahrs and ahrs-visualizer. 263 | * [RTIMULib2](https://github.com/RTIMULib/RTIMULib2) - another project 264 | that works with the MinIMU-9 and the Raspberry Pi. 265 | 266 | ## Version history 267 | 268 | - 4.0.0 (2023-07-11): 269 | - Added support for the MinIMU-9 v6 (LSM6DSO and LIS3MDL). 270 | - Changed the default I²C bus to /dev/i2c-1 to better serve Raspberry Pi users. 271 | - Updated the minimu9-ahrs-calibrator script to use Python 3. 272 | - 3.0.0 (2017-04-15): 273 | - Added support for the MinIMU-9 v5 (LSM6DS33 and LIS3MDL). 274 | - Added support for a configuration file at `~/.minimu9-ahrs`. 275 | - Fixed a bug that resulted in corrections from the accelerometer always being applied 276 | even if the acceleration magnitude was not close to 1 g (thanks nxdefiant). 277 | - Use a Linux timerfd for more accurate timing (thanks to nxdefiant for the idea). 278 | - Made the `minimu9-ahrs-calibrate` script store the raw data in `~/.minimu9-ahrs-cal-data`. 279 | - Changed the minimu9-ahrs-calibrator Python script to just do simple minima/maxima 280 | because the fancy optimization algorithm was not reliable. 281 | - Fixed the way boost_program_options is linked. 282 | - Expanded the README so it can replace the wiki. 283 | - 2.0.0 (2014-07-08): 284 | - Added support for the MinIMU-9 v3 (LSM303D and L3GD20H) 285 | - Removed the right-shifting of raw accelerometer outputs; the raw readings are now 286 | signed 16-bit numbers that can range from -32768 to 32767. Previously the readings 287 | were signed 12-bit numbers, so this new version effectively gives readings that 288 | are greater by a factor of 16. 289 | - Changed the minimu9-ahrs-calibrator Python script to use SciPy instead of the old optimization algorithm. 290 | - Changed the minimu9-ahrs-calibrator script to print a warning if there are fewer than 300 input vectors instead of exiting. 291 | - Changed the minimu9-ahrs-calibrator script to print a warning if the calibration looks wrong. 292 | - 1.1.1 (2012-10-17) 293 | - 1.1.0 (2012-10-15) 294 | - 1.0.0 (2012-10-06) 295 | 296 | [ahrs-visualizer]: https://github.com/DavidEGrayson/ahrs-visualizer 297 | [minimu9-ahrs source code]: https://github.com/DavidEGrayson/minimu9-ahrs 298 | [Raspberry Pi]: https://www.raspberrypi.org 299 | [kim]: http://www.nacionale.com/pololu-minimu9-step-by-step/ 300 | [av3]: https://www.pololu.com/product/2469 301 | [av4]: https://www.pololu.com/product/2470 302 | [av5]: https://www.pololu.com/product/2739 303 | [av6]: https://www.pololu.com/product/2863 304 | [mv0]: https://www.pololu.com/product/1264 305 | [mv1]: https://www.pololu.com/product/1265 306 | [mv2]: https://www.pololu.com/product/1268 307 | [mv3]: https://www.pololu.com/product/2468 308 | [mv5]: https://www.pololu.com/product/2738 309 | [mv6]: https://www.pololu.com/product/2862 310 | [balboa]: https://www.pololu.com/product/3575 311 | [ffwires]: https://www.pololu.com/catalog/category/66 312 | [wiring_pic]: http://www.davidegrayson.com/minimu9-ahrs/wiring.jpg 313 | [wiring_pic_small]: http://www.davidegrayson.com/minimu9-ahrs/wiring_560px.jpg 314 | [i2cdetect_sample]: https://gist.github.com/DavidEGrayson/2f0531a149d964574565 315 | -------------------------------------------------------------------------------- /exceptions.h: -------------------------------------------------------------------------------- 1 | #ifndef _AHRS_EXCEPTIONS_H 2 | #define _AHRS_EXCEPTIONS_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | static inline std::system_error posix_error() 9 | { 10 | return std::system_error(errno, std::system_category()); 11 | } 12 | 13 | static inline std::system_error posix_error(const char * what) 14 | { 15 | return std::system_error(errno, std::system_category(), what); 16 | } 17 | 18 | static inline std::system_error posix_error(const std::string & what) 19 | { 20 | return std::system_error(errno, std::system_category(), what); 21 | } 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /i2c_bus.cpp: -------------------------------------------------------------------------------- 1 | //#define DEBUG_I2C 2 | 3 | #include "i2c_bus.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | static inline std::system_error posix_error(const std::string & what) 14 | { 15 | return std::system_error(errno, std::system_category(), what); 16 | } 17 | 18 | i2c_bus::i2c_bus() : fd(-1) 19 | { 20 | } 21 | 22 | i2c_bus::i2c_bus(const std::string & name) : fd(-1) 23 | { 24 | open(name); 25 | } 26 | 27 | i2c_bus::i2c_bus(const i2c_bus & other) : fd(-1) 28 | { 29 | *this = other; 30 | } 31 | 32 | i2c_bus & i2c_bus::operator=(const i2c_bus & other) 33 | { 34 | if (other.fd == -1) 35 | { 36 | close(); 37 | } 38 | else 39 | { 40 | open_from_fd(other.fd); 41 | } 42 | return *this; 43 | } 44 | 45 | i2c_bus::~i2c_bus() 46 | { 47 | close(); 48 | } 49 | 50 | void i2c_bus::open(const std::string & name) 51 | { 52 | close(); 53 | fd = ::open(name.c_str(), O_RDWR); 54 | if (fd == -1) 55 | { 56 | throw posix_error(std::string("Failed to open I2C device ") + name); 57 | } 58 | } 59 | 60 | void i2c_bus::open_from_fd(int other_fd) 61 | { 62 | close(); 63 | fd = dup(other_fd); 64 | if (fd == -1) 65 | { 66 | throw posix_error("Failed to dup I2C device"); 67 | } 68 | } 69 | 70 | void i2c_bus::close() 71 | { 72 | if (fd != -1) 73 | { 74 | ::close(fd); 75 | fd = -1; 76 | } 77 | } 78 | 79 | void i2c_bus::write_byte_and_read(uint8_t address, uint8_t command, 80 | uint8_t * data, size_t size) 81 | { 82 | i2c_msg messages[2] = { 83 | { address, 0, 1, (typeof(i2c_msg().buf)) &command }, 84 | { address, I2C_M_RD, (typeof(i2c_msg().len)) size, (typeof(i2c_msg().buf)) data }, 85 | }; 86 | i2c_rdwr_ioctl_data ioctl_data = { messages, 2 }; 87 | 88 | int result = ioctl(fd, I2C_RDWR, &ioctl_data); 89 | 90 | if (result != 2) 91 | { 92 | throw posix_error("Failed to read from I2C"); 93 | } 94 | } 95 | 96 | void i2c_bus::write(uint8_t address, const uint8_t * data, size_t size) 97 | { 98 | i2c_msg messages[1] = { 99 | { address, 0, (typeof(i2c_msg().len)) size, (typeof(i2c_msg().buf)) data } 100 | }; 101 | i2c_rdwr_ioctl_data ioctl_data = { messages, 1 }; 102 | 103 | int result = ioctl(fd, I2C_RDWR, &ioctl_data); 104 | 105 | if (result != 1) 106 | { 107 | throw posix_error("Failed to write to I2C"); 108 | } 109 | } 110 | 111 | int i2c_bus::try_write_byte_and_read(uint8_t address, uint8_t byte, 112 | uint8_t * data, size_t size) 113 | { 114 | i2c_msg messages[2] = { 115 | { address, 0, 1, (typeof(i2c_msg().buf))&byte }, 116 | { address, I2C_M_RD, (typeof(i2c_msg().len))size, (typeof(i2c_msg().buf))data }, 117 | }; 118 | i2c_rdwr_ioctl_data ioctl_data = { messages, 2 }; 119 | 120 | int result = ioctl(fd, I2C_RDWR, &ioctl_data); 121 | 122 | #ifdef DEBUG_I2C 123 | fprintf(stderr, "I2C write 0x%02x 0x%02x and read %u bytes: result %d,", 124 | address, byte, size, result); 125 | if (result == 2) 126 | { 127 | for (size_t i = 0; i < size; i++) 128 | { 129 | fprintf(stderr, " 0x%02x", data[i]); 130 | } 131 | } 132 | fprintf(stderr, "\n"); 133 | #endif 134 | 135 | if (result != 2) 136 | { 137 | return -1; 138 | } 139 | 140 | return 0; 141 | } 142 | -------------------------------------------------------------------------------- /i2c_bus.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class i2c_bus 8 | { 9 | public: 10 | explicit i2c_bus(const std::string & name); 11 | i2c_bus(); 12 | 13 | // Move constructor 14 | i2c_bus(i2c_bus &&); 15 | 16 | // Copy constructor 17 | i2c_bus(const i2c_bus &); 18 | 19 | // Move assignment operator 20 | i2c_bus & operator=(i2c_bus &&); 21 | 22 | // Copy assignment operator 23 | i2c_bus & operator=(const i2c_bus &); 24 | 25 | ~i2c_bus(); 26 | 27 | void open(const std::string & name); 28 | void open_from_fd(int other_fd); 29 | void close(); 30 | 31 | void write_byte_and_read(uint8_t address, uint8_t byte, 32 | uint8_t * data, size_t size); 33 | 34 | void write(uint8_t device_address, const uint8_t * data, size_t size); 35 | 36 | int try_write_byte_and_read(uint8_t address, uint8_t byte, 37 | uint8_t * data, size_t size); 38 | 39 | void write_two_bytes(uint8_t address, uint8_t byte1, uint8_t byte2) 40 | { 41 | uint8_t buffer[] = { byte1, byte2 }; 42 | write(address, buffer, 2); 43 | } 44 | 45 | uint8_t write_byte_and_read_byte(uint8_t address, uint8_t byte1) 46 | { 47 | uint8_t byte2; 48 | write_byte_and_read(address, byte1, &byte2, 1); 49 | return byte2; 50 | } 51 | 52 | int try_write_byte_and_read_byte(uint8_t address, uint8_t byte1) noexcept 53 | { 54 | uint8_t byte2; 55 | int result = try_write_byte_and_read(address, byte1, &byte2, 1); 56 | if (result < 0) { return result; } 57 | return byte2; 58 | } 59 | 60 | private: 61 | int fd; 62 | }; 63 | -------------------------------------------------------------------------------- /imu.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "vector.h" 4 | 5 | class imu 6 | { 7 | public: 8 | virtual void read_raw() 9 | { 10 | read_mag_raw(); 11 | read_acc_raw(); 12 | read_gyro_raw(); 13 | } 14 | 15 | virtual void read_acc_raw() = 0; 16 | virtual void read_mag_raw() = 0; 17 | virtual void read_gyro_raw() = 0; 18 | 19 | virtual float get_acc_scale() const = 0; 20 | virtual float get_gyro_scale() const = 0; 21 | 22 | int32_t m[3]; 23 | int32_t a[3]; 24 | int32_t g[3]; 25 | 26 | // TODO: remove stuff below this point 27 | 28 | // Scaled readings 29 | virtual vector read_mag() = 0; // In body coords, scaled to -1..1 range 30 | virtual vector read_acc() = 0; // In body coords, with units = g 31 | virtual vector read_gyro() = 0; // In body coords, with units = rad/sec 32 | void read(){ read_mag(); read_acc(); read_gyro(); } 33 | 34 | virtual void measure_offsets() = 0; 35 | virtual void enable() = 0; 36 | virtual void load_calibration() = 0; 37 | 38 | vector gyro_offset; 39 | int_vector mag_min, mag_max; 40 | }; 41 | -------------------------------------------------------------------------------- /l3g.cpp: -------------------------------------------------------------------------------- 1 | #include "l3g.h" 2 | #include 3 | 4 | void l3g::handle::open(const comm_config & config) 5 | { 6 | if (!config.use_sensor) 7 | { 8 | throw std::runtime_error("L3G configuration is null."); 9 | } 10 | 11 | this->config = config; 12 | i2c.open(config.i2c_bus_name); 13 | } 14 | 15 | // Turns on the gyro and places it in normal mode. 16 | void l3g::handle::enable() 17 | { 18 | write_reg(CTRL_REG1, 0b00001111); // Normal power mode, all axes enabled 19 | write_reg(CTRL_REG4, 0b00100000); // 2000 dps full scale 20 | } 21 | 22 | void l3g::handle::write_reg(uint8_t reg, uint8_t value) 23 | { 24 | i2c.write_two_bytes(config.i2c_address, reg, value); 25 | } 26 | 27 | uint8_t l3g::handle::read_reg(uint8_t reg) 28 | { 29 | return i2c.write_byte_and_read_byte(config.i2c_address, reg); 30 | } 31 | 32 | void l3g::handle::read() 33 | { 34 | uint8_t block[6]; 35 | i2c.write_byte_and_read(config.i2c_address, 36 | 0x80 | OUT_X_L, block, sizeof(block)); 37 | 38 | g[0] = (int16_t)(block[1] << 8 | block[0]); 39 | g[1] = (int16_t)(block[3] << 8 | block[2]); 40 | g[2] = (int16_t)(block[5] << 8 | block[4]); 41 | } 42 | -------------------------------------------------------------------------------- /l3g.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "i2c_bus.h" 4 | 5 | namespace l3g 6 | { 7 | enum device_type 8 | { 9 | L3G4200D = 0xD3, 10 | L3GD20 = 0xD4, 11 | L3GD20H = 0xD7, 12 | }; 13 | 14 | enum i2c_addr 15 | { 16 | L3GD20_SA0_LOW_ADDR = 0x6B, 17 | L3GD20_SA0_HIGH_ADDR = 0x6A, 18 | L3G4200D_SA0_LOW_ADDR = 0x68, 19 | L3G4200D_SA0_HIGH_ADDR = 0x69, 20 | }; 21 | 22 | // For now, we use the old register names instead of the new ones that the 23 | // L3GD20H has. 24 | enum reg_addr 25 | { 26 | WHO_AM_I = 0x0F, 27 | CTRL_REG1 = 0x20, 28 | CTRL_REG2 = 0x21, 29 | CTRL_REG3 = 0x22, 30 | CTRL_REG4 = 0x23, 31 | CTRL_REG5 = 0x24, 32 | REFERENCE = 0x25, 33 | OUT_TEMP = 0x26, 34 | STATUS_REG = 0x27, 35 | OUT_X_L = 0x28, 36 | OUT_X_H = 0x29, 37 | OUT_Y_L = 0x2A, 38 | OUT_Y_H = 0x2B, 39 | OUT_Z_L = 0x2C, 40 | OUT_Z_H = 0x2D, 41 | FIFO_CTRL_REG = 0x2E, 42 | FIFO_SRC_REG = 0x2F, 43 | INT1_CFG = 0x30, 44 | INT1_SRC = 0x31, 45 | INT1_THS_XH = 0x32, 46 | INT1_THS_XL = 0x33, 47 | INT1_THS_YH = 0x34, 48 | INT1_THS_YL = 0x35, 49 | INT1_THS_ZH = 0x36, 50 | INT1_THS_ZL = 0x37, 51 | INT1_DURATION = 0x38, 52 | LOW_ODR = 0x39, 53 | }; 54 | 55 | struct comm_config { 56 | bool use_sensor = false; 57 | device_type device; 58 | std::string i2c_bus_name; 59 | i2c_addr i2c_address; 60 | }; 61 | 62 | class handle 63 | { 64 | public: 65 | void open(const comm_config &); 66 | 67 | // gyro angular velocity readings 68 | int32_t g[3]; 69 | 70 | void enable(); 71 | 72 | void write_reg(uint8_t reg, uint8_t value); 73 | uint8_t read_reg(uint8_t reg); 74 | void read(); 75 | 76 | protected: 77 | i2c_bus i2c; 78 | comm_config config; 79 | }; 80 | }; 81 | -------------------------------------------------------------------------------- /lis3mdl.cpp: -------------------------------------------------------------------------------- 1 | #include "lis3mdl.h" 2 | #include 3 | 4 | void lis3mdl::handle::open(const comm_config & config) 5 | { 6 | if (!config.use_sensor) 7 | { 8 | throw std::runtime_error("LIS3MDL configuration is null."); 9 | } 10 | 11 | this->config = config; 12 | i2c.open(config.i2c_bus_name); 13 | } 14 | 15 | void lis3mdl::handle::write_reg(reg_addr addr, uint8_t value) 16 | { 17 | i2c.write_two_bytes(config.i2c_address, addr, value); 18 | } 19 | 20 | void lis3mdl::handle::enable() 21 | { 22 | if (config.device == LIS3MDL) 23 | { 24 | // OM = 11 (ultra-high-performance mode for X and Y); DO = 100 (10 Hz ODR) 25 | write_reg(CTRL_REG1, 0b01110000); 26 | 27 | // FS = 00 (+/- 4 gauss full scale) 28 | write_reg(CTRL_REG2, 0b00000000); 29 | 30 | // MD = 00 (continuous-conversion mode) 31 | write_reg(CTRL_REG3, 0b00000000); 32 | 33 | // OMZ = 11 (ultra-high-performance mode for Z) 34 | write_reg(CTRL_REG4, 0b00001100); 35 | } 36 | else 37 | { 38 | std::runtime_error("Cannot enable unknown LIS3MDL device."); 39 | } 40 | } 41 | 42 | void lis3mdl::handle::read() 43 | { 44 | uint8_t block[6]; 45 | i2c.write_byte_and_read(config.i2c_address, 46 | 0x80 | OUT_X_L, block, sizeof(block)); 47 | m[0] = (int16_t)(block[0] | block[1] << 8); 48 | m[1] = (int16_t)(block[2] | block[3] << 8); 49 | m[2] = (int16_t)(block[4] | block[5] << 8); 50 | } 51 | -------------------------------------------------------------------------------- /lis3mdl.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace lis3mdl 7 | { 8 | enum device_type 9 | { 10 | LIS3MDL = 0x3D, 11 | }; 12 | 13 | enum i2c_addr 14 | { 15 | SA1_LOW_ADDR = 0x1C, 16 | SA1_HIGH_ADDR = 0x1E, 17 | }; 18 | 19 | enum reg_addr 20 | { 21 | WHO_AM_I = 0x0F, 22 | 23 | CTRL_REG1 = 0x20, 24 | CTRL_REG2 = 0x21, 25 | CTRL_REG3 = 0x22, 26 | CTRL_REG4 = 0x23, 27 | CTRL_REG5 = 0x24, 28 | 29 | STATUS_REG = 0x27, 30 | OUT_X_L = 0x28, 31 | OUT_X_H = 0x29, 32 | OUT_Y_L = 0x2A, 33 | OUT_Y_H = 0x2B, 34 | OUT_Z_L = 0x2C, 35 | OUT_Z_H = 0x2D, 36 | TEMP_OUT_L = 0x2E, 37 | TEMP_OUT_H = 0x2F, 38 | INT_CFG = 0x30, 39 | INT_SRC = 0x31, 40 | INT_THS_L = 0x32, 41 | INT_THS_H = 0x33, 42 | }; 43 | 44 | struct comm_config { 45 | bool use_sensor = false; 46 | device_type device; 47 | std::string i2c_bus_name; 48 | i2c_addr i2c_address; 49 | }; 50 | 51 | class handle 52 | { 53 | public: 54 | void open(const comm_config &); 55 | 56 | void enable(); 57 | 58 | void write_reg(reg_addr addr, uint8_t value); 59 | 60 | void read(); 61 | 62 | int32_t m[3]; 63 | 64 | protected: 65 | i2c_bus i2c; 66 | comm_config config; 67 | }; 68 | }; 69 | -------------------------------------------------------------------------------- /lsm303.cpp: -------------------------------------------------------------------------------- 1 | #include "lsm303.h" 2 | #include 3 | 4 | // Relevant Pololu products: 5 | // 6 | // #1250 LSM303DLH SA0_A pulled to GND, accessible via. 7 | // #1264 LSM303DLH + L3G4200D (v0) SA0_A pulled to GND, accessible thru-hole. 8 | // #1265 LSM303DLM + L3G4200D (v1) SA0_A pulled to GND, accessible thru-hole. 9 | // #1268 LSM303DLHC + L3GD20 (v2) (chip has no SA0 line) 10 | // #1273 LSM303DLM SA0_A pulled to GND, accessible via. 11 | // #2124 LSM303DLHC (chip has no SA0 line) 12 | // #2127 LSM303D SA0 pulled to VDD, accessible thru-hole. 13 | // #2468 LSM303D + L3GD20H (v3) SA0 pulled to VDD, accessible thru-hole 14 | 15 | void lsm303::handle::open(const comm_config & config) 16 | { 17 | if (!config.use_sensor) 18 | { 19 | throw std::runtime_error("LSM303 configuration is null."); 20 | } 21 | 22 | this->config = config; 23 | i2c.open(config.i2c_bus_name); 24 | } 25 | 26 | uint8_t lsm303::handle::read_mag_reg(uint8_t reg) 27 | { 28 | return i2c.write_byte_and_read_byte(config.i2c_address_mag, reg); 29 | } 30 | 31 | uint8_t lsm303::handle::read_acc_reg(uint8_t reg) 32 | { 33 | return i2c.write_byte_and_read_byte(config.i2c_address_acc, reg); 34 | } 35 | 36 | void lsm303::handle::write_mag_reg(uint8_t reg, uint8_t value) 37 | { 38 | i2c.write_two_bytes(config.i2c_address_mag, reg, value); 39 | } 40 | 41 | void lsm303::handle::write_acc_reg(uint8_t reg, uint8_t value) 42 | { 43 | i2c.write_two_bytes(config.i2c_address_acc, reg, value); 44 | } 45 | 46 | // Turns on the LSM303's accelerometer and magnetometers and places them in normal 47 | // mode. 48 | void lsm303::handle::enable() 49 | { 50 | if (config.device == LSM303D) 51 | { 52 | //// LSM303D Accelerometer 53 | 54 | // AODR = 0101 (50 Hz ODR) 55 | // AZEN = AYEN = AXEN = 1 (all axes enabled) 56 | write_acc_reg(CTRL1, 0b01010111); 57 | 58 | // AFS = 011 (8 g full scale) 59 | write_acc_reg(CTRL2, 0b00011000); 60 | 61 | //// LSM303D Magnetometer 62 | // M_RES = 11 (high resolution mode) 63 | // M_ODR = 001 (6.25 Hz ODR) 64 | write_mag_reg(CTRL5, 0b01100100); 65 | 66 | // MFS = 01 (4 gauss full scale) 67 | write_mag_reg(CTRL6, 0b00100000); 68 | 69 | // MLP = 0 (low power mode off) 70 | // MD = 00 (continuous-conversion mode) 71 | write_mag_reg(CTRL7, 0b00000000); 72 | } 73 | else if (config.device == LSM303DLHC) 74 | { 75 | //// LSM303DLHC Accelerometer 76 | 77 | // ODR = 0100 (50 Hz ODR) 78 | // LPen = 0 (normal mode) 79 | // Zen = Yen = Xen = 1 (all axes enabled) 80 | write_acc_reg(CTRL_REG1_A, 0b01000111); 81 | 82 | // FS = 10 (8 g full scale) 83 | // HR = 1 (high resolution enable) 84 | write_acc_reg(CTRL_REG4_A, 0b00101000); 85 | 86 | //// LSM303DLHC Magnetometer 87 | 88 | // DO = 011 (7.5 Hz ODR) 89 | write_mag_reg(CRA_REG_M, 0b00001100); 90 | 91 | // GN = 001 (+/- 1.3 gauss full scale) 92 | write_mag_reg(CRB_REG_M, 0b00100000); 93 | 94 | // MD = 00 (continuous-conversion mode) 95 | write_mag_reg(MR_REG_M, 0b00000000); 96 | } 97 | else 98 | { 99 | //// LSM303DLM or LSM303DLH Accelerometer 100 | 101 | // FS = 11 (8 g full scale) 102 | write_acc_reg(CTRL_REG4_A, 0b00110000); 103 | 104 | // PM = 001 (normal mode) 105 | // DR = 00 (50 Hz ODR) 106 | // Zen = Yen = Xen = 1 (all axes enabled) 107 | write_acc_reg(CTRL_REG1_A, 0b00100111); 108 | 109 | //// LSM303DLM or LSM303DLH Magnetometer 110 | 111 | // DO = 011 (7.5 Hz ODR) 112 | write_mag_reg(CRA_REG_M, 0b00001100); 113 | 114 | // GN = 001 (+/- 1.3 gauss full scale) 115 | write_mag_reg(CRB_REG_M, 0b00100000); 116 | 117 | // MD = 00 (continuous-conversion mode) 118 | write_mag_reg(MR_REG_M, 0b00000000); 119 | } 120 | } 121 | 122 | void lsm303::handle::read_acc() 123 | { 124 | uint8_t block[6]; 125 | i2c.write_byte_and_read(config.i2c_address_acc, 126 | 0x80 | OUT_X_L_A, block, sizeof(block)); 127 | a[0] = (int16_t)(block[0] | block[1] << 8); 128 | a[1] = (int16_t)(block[2] | block[3] << 8); 129 | a[2] = (int16_t)(block[4] | block[5] << 8); 130 | } 131 | 132 | void lsm303::handle::read_mag() 133 | { 134 | uint8_t block[6]; 135 | 136 | if (config.device == LSM303D) 137 | { 138 | // LSM303D: XYZ order, little endian 139 | i2c.write_byte_and_read(config.i2c_address_mag, 140 | 0x80 | D_OUT_X_L_M, block, sizeof(block)); 141 | m[0] = (int16_t)(block[0] | block[1] << 8); 142 | m[1] = (int16_t)(block[2] | block[3] << 8); 143 | m[2] = (int16_t)(block[4] | block[5] << 8); 144 | } 145 | else if (config.device == LSM303DLH) 146 | { 147 | // LSM303DLH: XYZ order, big endian 148 | i2c.write_byte_and_read(config.i2c_address_mag, 149 | 0x80 | DLH_OUT_X_H_M, block, sizeof(block)); 150 | m[0] = (int16_t)(block[1] | block[0] << 8); 151 | m[1] = (int16_t)(block[3] | block[2] << 8); 152 | m[2] = (int16_t)(block[5] | block[4] << 8); 153 | } 154 | else 155 | { 156 | // LSM303DLM, LSM303DLHC: XZY order, big endian (and same addresses) 157 | i2c.write_byte_and_read(config.i2c_address_mag, 158 | 0x80 | DLM_OUT_X_H_M, block, sizeof(block)); 159 | m[0] = (int16_t)(block[1] | block[0] << 8); 160 | m[1] = (int16_t)(block[5] | block[4] << 8); 161 | m[2] = (int16_t)(block[3] | block[2] << 8); 162 | } 163 | } 164 | 165 | void lsm303::handle::read() 166 | { 167 | read_acc(); 168 | read_mag(); 169 | } 170 | -------------------------------------------------------------------------------- /lsm303.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "i2c_bus.h" 5 | 6 | namespace lsm303 7 | { 8 | enum device_type 9 | { 10 | LSM303DLH = 1, // no WHO_AM_I register 11 | LSM303DLM = 0x3C, 12 | LSM303DLHC = 3, // undocumented WHO_AM_I register with 0x3C 13 | LSM303D = 0x49, 14 | }; 15 | 16 | enum i2c_addr 17 | { 18 | LSM303D_SA0_HIGH_ADDR = 0x1D, 19 | LSM303D_SA0_LOW_ADDR = 0x1E, 20 | LSM303_NON_D_MAG_ADDR = 0x1E, 21 | LSM303_NON_D_ACC_SA0_LOW_ADDR = 0x18, 22 | LSM303_NON_D_ACC_SA0_HIGH_ADDR = 0x19, 23 | }; 24 | 25 | enum reg_addr 26 | { 27 | TEMP_OUT_L = 0x05, // D 28 | TEMP_OUT_H = 0x06, // D 29 | 30 | STATUS_M = 0x07, // D 31 | 32 | INT_CTRL_M = 0x12, // D 33 | INT_SRC_M = 0x13, // D 34 | INT_THS_L_M = 0x14, // D 35 | INT_THS_H_M = 0x15, // D 36 | 37 | OFFSET_X_L_M = 0x16, // D 38 | OFFSET_X_H_M = 0x17, // D 39 | OFFSET_Y_L_M = 0x18, // D 40 | OFFSET_Y_H_M = 0x19, // D 41 | OFFSET_Z_L_M = 0x1A, // D 42 | OFFSET_Z_H_M = 0x1B, // D 43 | REFERENCE_X = 0x1C, // D 44 | REFERENCE_Y = 0x1D, // D 45 | REFERENCE_Z = 0x1E, // D 46 | 47 | CTRL0 = 0x1F, // D 48 | CTRL1 = 0x20, // D 49 | CTRL_REG1_A = 0x20, // DLH, DLM, DLHC 50 | CTRL2 = 0x21, // D 51 | CTRL_REG2_A = 0x21, // DLH, DLM, DLHC 52 | CTRL3 = 0x22, // D 53 | CTRL_REG3_A = 0x22, // DLH, DLM, DLHC 54 | CTRL4 = 0x23, // D 55 | CTRL_REG4_A = 0x23, // DLH, DLM, DLHC 56 | CTRL5 = 0x24, // D 57 | CTRL_REG5_A = 0x24, // DLH, DLM, DLHC 58 | CTRL6 = 0x25, // D 59 | CTRL_REG6_A = 0x25, // DLHC 60 | HP_FILTER_RESET_A = 0x25, // DLH, DLM 61 | CTRL7 = 0x26, // D 62 | REFERENCE_A = 0x26, // DLH, DLM, DLHC 63 | STATUS_A = 0x27, // D 64 | STATUS_REG_A = 0x27, // DLH, DLM, DLHC 65 | 66 | OUT_X_L_A = 0x28, 67 | OUT_X_H_A = 0x29, 68 | OUT_Y_L_A = 0x2A, 69 | OUT_Y_H_A = 0x2B, 70 | OUT_Z_L_A = 0x2C, 71 | OUT_Z_H_A = 0x2D, 72 | 73 | FIFO_CTRL = 0x2E, // D 74 | FIFO_CTRL_REG_A = 0x2E, // DLHC 75 | FIFO_SRC = 0x2F, // D 76 | FIFO_SRC_REG_A = 0x2F, // DLHC 77 | 78 | IG_CFG1 = 0x30, // D 79 | INT1_CFG_A = 0x30, // DLH, DLM, DLHC 80 | IG_SRC1 = 0x31, // D 81 | INT1_SRC_A = 0x31, // DLH, DLM, DLHC 82 | IG_THS1 = 0x32, // D 83 | INT1_THS_A = 0x32, // DLH, DLM, DLHC 84 | IG_DUR1 = 0x33, // D 85 | INT1_DURATION_A = 0x33, // DLH, DLM, DLHC 86 | IG_CFG2 = 0x34, // D 87 | INT2_CFG_A = 0x34, // DLH, DLM, DLHC 88 | IG_SRC2 = 0x35, // D 89 | INT2_SRC_A = 0x35, // DLH, DLM, DLHC 90 | IG_THS2 = 0x36, // D 91 | INT2_THS_A = 0x36, // DLH, DLM, DLHC 92 | IG_DUR2 = 0x37, // D 93 | INT2_DURATION_A = 0x37, // DLH, DLM, DLHC 94 | 95 | CLICK_CFG = 0x38, // D 96 | CLICK_CFG_A = 0x38, // DLHC 97 | CLICK_SRC = 0x39, // D 98 | CLICK_SRC_A = 0x39, // DLHC 99 | CLICK_THS = 0x3A, // D 100 | CLICK_THS_A = 0x3A, // DLHC 101 | TIME_LIMIT = 0x3B, // D 102 | TIME_LIMIT_A = 0x3B, // DLHC 103 | TIME_LATENCY = 0x3C, // D 104 | TIME_LATENCY_A = 0x3C, // DLHC 105 | TIME_WINDOW = 0x3D, // D 106 | TIME_WINDOW_A = 0x3D, // DLHC 107 | 108 | Act_THS = 0x3E, // D 109 | Act_DUR = 0x3F, // D 110 | 111 | CRA_REG_M = 0x00, // DLH, DLM, DLHC 112 | CRB_REG_M = 0x01, // DLH, DLM, DLHC 113 | MR_REG_M = 0x02, // DLH, DLM, DLHC 114 | 115 | SR_REG_M = 0x09, // DLH, DLM, DLHC 116 | IRA_REG_M = 0x0A, // DLH, DLM, DLHC 117 | IRB_REG_M = 0x0B, // DLH, DLM, DLHC 118 | IRC_REG_M = 0x0C, // DLH, DLM, DLHC 119 | 120 | WHO_AM_I = 0x0F, // D 121 | WHO_AM_I_M = 0x0F, // DLM 122 | 123 | TEMP_OUT_H_M = 0x31, // DLHC 124 | TEMP_OUT_L_M = 0x32, // DLHC 125 | 126 | // dummy addresses for registers in different locations on different devices; 127 | OUT_X_H_M = -1, 128 | OUT_X_L_M = -2, 129 | OUT_Y_H_M = -3, 130 | OUT_Y_L_M = -4, 131 | OUT_Z_H_M = -5, 132 | OUT_Z_L_M = -6, 133 | // update dummy_reg_count if registers are added here! 134 | 135 | // device-specific register addresses 136 | 137 | DLH_OUT_X_H_M = 0x03, 138 | DLH_OUT_X_L_M = 0x04, 139 | DLH_OUT_Y_H_M = 0x05, 140 | DLH_OUT_Y_L_M = 0x06, 141 | DLH_OUT_Z_H_M = 0x07, 142 | DLH_OUT_Z_L_M = 0x08, 143 | 144 | DLM_OUT_X_H_M = 0x03, 145 | DLM_OUT_X_L_M = 0x04, 146 | DLM_OUT_Z_H_M = 0x05, 147 | DLM_OUT_Z_L_M = 0x06, 148 | DLM_OUT_Y_H_M = 0x07, 149 | DLM_OUT_Y_L_M = 0x08, 150 | 151 | DLHC_OUT_X_H_M = 0x03, 152 | DLHC_OUT_X_L_M = 0x04, 153 | DLHC_OUT_Z_H_M = 0x05, 154 | DLHC_OUT_Z_L_M = 0x06, 155 | DLHC_OUT_Y_H_M = 0x07, 156 | DLHC_OUT_Y_L_M = 0x08, 157 | 158 | D_OUT_X_L_M = 0x08, 159 | D_OUT_X_H_M = 0x09, 160 | D_OUT_Y_L_M = 0x0A, 161 | D_OUT_Y_H_M = 0x0B, 162 | D_OUT_Z_L_M = 0x0C, 163 | D_OUT_Z_H_M = 0x0D, 164 | }; 165 | 166 | struct comm_config 167 | { 168 | bool use_sensor = false; 169 | device_type device; 170 | std::string i2c_bus_name; 171 | i2c_addr i2c_address_acc; 172 | i2c_addr i2c_address_mag; 173 | }; 174 | 175 | class handle 176 | { 177 | public: 178 | void open(const comm_config &); 179 | 180 | int32_t a[3]; // accelerometer readings 181 | int32_t m[3]; // magnetometer readings 182 | 183 | void enable(); 184 | 185 | void write_acc_reg(uint8_t reg, uint8_t value); 186 | uint8_t read_acc_reg(uint8_t reg); 187 | void write_mag_reg(uint8_t reg, uint8_t value); 188 | uint8_t read_mag_reg(uint8_t reg); 189 | 190 | void read_acc(); 191 | void read_mag(); 192 | void read(); 193 | 194 | protected: 195 | i2c_bus i2c; 196 | comm_config config; 197 | }; 198 | }; 199 | -------------------------------------------------------------------------------- /lsm6.cpp: -------------------------------------------------------------------------------- 1 | #include "lsm6.h" 2 | #include 3 | 4 | void lsm6::handle::open(const comm_config & config) 5 | { 6 | if (!config.use_sensor) 7 | { 8 | throw std::runtime_error("LSM6 configuration is null."); 9 | } 10 | 11 | this->config = config; 12 | i2c.open(config.i2c_bus_name); 13 | } 14 | 15 | void lsm6::handle::enable() 16 | { 17 | if (config.device == LSM6DS33 || config.device == LSM6DSO) 18 | { 19 | //// LSM6DS33/LSM6DSO gyro 20 | 21 | // ODR = 1000 (1.66 kHz (high performance)) 22 | // FS_G = 11 (2000 dps) 23 | write_reg(CTRL2_G, 0b10001100); 24 | 25 | // defaults 26 | write_reg(CTRL7_G, 0b00000000); 27 | 28 | //// LSM6DS33/LSM6DSO accelerometer 29 | 30 | // ODR = 1000 (1.66 kHz (high performance)) 31 | // FS_XL = 11 (8 g full scale) 32 | write_reg(CTRL1_XL, 0b10001100); 33 | 34 | //// common 35 | 36 | // IF_INC = 1 (automatically increment address register) 37 | write_reg(CTRL3_C, 0b00000100); 38 | } 39 | else 40 | { 41 | throw std::runtime_error("Cannot enable unknown LSM6 device."); 42 | } 43 | } 44 | 45 | void lsm6::handle::write_reg(reg_addr addr, uint8_t value) 46 | { 47 | i2c.write_two_bytes(config.i2c_address, addr, value); 48 | } 49 | 50 | void lsm6::handle::read_gyro() 51 | { 52 | uint8_t block[6]; 53 | i2c.write_byte_and_read(config.i2c_address, OUTX_L_G, block, sizeof(block)); 54 | g[0] = (int16_t)(block[0] | block[1] << 8); 55 | g[1] = (int16_t)(block[2] | block[3] << 8); 56 | g[2] = (int16_t)(block[4] | block[5] << 8); 57 | } 58 | 59 | void lsm6::handle::read_acc() 60 | { 61 | uint8_t block[6]; 62 | i2c.write_byte_and_read(config.i2c_address, OUTX_L_XL, block, sizeof(block)); 63 | a[0] = (int16_t)(block[0] | block[1] << 8); 64 | a[1] = (int16_t)(block[2] | block[3] << 8); 65 | a[2] = (int16_t)(block[4] | block[5] << 8); 66 | } 67 | -------------------------------------------------------------------------------- /lsm6.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace lsm6 7 | { 8 | enum device_type 9 | { 10 | LSM6DS33 = 0x69, 11 | LSM6DSO = 0x6C, 12 | }; 13 | 14 | enum i2c_addr 15 | { 16 | SA0_LOW_ADDR = 0x6A, 17 | SA0_HIGH_ADDR = 0x6B, 18 | }; 19 | 20 | enum reg_addr 21 | { 22 | INT1_CTRL = 0x0D, 23 | INT2_CTRL = 0x0E, 24 | WHO_AM_I = 0x0F, 25 | CTRL1_XL = 0x10, 26 | CTRL2_G = 0x11, 27 | CTRL3_C = 0x12, 28 | CTRL4_C = 0x13, 29 | CTRL5_C = 0x14, 30 | CTRL6_C = 0x15, 31 | CTRL7_G = 0x16, 32 | CTRL8_XL = 0x17, 33 | CTRL9_XL = 0x18, 34 | CTRL10_C = 0x19, 35 | 36 | STATUS_REG = 0x1E, 37 | 38 | OUT_TEMP_L = 0x20, 39 | OUT_TEMP_H = 0x21, 40 | OUTX_L_G = 0x22, 41 | OUTX_H_G = 0x23, 42 | OUTY_L_G = 0x24, 43 | OUTY_H_G = 0x25, 44 | OUTZ_L_G = 0x26, 45 | OUTZ_H_G = 0x27, 46 | OUTX_L_XL = 0x28, 47 | OUTX_H_XL = 0x29, 48 | OUTY_L_XL = 0x2A, 49 | OUTY_H_XL = 0x2B, 50 | OUTZ_L_XL = 0x2C, 51 | OUTZ_H_XL = 0x2D, 52 | 53 | FIFO_STATUS1 = 0x3A, 54 | FIFO_STATUS2 = 0x3B, 55 | }; 56 | 57 | struct comm_config { 58 | bool use_sensor = false; 59 | device_type device; 60 | std::string i2c_bus_name; 61 | i2c_addr i2c_address; 62 | }; 63 | 64 | class handle 65 | { 66 | public: 67 | void open(const comm_config &); 68 | 69 | void enable(); 70 | 71 | void write_reg(reg_addr addr, uint8_t value); 72 | 73 | void read_gyro(); 74 | void read_acc(); 75 | 76 | // gyro angular velocity readings 77 | int32_t g[3]; 78 | 79 | // acceleration readings 80 | int32_t a[3]; 81 | 82 | protected: 83 | i2c_bus i2c; 84 | comm_config config; 85 | }; 86 | }; 87 | -------------------------------------------------------------------------------- /minimu9-ahrs-calibrate: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | echo "To calibrate the magnetometer, start rotating the IMU through " 3 | echo "as many different orientations as possible." 4 | minimu9-ahrs --mode raw $@ | head -n2000 > ~/.minimu9-ahrs-cal-data 5 | echo "You can stop rotating the IMU now." 6 | minimu9-ahrs-calibrator < ~/.minimu9-ahrs-cal-data > ~/.minimu9-ahrs-cal 7 | -------------------------------------------------------------------------------- /minimu9-ahrs-calibrate.1: -------------------------------------------------------------------------------- 1 | .TH MINIMU9-AHRS-CALIBRATE 1 2 | .SH NAME 3 | minimu9-ahrs-calibrate - generate a magnetometer calibration file for the MinIMU-9 4 | .SH SYNOPSIS 5 | .B minimu9-ahrs-calibrate [OPTIONS] 6 | .SH DESCRIPTION 7 | This is a small shell script that takes a few thousand lines of 8 | the output from 9 | \fBminimu9-ahrs --mode raw\fP and passes them to \fBminimu9-calibrator\fP to 10 | create the calibration file needed by minimu9-ahrs, 11 | \fB~/.minimu9-ahrs-cal\fP. 12 | This script also stores the raw data in \fB~/.minimu9-ahrs-cal-data\fP so you can examine it or try different calibration methods. 13 | .P 14 | You should rotate the IMU through as many different angles as possible while this script is running. 15 | .SS OPTIONS 16 | Any command-line arguments provided to this script will be directly 17 | passed to \fBminimu9-ahrs\fP. 18 | .SH FILES 19 | .TP 20 | ~/.minimu9-ahrs-cal 21 | Calibration file for the magnetomer. 22 | .TP 23 | ~/.minimu9-ahrs-cal-data 24 | Raw data used to generate the calibration. 25 | .SH AUTHOR 26 | .nf 27 | David Grayson 28 | http://www.github.com/DavidEGrayson/ 29 | .fi 30 | .SH SEE ALSO 31 | minimu9-ahrs(1), minimu9-ahrs-calibrator(1) 32 | -------------------------------------------------------------------------------- /minimu9-ahrs-calibrator: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import sys 4 | import math 5 | 6 | verbose = False 7 | 8 | def average(list): 9 | return sum(list)/len(list) 10 | 11 | def percentile_to_value(list, *percentiles): 12 | list = sorted(list) 13 | return [list[int(p / 100.0 * (len(list)-1))] for p in percentiles] 14 | 15 | def variance(list): 16 | m = average(list) 17 | return sum([(i-m)**2 for i in list]) / float(len(list)) 18 | 19 | def std_deviation(list): 20 | return math.sqrt(variance(list)) 21 | 22 | class Vector(object): 23 | def __init__(self, x, y, z): 24 | self.x, self.y, self.z = x, y, z 25 | def __str__(self): 26 | return "(" + str(self.x) + ", " + str(self.y) + ", " + str(self.z) + ")" 27 | 28 | def __getitem__(self, key): 29 | if key == 0: 30 | return self.x 31 | elif key == 1: 32 | return self.y 33 | elif key == 2: 34 | return self.z 35 | else: 36 | raise Exception("Invalid key") 37 | 38 | def magnitude(self): 39 | return math.sqrt(self.x**2 + self.y**2 + self.z**2) 40 | 41 | Axes = range(3) 42 | 43 | class Calibration: 44 | def __init__(self, values, raw_readings=None): 45 | self.values = values 46 | self.raw_readings = raw_readings 47 | 48 | def scale(self, raw_reading): 49 | return Vector((raw_reading[0] - self.values[0])/float(self.values[1] - self.values[0]) * 2 - 1, 50 | (raw_reading[1] - self.values[2])/float(self.values[3] - self.values[2]) * 2 - 1, 51 | (raw_reading[2] - self.values[4])/float(self.values[5] - self.values[4]) * 2 - 1) 52 | 53 | def __str__(self): 54 | return "%d %d %d %d %d %d" % tuple(self.values) 55 | 56 | def info_string(self): 57 | return "%-32s avg=%7.4f stdev=%7.4f" % ( 58 | str(self), 59 | average(self.scaled_magnitudes()), 60 | std_deviation(self.scaled_magnitudes()) 61 | ) 62 | 63 | def scaled_magnitudes(self): 64 | return [s.magnitude() for s in self.scaled_readings()] 65 | 66 | def scaled_readings(self): 67 | return [self.scale(r) for r in self.raw_readings] 68 | 69 | 70 | def run(file=sys.stdin): 71 | try: 72 | raw_readings = read_vectors(file) 73 | 74 | if len(raw_readings) < 300: 75 | print("Warning: Only " + str(len(raw_readings)) + " readings were provided.", 76 | file=sys.stderr) 77 | 78 | cal = simple_minmax_calibration(raw_readings) 79 | 80 | if verbose: 81 | print("calibration info: " + cal.info_string(), file=sys.stderr) 82 | 83 | print(cal) 84 | 85 | except KeyboardInterrupt: 86 | print("", file=sys.stderr) # newline to tidy things up 87 | pass 88 | 89 | def read_vectors(file): 90 | vectors = [Vector(*[int(s) for s in line.split()[0:3]]) for line in file] 91 | return vectors 92 | 93 | def simple_minmax_calibration(readings): 94 | guess = [] 95 | for axis in Axes: 96 | values = [v[axis] for v in readings] 97 | guess.extend([min(values), max(values)]) 98 | return Calibration(guess, readings) 99 | 100 | if __name__=='__main__': 101 | run() 102 | -------------------------------------------------------------------------------- /minimu9-ahrs-calibrator.1: -------------------------------------------------------------------------------- 1 | .TH MINIMU9-AHRS-CALIBRATOR 1 2 | .SH NAME 3 | minimu9-ahrs-calibrator - read raw IMU data on standard 4 | input and output a linear magnetometer calibration file 5 | for use with minimu9-ahrs 6 | .SH SYNOPSIS 7 | .B minimu9-ahrs-calibrator < raw_readings > ~/.minimu9-ahrs-cal 8 | .SH DESCRIPTION 9 | 10 | This is a Python 2 script that takes raw IMU readings on its standard 11 | input and uses the readings to compute a linear calibation 12 | of the magnetometer. 13 | The IMU should be rotated through as many different orientations 14 | while generating the input. 15 | .P 16 | The input format consists of lines where each line has the format: 17 | .IP 18 | \fBMX MY MZ [EXTRA INPUT]\fP 19 | .P 20 | where \fBMX MY MZ\fP are integers that represent the raw reading from 21 | the magnetometor. 22 | Extra input on the line after the magnetomer readings is ignored. 23 | .P 24 | When the input stream ends, the script makes a simple linear calibration by 25 | finding the minimum and maximum values for each axis and outputting them on the 26 | standard output. 27 | .P 28 | The output format is: 29 | .IP 30 | \fBMIN_X MAX_X MIN_Y MAX_Y MIN_Z MAX_Z\fP 31 | .P 32 | The scaling formulae to convert raw readings to scaled readings are: 33 | .IP 34 | .nf 35 | scaled_x = (raw_x - min_x) / (max_x - min_x) * 2 - 1 36 | scaled_y = (raw_y - min_y) / (max_y - min_y) * 2 - 1 37 | scaled_z = (raw_z - min_z) / (max_z - min_z) * 2 - 1 38 | .fi 39 | .P 40 | Therefore, if the raw X value is equal to MIN_X 41 | (the first number in the calibration), then 42 | the scaled X value would be -1.0. 43 | .SH NOTES 44 | This script is very simple. There are more advanced ways to calibrate 45 | magnetometers that might give you better results. 46 | 47 | .SH AUTHOR 48 | .nf 49 | David Grayson 50 | http://www.github.com/DavidEGrayson/ 51 | .fi 52 | .SH SEE ALSO 53 | minimu9-ahrs(1), minimu9-ahrs-calibrate(1) 54 | -------------------------------------------------------------------------------- /minimu9-ahrs.1: -------------------------------------------------------------------------------- 1 | .TH MINIMU9-AHRS 1 2 | .SH NAME 3 | minimu9-ahrs - read data from the Pololu MinIMU-9 over I²C 4 | .SH SYNOPSIS 5 | .B minimu9-ahrs [\fIOPTIONS\fP] 6 | .SH DESCRIPTION 7 | minimu9-ahrs is a program for reading data from the Pololu MinIMU-9 over I²C. 8 | It supports MinIMU-9 versions v0, v1, v2, v3, v5, and v6. 9 | The program can output the raw sensor data from the magnetometer, accelerometer, 10 | and gyro or it can use that raw data to compute the orientation of the IMU. 11 | .P 12 | The program auto-detects what version of MinIMU-9 is connected. 13 | .SS OPTIONS 14 | .TP 15 | \fB-h, --help\fP 16 | produce help message 17 | .TP 18 | \fB-v, --version\fP 19 | print version number 20 | .TP 21 | \fB-b, --i2c-bus\fP 22 | I²C-bus the IMU is connected to (default: /dev/i2c-1) 23 | .TP 24 | \fB--mode \fIMODE\fR 25 | Specify what algorithm to use. 26 | .nf 27 | \fBnormal\fP (default): Fuse compass and gyro. 28 | \fBgyro-only\fP: Use only gyro (drifts). 29 | \fBcompass-only\fP: Use only compass (noisy). 30 | \fBraw\fP: Just print raw values from sensors. 31 | .fi 32 | .TP 33 | \fB--output \fIFORMAT\fR 34 | Specify how to output the orientation. 35 | Has no effect if \fB--mode raw\fP is specified. 36 | .nf 37 | \fBmatrix\fP (default): Direction Cosine Matrix. 38 | \fBquaternion\fP: Quaternion. 39 | \fBeuler\fP: Euler angles (yaw, pitch, roll). 40 | 41 | .SS COORDINATE SYSTEMS 42 | There are two coordinate systems in use. 43 | .P 44 | The \fBground coordinate system\fP uses the following axes, in this order: 45 | \fBNorth\fP, \fBEast\fP, and \fBDown\fP. 46 | .P 47 | The \fBbody coordinate system\fP represents the orientation of the IMU, and 48 | uses the following axes, as labeled on the IMU: 49 | \fBX\fP, \fBY\fP, and \fBZ\fP. 50 | 51 | .SS RAW OUTPUT FORMAT 52 | 53 | If \fB--mode raw\fP is specified, the format of the output will be 54 | nine integers: 55 | .IP 56 | \fBMX MY MZ AX AY AZ GX GY GZ\fP 57 | .P 58 | where \fBMX MY MZ\fP is the raw reading from the magnetometer, 59 | \fBAX AY AZ\fP is the raw reading from the accelerometer, and 60 | \fBGX GY GZ\fP is the raw reading from the gyro. 61 | 62 | .SS ORIENTATION OUTPUT FORMATS 63 | 64 | Unless \fB--mode raw\fP is specified, the output format will be of the form: 65 | .IP 66 | \fIORIENTATION\fB AX AY AZ BX BY BZ\fR 67 | .P 68 | where the format of \fIORIENTATION\fP is determined by the 69 | \fB--output\fP argument, 70 | \fBAX AY AZ\fP is the scaled acceleration vector in units of 1 g, and 71 | \fBBX BY BZ\fP is the scaled magnetic field vector, whose magnitude should 72 | normally be close to 1. 73 | 74 | .SS ORIENTATION AS A MATRIX 75 | 76 | The default output format is \fB--output matrix\fP. 77 | In this mode, a 3x3 transformation matrix 78 | (also known as the direction cosine matrix) is sent to the standard output. 79 | The matrix is sent in row-major order; the first three numbers on the line 80 | are row 0, etc. 81 | .P 82 | The matrix is defined as the matrix that transforms column vectors from 83 | the \fBbody coordinate system\fP to the \fBground coordinate system\fP. 84 | Accordingly, its nine elements can be specified as dot products of the 85 | unit vectors representing the two coordinate systems: 86 | .IP 87 | .nf 88 | north·x north·y north·z 89 | east·x east·y east·z 90 | down·x down·y down·z 91 | .fi 92 | 93 | .SS ORIENTATION AS A QUATERNION 94 | If \fB--output quaternion\fP is specified, the orientation is formatted 95 | as a unit quaternion (four numbers). It is a left quaternion that 96 | transforms vectors from the \fBbody coordinate system\fP to the 97 | \fBground coordinate system\fP. 98 | 99 | .SS ORIENTATION AS EULER ANGLES 100 | If \fB--output euler\fP is specified, the orintation is formatted as three 101 | Euler angles in the following order: yaw, pitch, roll. 102 | These are the standard angles used to represent the orientation of an airplane. 103 | The angles are output in degrees. 104 | .P 105 | The Euler angles represent the amount of rotation needed in three 106 | distinct steps to turn the ground coordinate system into the body 107 | coordinate system. 108 | .P 109 | Imagine you start with a coordinate system that is equal to ground 110 | coordinate system: north, east, down. 111 | .P 112 | Now rotate the coordinate system about its third axis 113 | (still pointing down) by a certain angle that we will call the \fByaw\fP. 114 | When viewed from above, the yaw rotation goes in the clockwise direction. 115 | .P 116 | Now rotate the coordinate system about its second axis by a certain 117 | angle that we will call the \fBpitch\fP. A positive pitch makes 118 | the first axis of the coordinate system go up. A pitch of 90.0 119 | would make the first axis point straight up, while a pitch of -90.0 120 | would make the first axis point straight down. 121 | .P 122 | Now rotate the coordinate system about is first axis by a certain 123 | angle that we will call the \fBroll\fP. 124 | .P 125 | The coordinate system you now have is equal to the 126 | \fBbody coordinate system\fP (x, y, z). 127 | 128 | .SH FILES 129 | .TP 130 | ~/.minimu9-ahrs-cal 131 | Calibration file for the magnetomer, needed unless \fB--mode raw\fP is specified. 132 | This file should be a one-line file with 6 integers separated by spaces: 133 | minimum x, maximum x, minimum y, maximum y, minimum z, maximum z. 134 | These numbers specify the linear mapping from the raw ellipsoid to 135 | the unit sphere. For example, if "minimum x" is -414, it means that a 136 | magnetometer reading of -414 on the X axis will get mapped to -1.0 when 137 | the readings are scaled. Run \fBminimu9-ahrs-calibrate\fP to generate this 138 | file. 139 | .TP 140 | ~/.minimu9-ahrs 141 | Configuration file that lets you set default values for some of the command-line 142 | options. Every line should have the format \fBKEY=VALUE\fP. Currently, the 143 | only supported option is i2c-bus. To set the default I²C bus to /dev/i2c-1, you 144 | would write: 145 | .RS 2 146 | .IP 147 | i2c-bus=/dev/i2c-1 148 | .SH AUTHOR 149 | .nf 150 | David Grayson 151 | http://www.github.com/DavidEGrayson/ 152 | .fi 153 | .SH SEE ALSO 154 | minimu9-ahrs-calibrate(1), minimu9-ahrs-calibrator(1) 155 | -------------------------------------------------------------------------------- /minimu9-ahrs.cpp: -------------------------------------------------------------------------------- 1 | // For efficiency in compiling, this is the only file that uses the 2 | // Eigen lirbary and the 'vector' type we made from it. 3 | 4 | #include "vector.h" 5 | #include "version.h" 6 | #include "prog_options.h" 7 | #include "minimu9.h" 8 | #include "exceptions.h" 9 | #include "pacer.h" 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | // TODO: print warning if accelerometer magnitude is not close to 1 when starting up 22 | 23 | // An Euler angle could take 8 chars: -234.678, but usually we only need 6. 24 | float field_width = 6; 25 | #define FLOAT_FORMAT std::fixed << std::setprecision(3) << std::setw(field_width) 26 | 27 | std::ostream & operator << (std::ostream & os, const vector & vector) 28 | { 29 | return os << FLOAT_FORMAT << vector(0) << ' ' 30 | << FLOAT_FORMAT << vector(1) << ' ' 31 | << FLOAT_FORMAT << vector(2); 32 | } 33 | 34 | std::ostream & operator << (std::ostream & os, const matrix & matrix) 35 | { 36 | return os << (vector)matrix.row(0) << ' ' 37 | << (vector)matrix.row(1) << ' ' 38 | << (vector)matrix.row(2); 39 | } 40 | 41 | std::ostream & operator << (std::ostream & os, const quaternion & quat) 42 | { 43 | return os << FLOAT_FORMAT << quat.w() << ' ' 44 | << FLOAT_FORMAT << quat.x() << ' ' 45 | << FLOAT_FORMAT << quat.y() << ' ' 46 | << FLOAT_FORMAT << quat.z(); 47 | } 48 | 49 | typedef void rotation_output_function(quaternion & rotation); 50 | 51 | void output_quaternion(quaternion & rotation) 52 | { 53 | std::cout << rotation; 54 | } 55 | 56 | void output_matrix(quaternion & rotation) 57 | { 58 | std::cout << rotation.toRotationMatrix(); 59 | } 60 | 61 | void output_euler(quaternion & rotation) 62 | { 63 | std::cout << (vector)(rotation.toRotationMatrix().eulerAngles(2, 1, 0) 64 | * (180 / M_PI)); 65 | } 66 | 67 | void stream_raw_values(imu & imu) 68 | { 69 | imu.enable(); 70 | while(1) 71 | { 72 | imu.read_raw(); 73 | printf("%7d %7d %7d %7d %7d %7d %7d %7d %7d\n", 74 | imu.m[0], imu.m[1], imu.m[2], 75 | imu.a[0], imu.a[1], imu.a[2], 76 | imu.g[0], imu.g[1], imu.g[2] 77 | ); 78 | usleep(20*1000); 79 | } 80 | } 81 | 82 | //! Uses the acceleration and magnetic field readings from the compass 83 | // to get a noisy estimate of the current rotation matrix. 84 | // This function is where we define the coordinate system we are using 85 | // for the ground coords: North, East, Down. 86 | matrix rotation_from_compass(const vector & acceleration, const vector & magnetic_field) 87 | { 88 | vector down = -acceleration; // usually true 89 | vector east = down.cross(magnetic_field); // actually it's magnetic east 90 | vector north = east.cross(down); 91 | 92 | east.normalize(); 93 | north.normalize(); 94 | down.normalize(); 95 | 96 | matrix r; 97 | r.row(0) = north; 98 | r.row(1) = east; 99 | r.row(2) = down; 100 | return r; 101 | } 102 | 103 | typedef void fuse_function(quaternion & rotation, float dt, const vector & angular_velocity, 104 | const vector & acceleration, const vector & magnetic_field); 105 | 106 | void fuse_compass_only(quaternion & rotation, float dt, const vector& angular_velocity, 107 | const vector & acceleration, const vector & magnetic_field) 108 | { 109 | (void)dt; 110 | (void)angular_velocity; 111 | (void)acceleration; 112 | (void)magnetic_field; 113 | // Implicit conversion of rotation matrix to quaternion. 114 | rotation = rotation_from_compass(acceleration, magnetic_field); 115 | } 116 | 117 | // Uses the given angular velocity and time interval to calculate 118 | // a rotation and applies that rotation to the given quaternion. 119 | // w is angular velocity in radians per second. 120 | // dt is the time. 121 | void rotate(quaternion & rotation, const vector & w, float dt) 122 | { 123 | // Multiply by first order approximation of the 124 | // quaternion representing this rotation. 125 | rotation *= quaternion(1, w(0)*dt/2, w(1)*dt/2, w(2)*dt/2); 126 | rotation.normalize(); 127 | } 128 | 129 | void fuse_gyro_only(quaternion & rotation, float dt, const vector & angular_velocity, 130 | const vector & acceleration, const vector & magnetic_field) 131 | { 132 | (void)acceleration; 133 | (void)magnetic_field; 134 | rotate(rotation, angular_velocity, dt); 135 | } 136 | 137 | void fuse_default(quaternion & rotation, float dt, const vector & angular_velocity, 138 | const vector & acceleration, const vector & magnetic_field) 139 | { 140 | vector correction = vector(0, 0, 0); 141 | 142 | if (fabs(acceleration.norm() - 1) <= 0.3) 143 | { 144 | // The magnetidude of acceleration is close to 1 g, so 145 | // it might be pointing up and we can do drift correction. 146 | 147 | const float correction_strength = 1; 148 | 149 | matrix rotation_compass = rotation_from_compass(acceleration, magnetic_field); 150 | matrix rotation_matrix = rotation.toRotationMatrix(); 151 | 152 | correction = ( 153 | rotation_compass.row(0).cross(rotation_matrix.row(0)) + 154 | rotation_compass.row(1).cross(rotation_matrix.row(1)) + 155 | rotation_compass.row(2).cross(rotation_matrix.row(2)) 156 | ) * correction_strength; 157 | 158 | } 159 | 160 | rotate(rotation, angular_velocity + correction, dt); 161 | } 162 | 163 | void ahrs(imu & imu, fuse_function * fuse, rotation_output_function * output) 164 | { 165 | imu.load_calibration(); 166 | imu.enable(); 167 | imu.measure_offsets(); 168 | 169 | // The quaternion that can convert a vector in body coordinates 170 | // to ground coordinates when it its changed to a matrix. 171 | quaternion rotation = quaternion::Identity(); 172 | 173 | // Set up a timer that expires every 20 ms. 174 | pacer loop_pacer; 175 | loop_pacer.set_period_ns(20000000); 176 | 177 | auto start = std::chrono::steady_clock::now(); 178 | while (1) 179 | { 180 | auto last_start = start; 181 | start = std::chrono::steady_clock::now(); 182 | std::chrono::nanoseconds duration = start - last_start; 183 | float dt = duration.count() / 1e9; 184 | if (dt < 0) { throw std::runtime_error("Time went backwards."); } 185 | 186 | vector angular_velocity = imu.read_gyro(); 187 | vector acceleration = imu.read_acc(); 188 | vector magnetic_field = imu.read_mag(); 189 | 190 | fuse(rotation, dt, angular_velocity, acceleration, magnetic_field); 191 | 192 | output(rotation); 193 | std::cout << " " << acceleration << " " << magnetic_field << std::endl; 194 | 195 | loop_pacer.pace(); 196 | } 197 | } 198 | 199 | int main_with_exceptions(int argc, char **argv) 200 | { 201 | prog_options options = get_prog_options(argc, argv); 202 | 203 | if (options.show_help) 204 | { 205 | print_command_line_options_desc(); 206 | std::cout << "For more information, run: man minimu9-ahrs" << std::endl; 207 | return 0; 208 | } 209 | 210 | if (options.show_version) 211 | { 212 | std::cout << VERSION << std::endl; 213 | return 0; 214 | } 215 | 216 | // Decide what sensors we want to use. 217 | sensor_set set; 218 | set.mag = set.acc = set.gyro = true; 219 | 220 | minimu9::comm_config config = minimu9::auto_detect(options.i2c_bus_name); 221 | 222 | sensor_set missing = set - minimu9::config_sensor_set(config); 223 | if (missing) 224 | { 225 | if (missing.mag) 226 | { 227 | std::cerr << "Error: No magnetometer found." << std::endl; 228 | } 229 | if (missing.acc) 230 | { 231 | std::cerr << "Error: No accelerometer found." << std::endl; 232 | } 233 | if (missing.gyro) 234 | { 235 | std::cerr << "Error: No gyro found." << std::endl; 236 | } 237 | std::cerr << "Error: Needed sensors are missing." << std::endl; 238 | return 1; 239 | } 240 | 241 | config = minimu9::disable_redundant_sensors(config, set); 242 | 243 | minimu9::handle imu; 244 | imu.open(config); 245 | 246 | rotation_output_function * output; 247 | 248 | // Figure out the output mode. 249 | if (options.output_mode == "matrix") 250 | { 251 | output = &output_matrix; 252 | } 253 | else if (options.output_mode == "quaternion") 254 | { 255 | output = &output_quaternion; 256 | } 257 | else if (options.output_mode == "euler") 258 | { 259 | field_width += 2; // See comment above for field_width. 260 | output = &output_euler; 261 | } 262 | else 263 | { 264 | std::cerr << "Unknown output mode '" << options.output_mode << "'" << std::endl; 265 | return 1; 266 | } 267 | 268 | // Figure out the basic operating mode and start running. 269 | if (options.mode == "raw") 270 | { 271 | stream_raw_values(imu); 272 | } 273 | else if (options.mode == "gyro-only") 274 | { 275 | ahrs(imu, &fuse_gyro_only, output); 276 | } 277 | else if (options.mode == "compass-only") 278 | { 279 | ahrs(imu, &fuse_compass_only, output); 280 | } 281 | else if (options.mode == "normal") 282 | { 283 | ahrs(imu, &fuse_default, output); 284 | } 285 | else 286 | { 287 | std::cerr << "Unknown mode '" << options.mode << "'" << std::endl; 288 | return 1; 289 | } 290 | return 0; 291 | } 292 | 293 | int main(int argc, char ** argv) 294 | { 295 | try 296 | { 297 | main_with_exceptions(argc, argv); 298 | } 299 | catch(const std::system_error & error) 300 | { 301 | std::string what = error.what(); 302 | const std::error_code & code = error.code(); 303 | std::cerr << "Error: " << what << " (" << code << ")" << std::endl; 304 | return 2; 305 | } 306 | catch(const std::exception & error) 307 | { 308 | std::cerr << "Error: " << error.what() << std::endl; 309 | return 9; 310 | } 311 | } 312 | -------------------------------------------------------------------------------- /minimu9.cpp: -------------------------------------------------------------------------------- 1 | #include "vector.h" 2 | #include "minimu9.h" 3 | #include "exceptions.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | minimu9::comm_config minimu9::auto_detect(const std::string & i2c_bus_name) 11 | { 12 | i2c_bus bus(i2c_bus_name.c_str()); 13 | minimu9::comm_config config; 14 | 15 | // Detect LSM6 devices. 16 | { 17 | auto addrs = { lsm6::SA0_LOW_ADDR, lsm6::SA0_HIGH_ADDR }; 18 | for (uint8_t addr : addrs) 19 | { 20 | int result = bus.try_write_byte_and_read_byte(addr, lsm6::WHO_AM_I); 21 | if (result == lsm6::LSM6DS33 || result == lsm6::LSM6DSO) 22 | { 23 | config.lsm6.use_sensor = true; 24 | config.lsm6.device = (lsm6::device_type)result; 25 | config.lsm6.i2c_bus_name = i2c_bus_name; 26 | config.lsm6.i2c_address = (lsm6::i2c_addr)addr; 27 | break; 28 | } 29 | } 30 | } 31 | 32 | // Detect LIS3MDL devices. 33 | { 34 | auto addrs = { lis3mdl::SA1_LOW_ADDR, lis3mdl::SA1_HIGH_ADDR }; 35 | for (uint8_t addr : addrs) 36 | { 37 | int result = bus.try_write_byte_and_read_byte(addr, lis3mdl::WHO_AM_I); 38 | if (result == lis3mdl::LIS3MDL) 39 | { 40 | config.lis3mdl.use_sensor = true; 41 | config.lis3mdl.device = (lis3mdl::device_type)result; 42 | config.lis3mdl.i2c_bus_name = i2c_bus_name; 43 | config.lis3mdl.i2c_address = (lis3mdl::i2c_addr)addr; 44 | break; 45 | } 46 | } 47 | } 48 | 49 | // Detect L3G devices. 50 | { 51 | auto addrs = { 52 | l3g::L3GD20_SA0_LOW_ADDR, 53 | l3g::L3GD20_SA0_HIGH_ADDR, 54 | l3g::L3G4200D_SA0_LOW_ADDR, 55 | l3g::L3G4200D_SA0_HIGH_ADDR, 56 | }; 57 | for (uint8_t addr : addrs) 58 | { 59 | int result = bus.try_write_byte_and_read_byte(addr, l3g::WHO_AM_I); 60 | if (result == l3g::L3G4200D || result == l3g::L3GD20 61 | || result == l3g::L3GD20H) 62 | { 63 | config.l3g.use_sensor = true; 64 | config.l3g.device = (l3g::device_type)result; 65 | config.l3g.i2c_bus_name = i2c_bus_name; 66 | config.l3g.i2c_address = (l3g::i2c_addr)addr; 67 | break; 68 | } 69 | } 70 | } 71 | 72 | // Detect LSM303 devices. 73 | { 74 | auto & c = config.lsm303; 75 | if (lsm303::LSM303D == bus.try_write_byte_and_read_byte( 76 | lsm303::LSM303D_SA0_HIGH_ADDR, lsm303::WHO_AM_I)) 77 | { 78 | c.use_sensor = true; 79 | c.device = lsm303::LSM303D; 80 | c.i2c_bus_name = i2c_bus_name; 81 | c.i2c_address_acc = c.i2c_address_mag = 82 | lsm303::LSM303D_SA0_HIGH_ADDR; 83 | } 84 | else if (lsm303::LSM303D == bus.try_write_byte_and_read_byte( 85 | lsm303::LSM303D_SA0_LOW_ADDR, lsm303::WHO_AM_I)) 86 | { 87 | c.use_sensor = true; 88 | c.device = lsm303::LSM303D; 89 | c.i2c_bus_name = i2c_bus_name; 90 | c.i2c_address_acc = c.i2c_address_mag = 91 | lsm303::LSM303D_SA0_LOW_ADDR; 92 | } 93 | // Remaining possibilities: LSM303DLHC, LSM303DLM, or LSM303DLH. 94 | // 95 | // LSM303DLHC seems to respond to WHO_AM_I request the same way as DLM, even 96 | // though this register isn't documented in its datasheet, and LSM303DLH does 97 | // not have a WHO_AM_I. 98 | // 99 | // Lets assume that if it's a LSM303DLM and LSM303DLH then SA0 is low, 100 | // because that is how the Pololu boards pull that pin and this lets us 101 | // distinguish between the LSM303DLHC and those other chips. 102 | else if (bus.try_write_byte_and_read_byte( 103 | lsm303::LSM303_NON_D_ACC_SA0_HIGH_ADDR, lsm303::CTRL_REG1_A) >= 0) 104 | { 105 | // There's an accelerometer on a chip with SA0 high, so by the 106 | // logic above, guess that it's an LSM303DLHC. 107 | c.use_sensor = true; 108 | c.device = lsm303::LSM303DLHC; 109 | c.i2c_bus_name = i2c_bus_name; 110 | c.i2c_address_acc = lsm303::LSM303_NON_D_ACC_SA0_HIGH_ADDR; 111 | c.i2c_address_mag = lsm303::LSM303_NON_D_MAG_ADDR; 112 | } 113 | // Remaining possibilities: LSM303DLM or LSM303DLH, SA0 assumed low. 114 | else if (bus.try_write_byte_and_read_byte( 115 | lsm303::LSM303_NON_D_ACC_SA0_LOW_ADDR, lsm303::CTRL_REG1_A) >= 0) 116 | { 117 | // Found the acceleromter for an LSM303DLM or LSM303DLH. 118 | // Use the WHO_AM_I register to distinguish the two. 119 | 120 | int result = bus.try_write_byte_and_read_byte( 121 | lsm303::LSM303_NON_D_MAG_ADDR, lsm303::WHO_AM_I_M); 122 | 123 | if (result == lsm303::LSM303DLM) 124 | { 125 | // Detected LSM303DLM with SA0 low. 126 | c.use_sensor = true; 127 | c.device = lsm303::LSM303DLM; 128 | c.i2c_bus_name = i2c_bus_name; 129 | c.i2c_address_acc = lsm303::LSM303_NON_D_ACC_SA0_LOW_ADDR; 130 | c.i2c_address_mag = lsm303::LSM303_NON_D_MAG_ADDR; 131 | } 132 | else 133 | { 134 | // Guess that it's an LSM303DLH with SA0 low. 135 | c.use_sensor = true; 136 | c.device = lsm303::LSM303DLH; 137 | c.i2c_bus_name = i2c_bus_name; 138 | c.i2c_address_acc = lsm303::LSM303_NON_D_ACC_SA0_LOW_ADDR; 139 | c.i2c_address_mag = lsm303::LSM303_NON_D_MAG_ADDR; 140 | } 141 | } 142 | } 143 | 144 | return config; 145 | } 146 | 147 | sensor_set minimu9::config_sensor_set(const comm_config & config) 148 | { 149 | sensor_set set; 150 | 151 | if (config.lsm6.use_sensor) 152 | { 153 | set.acc = true; 154 | set.gyro = true; 155 | } 156 | 157 | if (config.lis3mdl.use_sensor) 158 | { 159 | set.mag = true; 160 | } 161 | 162 | if (config.lsm303.use_sensor) 163 | { 164 | set.mag = true; 165 | set.acc = true; 166 | } 167 | 168 | if (config.l3g.use_sensor) 169 | { 170 | set.gyro = true; 171 | } 172 | 173 | return set; 174 | } 175 | 176 | minimu9::comm_config minimu9::disable_redundant_sensors( 177 | const comm_config & in, const sensor_set & needed) 178 | { 179 | comm_config config = in; 180 | 181 | sensor_set missing = needed; 182 | 183 | if (!(missing.acc || missing.gyro)) 184 | { 185 | config.lsm6.use_sensor = false; 186 | } 187 | else if (config.lsm6.use_sensor) 188 | { 189 | missing.acc = false; 190 | missing.gyro = false; 191 | } 192 | 193 | if (!missing.mag) 194 | { 195 | config.lis3mdl.use_sensor = false; 196 | } 197 | else if (config.lis3mdl.use_sensor) 198 | { 199 | missing.mag = false; 200 | } 201 | 202 | if (!(missing.mag || missing.acc)) 203 | { 204 | config.lsm303.use_sensor = false; 205 | } 206 | else if (config.lsm303.use_sensor) 207 | { 208 | missing.mag = false; 209 | missing.acc = false; 210 | } 211 | 212 | if (!missing.gyro) 213 | { 214 | config.l3g.use_sensor = false; 215 | } 216 | else if (config.l3g.use_sensor) 217 | { 218 | missing.gyro = false; 219 | } 220 | 221 | return config; 222 | } 223 | 224 | void minimu9::handle::open(const comm_config & config) 225 | { 226 | this->config = config; 227 | 228 | if (config.lsm6.use_sensor) 229 | { 230 | lsm6.open(config.lsm6); 231 | } 232 | 233 | if (config.lis3mdl.use_sensor) 234 | { 235 | lis3mdl.open(config.lis3mdl); 236 | } 237 | 238 | if (config.lsm303.use_sensor) 239 | { 240 | lsm303.open(config.lsm303); 241 | } 242 | 243 | if (config.l3g.use_sensor) 244 | { 245 | l3g.open(config.l3g); 246 | } 247 | } 248 | 249 | void minimu9::handle::enable() 250 | { 251 | if (config.lsm6.use_sensor) 252 | { 253 | lsm6.enable(); 254 | } 255 | 256 | if (config.lis3mdl.use_sensor) 257 | { 258 | lis3mdl.enable(); 259 | } 260 | 261 | if (config.lsm303.use_sensor) 262 | { 263 | lsm303.enable(); 264 | } 265 | 266 | if (config.l3g.use_sensor) 267 | { 268 | l3g.enable(); 269 | } 270 | } 271 | 272 | void minimu9::handle::load_calibration() 273 | { 274 | wordexp_t expansion_result; 275 | wordexp("~/.minimu9-ahrs-cal", &expansion_result, 0); 276 | 277 | std::ifstream file(expansion_result.we_wordv[0]); 278 | if (file.fail()) 279 | { 280 | throw posix_error("Failed to open calibration file ~/.minimu9-ahrs-cal"); 281 | } 282 | 283 | file >> mag_min(0) >> mag_max(0) 284 | >> mag_min(1) >> mag_max(1) 285 | >> mag_min(2) >> mag_max(2); 286 | if (file.fail() || file.bad()) 287 | { 288 | throw std::runtime_error("Failed to parse calibration file ~/.minimu9-ahrs-cal"); 289 | } 290 | } 291 | 292 | void minimu9::handle::read_mag_raw() 293 | { 294 | if (config.lis3mdl.use_sensor) 295 | { 296 | lis3mdl.read(); 297 | for (int i = 0; i < 3; i++) { m[i] = lis3mdl.m[i]; } 298 | } 299 | else if (config.lsm303.use_sensor) 300 | { 301 | lsm303.read_mag(); 302 | for (int i = 0; i < 3; i++) { m[i] = lsm303.m[i]; } 303 | } 304 | else 305 | { 306 | throw std::runtime_error("No magnetometer to read."); 307 | } 308 | } 309 | 310 | void minimu9::handle::read_acc_raw() 311 | { 312 | if (config.lsm6.use_sensor) 313 | { 314 | lsm6.read_acc(); 315 | for (int i = 0; i < 3; i++) { a[i] = lsm6.a[i]; } 316 | } 317 | else if (config.lsm303.use_sensor) 318 | { 319 | lsm303.read_acc(); 320 | for (int i = 0; i < 3; i++) { a[i] = lsm303.a[i]; } 321 | } 322 | else 323 | { 324 | throw std::runtime_error("No accelerometer to read."); 325 | } 326 | } 327 | 328 | void minimu9::handle::read_gyro_raw() 329 | { 330 | if (config.lsm6.use_sensor) 331 | { 332 | lsm6.read_gyro(); 333 | for (int i = 0; i < 3; i++) { g[i] = lsm6.g[i]; } 334 | } 335 | else if (config.l3g.use_sensor) 336 | { 337 | l3g.read(); 338 | for (int i = 0; i < 3; i++) { g[i] = l3g.g[i]; } 339 | } 340 | else 341 | { 342 | throw std::runtime_error("No gyro to read."); 343 | } 344 | } 345 | 346 | float minimu9::handle::get_acc_scale() const 347 | { 348 | // Info about linear acceleration sensitivity from datasheets: 349 | // LSM303DLM: at FS = 8 g, 3.9 mg/digit (12-bit reading) 350 | // LSM303DLHC: at FS = 8 g, 4 mg/digit (12-bit reading probably an approximation) 351 | // LSM303DLH: at FS = 8 g, 3.9 mg/digit (12-bit reading) 352 | // LSM303D: at FS = 8 g, 0.244 mg/LSB (16-bit reading) 353 | // LSM6DS33: at FS = 8 g, 0.244 mg/LSB (16-bit reading) 354 | return 0.000244; 355 | } 356 | 357 | float minimu9::handle::get_gyro_scale() const 358 | { 359 | // Info about sensitivity from datasheets: 360 | // L3G4200D, FS = 2000 dps: 70 mdps/digit 361 | // L3GD20, FS = 2000 dps: 70 mdps/digit 362 | // L3GD20H, FS = 2000 dps: 70 mdps/digit 363 | // LSM6DS33, FS = 2000 dps: 70 mdps/digit 364 | return 0.07 * 3.14159265 / 180; 365 | } 366 | 367 | void minimu9::handle::measure_offsets() 368 | { 369 | // LSM303 accelerometer: 8 g sensitivity. 3.8 mg/digit; 1 g = 256. 370 | gyro_offset = vector::Zero(); 371 | const int sampleCount = 32; 372 | for(int i = 0; i < sampleCount; i++) 373 | { 374 | read_gyro_raw(); 375 | gyro_offset += vector_from_ints(&g); 376 | usleep(20 * 1000); 377 | } 378 | gyro_offset /= sampleCount; 379 | } 380 | 381 | vector minimu9::handle::read_mag() 382 | { 383 | read_mag_raw(); 384 | 385 | vector v; 386 | v(0) = (float)(m[0] - mag_min(0)) / (mag_max(0) - mag_min(0)) * 2 - 1; 387 | v(1) = (float)(m[1] - mag_min(1)) / (mag_max(1) - mag_min(1)) * 2 - 1; 388 | v(2) = (float)(m[2] - mag_min(2)) / (mag_max(2) - mag_min(2)) * 2 - 1; 389 | return v; 390 | } 391 | 392 | vector minimu9::handle::read_acc() 393 | { 394 | read_acc_raw(); 395 | return vector_from_ints(&a) * get_acc_scale(); 396 | } 397 | 398 | vector minimu9::handle::read_gyro() 399 | { 400 | read_gyro_raw(); 401 | return (vector_from_ints(&g) - gyro_offset) * get_gyro_scale(); 402 | } 403 | -------------------------------------------------------------------------------- /minimu9.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // TODO: I'd rather have the minimu9 and imu class not know anything about 4 | // floating point vectors and calibration. Maybe just provide floating 5 | // point scaling factors so that higher-level code can make sense of the 6 | // raw vectors. 7 | 8 | #include "imu.h" 9 | #include "lsm303.h" 10 | #include "l3g.h" 11 | #include "lsm6.h" 12 | #include "lis3mdl.h" 13 | #include "sensor_set.h" 14 | 15 | namespace minimu9 16 | { 17 | // Represents the sensors of the MinIMU-9 and how to communicate with them. 18 | struct comm_config { 19 | lsm303::comm_config lsm303; 20 | l3g::comm_config l3g; 21 | lis3mdl::comm_config lis3mdl; 22 | lsm6::comm_config lsm6; 23 | }; 24 | 25 | comm_config auto_detect(const std::string & i2c_bus_name); 26 | 27 | sensor_set config_sensor_set(const comm_config &); 28 | 29 | comm_config disable_redundant_sensors(const comm_config &, const sensor_set &); 30 | 31 | class handle : public imu { 32 | public: 33 | void open(const comm_config &); 34 | 35 | comm_config config; 36 | lsm6::handle lsm6; 37 | lis3mdl::handle lis3mdl; 38 | lsm303::handle lsm303; 39 | l3g::handle l3g; 40 | 41 | virtual void read_acc_raw(); 42 | virtual void read_mag_raw(); 43 | virtual void read_gyro_raw(); 44 | 45 | virtual float get_acc_scale() const; 46 | virtual float get_gyro_scale() const; 47 | 48 | virtual vector read_acc(); 49 | virtual vector read_mag(); 50 | virtual vector read_gyro(); 51 | 52 | virtual void enable(); 53 | virtual void load_calibration(); 54 | virtual void measure_offsets(); 55 | }; 56 | } 57 | -------------------------------------------------------------------------------- /pacer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "exceptions.h" 7 | 8 | class pacer 9 | { 10 | public: 11 | 12 | pacer() { } 13 | 14 | pacer(const pacer &) = delete; 15 | 16 | pacer operator=(const pacer &) = delete; 17 | 18 | pacer(pacer &&) = delete; 19 | 20 | pacer operator=(pacer &&) = delete; 21 | 22 | ~pacer() 23 | { 24 | close(); 25 | } 26 | 27 | void set_period_ns(uint32_t nanoseconds) 28 | { 29 | if (fd == -1) 30 | { 31 | fd = timerfd_create(CLOCK_MONOTONIC, 0); 32 | if (fd == -1) 33 | { 34 | throw posix_error("Failed to create timerfd"); 35 | } 36 | } 37 | 38 | struct itimerspec spec = { {0, 0}, {0, 0} }; 39 | spec.it_value.tv_nsec = 1; 40 | spec.it_interval.tv_nsec = nanoseconds; 41 | int result = timerfd_settime(fd, 0, &spec, NULL); 42 | if (result == -1) 43 | { 44 | throw posix_error("Failed to set timerfd interval"); 45 | } 46 | } 47 | 48 | uint64_t pace() 49 | { 50 | uint64_t expirations = 0; 51 | ssize_t result = read(fd, &expirations, sizeof(expirations)); 52 | if (result != 8) 53 | { 54 | throw std::runtime_error("Failed to read from timer."); 55 | } 56 | return expirations; 57 | } 58 | 59 | void close() 60 | { 61 | if (fd != -1) 62 | { 63 | ::close(fd); 64 | fd = -1; 65 | } 66 | } 67 | 68 | private: 69 | int fd = -1; 70 | }; 71 | -------------------------------------------------------------------------------- /prog_options.cpp: -------------------------------------------------------------------------------- 1 | #include "prog_options.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | namespace opts = boost::program_options; 7 | 8 | static opts::options_description general_options_desc() 9 | { 10 | opts::options_description desc("General options"); 11 | desc.add_options() 12 | ("help,h", 13 | "produce help message") 14 | ("version,v", 15 | "print version number") 16 | ; 17 | return desc; 18 | } 19 | 20 | static opts::options_description sensor_options_desc(prog_options & options) 21 | { 22 | opts::options_description desc("Sensor options"); 23 | desc.add_options() 24 | ("i2c-bus,b", 25 | opts::value(&options.i2c_bus_name)->default_value("/dev/i2c-1"), 26 | "I2C bus the IMU is connected to") 27 | ; 28 | return desc; 29 | } 30 | 31 | static opts::options_description processing_options_desc(prog_options & options) 32 | { 33 | opts::options_description desc("Processing options"); 34 | desc.add_options() 35 | ("mode", 36 | opts::value(&options.mode)->default_value("normal"), 37 | "specifies what algorithm to use.\n" 38 | "normal: Fuse compass and gyro.\n" 39 | "gyro-only: Use only gyro (drifts).\n" 40 | "compass-only: Use only compass (noisy).\n" 41 | "raw: Just print raw values from sensors axes.") 42 | ("output", 43 | opts::value(&options.output_mode)->default_value("matrix"), 44 | "specifies how to output the orientation.\n" 45 | "matrix: Direction Cosine Matrix.\n" 46 | "quaternion: Quaternion.\n" 47 | "euler: Euler angles (yaw, pitch, roll).\n") 48 | ; 49 | return desc; 50 | } 51 | 52 | static opts::options_description command_line_options_desc(prog_options & options) 53 | { 54 | return general_options_desc() 55 | .add(sensor_options_desc(options)) 56 | .add(processing_options_desc(options)) 57 | ; 58 | } 59 | 60 | static opts::options_description config_file_options_desc(prog_options & options) 61 | { 62 | return sensor_options_desc(options); 63 | } 64 | 65 | void print_command_line_options_desc() 66 | { 67 | prog_options options; 68 | std::cout << command_line_options_desc(options) << std::endl; 69 | } 70 | 71 | prog_options get_prog_options(int argc, char ** argv) 72 | { 73 | prog_options options; 74 | 75 | opts::variables_map vmap; 76 | 77 | // Command-line options 78 | { 79 | // TODO: reject positional args 80 | auto desc = command_line_options_desc(options); 81 | auto parser = opts::command_line_parser(argc, argv).options(desc) 82 | .positional(opts::positional_options_description()); 83 | opts::store(parser.run(), vmap); 84 | opts::notify(vmap); 85 | if (vmap.count("help")) { options.show_help = true; } 86 | if (vmap.count("version")) { options.show_version = true; } 87 | } 88 | 89 | // Config file 90 | { 91 | auto desc = config_file_options_desc(options); 92 | wordexp_t expansion_result; 93 | wordexp("~/.minimu9-ahrs", &expansion_result, 0); 94 | std::ifstream file(expansion_result.we_wordv[0]); 95 | if (file) 96 | { 97 | opts::store(opts::parse_config_file(file, desc), vmap); 98 | opts::notify(vmap); 99 | } 100 | } 101 | 102 | return options; 103 | } 104 | 105 | // TODO: I guess this part should handle calibration files too 106 | -------------------------------------------------------------------------------- /prog_options.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | struct prog_options 6 | { 7 | bool show_help = false; 8 | bool show_version = false; 9 | std::string mode; 10 | std::string output_mode; 11 | std::string i2c_bus_name; 12 | }; 13 | 14 | void print_command_line_options_desc(); 15 | 16 | prog_options get_prog_options(int argc, char ** argv); 17 | 18 | -------------------------------------------------------------------------------- /sensor_set.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | struct sensor_set 4 | { 5 | bool mag = false; 6 | bool acc = false; 7 | bool gyro = false; 8 | 9 | operator bool() 10 | { 11 | return mag || acc || gyro; 12 | } 13 | 14 | inline friend sensor_set operator-(const sensor_set &, const sensor_set &); 15 | }; 16 | 17 | inline sensor_set operator-( 18 | const sensor_set & c1, 19 | const sensor_set & c2) 20 | { 21 | sensor_set r; 22 | r.mag = c1.mag && !c2.mag; 23 | r.acc = c1.acc && !c2.acc; 24 | r.gyro = c1.gyro && !c2.gyro; 25 | return r; 26 | } 27 | -------------------------------------------------------------------------------- /vector.h: -------------------------------------------------------------------------------- 1 | #ifndef _version_h 2 | #define _version_h 3 | 4 | #include 5 | #include 6 | #include 7 | typedef Eigen::Vector3f vector; 8 | typedef Eigen::Vector3i int_vector; 9 | typedef Eigen::Matrix3f matrix; 10 | typedef Eigen::Quaternionf quaternion; 11 | 12 | static inline vector vector_from_ints(int32_t (*ints)[3]) 13 | { 14 | return vector((float)(*ints)[0], (float)(*ints)[1], (float)(*ints)[2]); 15 | } 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /version.h: -------------------------------------------------------------------------------- 1 | #define VERSION "4.0.0" 2 | --------------------------------------------------------------------------------