I have ported the AHRS software from my autopilot over to Arduino.

NOTE: This AHRS code is outdated. Please check out my new and improved Quaternion based AHRS for Arduino.

Parts Used:

Arduino Nano

AltIMU-10

It uses quaternions to store and represent orientation, but it can output euler angles (heading, pitch and roll), a rotation matrix or a quaternion. The output can be continuous at 50 readings per second or it can be read on request.

A lot of orientation sensors take a while (30 seconds or more) to warm up and orient themselves. This AHRS takes about 0.2 seconds. This is because during initialization the readings from the accelerometers and magnetometers are used to calculate orientation. After that the gyroscopes are the main sensors and acceleration and magnetic north are only used to keep the gyros from drifting.

The AHRS also loads and saves installation offsets. If you fit your AltIMU to a model plane or something, you don’t have to align the axis of the AltIMU with the vehicle. You can glue it on any way you want. Once you’ve installed it you can simply calibrate the AHRS so it knows how it is oriented.

The FireTail AHRS software was based on the Raspberry Pi AHRS by David Grayson. Link. Fundamentally the AHRS algorithm is the same. The primary difference is this uses its own maths library. It is more efficient in a few ways because the maths library was specifically designed for this purpose. For example, to get euler angles from a quaternion the original Raspberry Pi AHRS library converted the quaternion to a rotation matrix and extracted the angles from that. This code extracts euler angles directly from the quaternion.

#### The Maths

For those who are interested in how this algorithm works, I’ve written up a post. It goes through all the mathematics behind the system.

AHRS Maths

This post describes how to correct accelerometer readings for changes in acceleration. Centrifugal force correction is not implemented in this AHRS code, but if you read this you could do it yourself.

Quaternion AHRS and Centrifugal Force

#### Commands

All commands are to be terminated with a new line character (‘\n’).

“output_euler\n” – Switches the output to euler angles (heading, pitch & roll in degrees). This is the default output.

“output_mat\n” – Switches output to a north-east-down (NED) direction cosine matrix.

“output_quat\n” – Switches output to quaternion.

“auto_on\n” – Turns on automatic output, 50Hz rate

“auto_off\n” – Turns off automatic output

“set_offset\n” – Sets the installation offset. You need to do this after you have installed the AltIMU-10 in your aircraft/car/whatever. Point the vehicle towards magnetic north and keep it level, then send this command. It will write the current orientation into EEPROM. From then on, it will consider the current orientation to be north & level.

“mag_cal\n” – Calibrates the magnetometers. This takes 60 seconds. During this time you should do your best to align each magnetometer axis with magnetic north and then magnetic south. It’s advisable to do a mag cal before setting installation offset.

I actually have not tested the mag cal function yet because my Nano is occupied with another project at the moment. It’s a simple function and I expect will work fine, but if anyone could give it a quick test I’d greatly appreciate it.

Note: This software is distributed under the GPL v3 and comes with absolutely NO WARRANTY

1. Posted November 20, 2013 at 7:16 am |

Nice work! I am glad you found my code to be useful and ported it to the Arduino! That was something I kind of wanted to do for a long time.

I managed to compile your code in the Arduino IDE 1.5.4 on Windows 8, but I had some trouble so I’d like to share my steps:

1) Rename ahrs.cpp to ahrs.ino so Arduino knows which file is the main one.

2) Delete the Makefile if you want; it will not be used.

3) On Windows with the Arduino IDE 1.5.4, the compiler gives weird errors regarding the templated EEPROM functions. The workaround is to add these lines to the top of ahrs.ino:

template int EEPROM_writeAnything(int ee, const T& value);
template int EEPROM_readAnything(int ee, T& value);

4) Delete twi.c and twi.h to avoid multiple definition errors. Those functions comes with avr-libc.

5) If you have installed Pololu’s L3G, LPS331, or LSM303 libraries, you need to uninstall them now or else the Arduino IDE will try to link them in and you will get multiple definition errors.

I also managed to actually run this code on an Arduino UNO with a MinIMU-9 v2. To do that, I followed these steps:

6) Comment out the five lines referring to the lps331 in ahrs.ino, because the MinIMU-9 v2 does not have that sensor and the lines to read from it will hang forever.

7) I was getting all “nans” in my output. There is a line in setup() that tests to see whether the offset read from EEPROM is valid. On my board, the offset was always read as “nan nan nan nan”, so its magnitude would be a nan and “nan >= 0.1″ is probably false. I changed that line to be:

if(abs(offset.magnitude() – 1) >= 0.1 || isnan(offset.magnitude()))

At this point, the sketch worked and the numbers it gave for the Euler angles seemed plausible. I didn’t look at them too carefully but they were somehow reflecting the movement of the board. To get accurate yaw numbers, you would need to calibrate your magnetometer and add lines that set lsm303.m_max and lsm303.m_min appropriately with the maximum and minimum values for each axes on the magnetometer. That is one of the trickiest parts of using these types of devices.

• Camel
Posted November 21, 2013 at 9:13 am |

Hi David,

Thanks for posting! I’m pleased you’ve found this. I’ve spent many hours studying your AHRS code.

I’ve incorporated most of your steps and uploaded a new .zip. I had trouble with the template step though, there must some differences between our AVR compilers.

• Posted November 22, 2013 at 5:19 am |

The Arduino IDE in Windows is based on WinAVR, which was last updated in 2010. But the Arduino IDE seems to actually include a version of WinAVR from 2008. If you are using Linux or programming an ARM-based Arduino, your compiler is probably newer and better.

• Camel
Posted November 23, 2013 at 4:56 pm |

My compiler is dated 2010. Anyway, I’ve removed the templated functions and replaced them with functions that specifically take quaternions as arguments. Templated functions wasn’t really the best way to go about it.
You might be interested in this David – I’m using acceleration and magnetometer vectors to come up with a quaternion during startup. That way initialisation time is basically eliminated!
I’ve fixed a whole lot of other things too, there were some serious errors in the code before. But I’m pretty happy with it now.

2. Posted November 20, 2013 at 7:17 am |

The formatting messed up step 3. Those lines should be:

template <class T> int EEPROM_writeAnything(int ee, const T& value);
template <class T> int EEPROM_readAnything(int ee, T& value);

3. mubin
Posted December 28, 2013 at 11:13 am |

Hi,
Thanks for your nice work, I also tried it but not successful, I sompiled and run the minimu9-ahrs and got the raw data from the sensors. I also compiled the ahrs-visualizer and I am able to get the –help and –version options, which means the program is compiled and runable. But If I try the minimu9-ahrs | ahrs-visualizer command, it does not show anything just hangs. What can I do?

• Camel
Posted December 28, 2013 at 2:18 pm |

Hello,
Is this a question about David Grayson’s AHRS software for the Raspberry Pi? If so you would be better posting this question on his blog, or the Pololu forums.
Cheers, Camel

4. Noyan
Posted January 15, 2014 at 3:52 am |

Hi thanks this such a great code. Are there any possibility to read these values over arduinos serial monitor ? For driving servos over the arduino itself.

• Camel
Posted January 16, 2014 at 8:58 am |

Reading the output with the Arduino IDE serial monitor is definitely possible. Driving servos with the Arduino is also very possible. Link

5. FARUK
Posted January 18, 2014 at 12:42 am |

Hello;

I’m establishing a quadcopter for my graduation project. I have a Altimu-10 sensor so I am trying to use your code for this. Even I have done all the steps correctly in the comments and the thread, I couldn’t get the code work. Please help me with this.

My error is :

In file included from ahrs.ino:23:
/EEPROM.cpp:24: fatal error: avr/eeprom.h: No such file or directory
compilation terminated.

I have done all the things about EEPROM.h too. I have updated and tried all different versions and different computers. By the way I’m using Arduino Due

• Camel
Posted January 18, 2014 at 10:34 am |

Hi!

This would be because the Arduino Due is based on an ARM chip that doesn’t have any EEPROM. This code won’t run on the Due without a fair bit of modification.
However, you could have a look at the source code for FireTail. It runs on the Due and uses the same algorithm.

FireTail source

This file contains the AHRS code.

The Arduino Due has no EEPROM, but it has plenty of flash memory that can be used to save settings. The files flash.cpp and flash.h contain code that reads/writes settings to flash memory. Just be aware that, unlike EEPROM, all flash memory is erased when you upload sketches to the Due.

Another notable difference between the Due and AVR based Arduinos is that the Wire library used for I2C behaves a bit differently. The FireTail source contains working code for the L3G gyros, LPS331 baro pressure sensor and LSM303 accel/magnetomers used on the AltIMU-10. Also the Due has two I2C busses. I wasn’t able to get the AltIMU-10 working on the first I2C bus. I didn’t really investigate why, so it might work on the first bus, but anyway I’ve changed all of the I2C code so that it uses Wire1 (the second bus) instead of Wire.

I hope this is enough information to get you started!

• FARUK
Posted January 18, 2014 at 11:04 pm |

First of all, Thank you. Really really thank you.

I believe you will be glad if I show you my progress of quadcopter.

I have designed a shield for the arduino due. I am going to send you two pictures both mounted imu and without mounted imu. I have soldered two pull up resistors for the SDA1 and SCL1 inputs. What do you think about that? Here is the links: (sorry for the bad quality )

https://www.dropbox.com/s/9qecaxtm2b9nsk5/bos.jpg

https://www.dropbox.com/s/fnmf3k6zcdikwvc/imulu.jpg

What do you think about them?And of course here is my quadcopter:

https://www.dropbox.com/s/gni9v7hhnmjs4zq/3.jpg

This is not its final build. I have mounted high landing skid and mounted the battery to its bottom. Its an old picture.

And Finally my code, I have progressed so far. I am accurately reading PPM signal from the receiver. And putting it to the ESC. I haven’t written the PWM output but its not a big deal. Just ESC1.write(throttle+pitch+roll+yaw);

My question is, how do you think to get together my code and and your code? Would you please help me with that?

By the way your code.. I couln’t work it. It says there is no wire.h library. But I am currently working on it.

Meanwhile, maybe for the bad quality, I want to tell my pin configuraiton. I have attached pitch, roll, throttle , yaw;respectively to the A0,A1,A2,A3. And ESC1,…… ESC4 to digital outputs from 7 to 4.

• Camel
Posted January 18, 2014 at 11:28 pm |

Hi, nice work on the quadcopter. Your board looks nice and neat which is good. Mine is a mess.

FireTail is a fixed wing autopilot, but I think with some work it could be modified to work for quads. I’m not sure exactly what would need to be changed but all of the code for the different autopilot modes is in firetail.ino – look for a comment that says ‘autopilot time’. There is a switch-case statement there which contains code for each autopilot mode. I suspect you could be able to get your quad to operate by simply modifying the code there.

6. Callout
Posted January 18, 2014 at 11:01 am |

I am running this on my Arduino Due (had to comment out the EEPROM sections) and unfortunately instead of having a 50Hz refresh rate it is more like 3Hz. Do you have any idea why this could be?

• Callout
Posted January 18, 2014 at 11:11 am |

Agh, hold on, I just saw your reply to FARUK, ignore this and hopefully I can get things sorted.

7. massimo
Posted February 28, 2014 at 11:21 am |

I have tried to run this but as an output I get very weird letters. like this
` óØâÙÛìØ«âèþæèþæØ[ìØ `

• Camel
Posted February 28, 2014 at 11:42 am |

Are you using the correct baud rate? It should be 115200.

• massimo
Posted February 28, 2014 at 12:01 pm |

Auch ok that fixed the problem, but the output is just
AltIMU-10 AHRS & Alt
http://www.CamelSoftware.com
AltIMU-10 AHRS & Alt
http://www.CamelSoftware.com
…….

• Camel
Posted February 28, 2014 at 12:04 pm |

Something must be causing the Arduino to crash and reset. What AVR chip/Arduino are you using? Does it have sufficient memory?

• massimo
Posted February 28, 2014 at 12:09 pm |

Arduino Uno, mmmm not sure about that I just run that code…

• Camel
Posted February 28, 2014 at 12:13 pm |

Weird . . . the Uno should be able to run it. Have you checked your wiring?

• massimo
Posted February 28, 2014 at 12:12 pm |

I have a Altimu-10, that should be ok right?

• Camel
Posted February 28, 2014 at 12:18 pm |

Yeah AltIMU-10 is fine.

• massimo
Posted February 28, 2014 at 12:17 pm |

Apparently crashes and reset at lps331.init();

8. massimo
Posted February 28, 2014 at 12:17 pm |

Apparently crashes and reset at lps331.init();

• Posted February 28, 2014 at 12:23 pm |

Maybe try manually specifying the address

lps331.init(LPS331_SA0_LOW);

or

lps331.init(LPS331_SA0_HIGH);

• massimo
Posted February 28, 2014 at 12:29 pm |

this is weird:
``` Serial.begin(115200); Serial.setTimeout(1); Serial.println("AltIMU-10 AHRS & Alt"); Serial.println("www.CamelSoftware.com"); delta_t = millis();```

``` Wire.begin(); l3g.init(); l3g.enableDefault(); l3g.measureOffsets(); lsm303.init(); ```

``` lsm303.enableDefault(); lps331.init(LPS331_SA0_HIGH); Serial.println("MASSIMO dalfugadslfuasglasdgu"); lps331.enableDefault();```
Output:
AltIMU-10 AHRS & Alt
http://www.CamelSoftware.com
AltIMU-10 AHRS & Alt
http://www.CamelSoftware.com
MASSIMO dalfAltIMU-10 AHRS & Alt
http://www.CamelSoftware.com
MASSIMO dalfAltIMU-10 AHRS & Alt

• Posted February 28, 2014 at 12:37 pm |

It seems to be crashing during hardware initialisation, I suspect it’s a hardware or wiring issue. Have you tested the AltIMU-10 before trying this code? If not maybe try another AHRS algorithm to confirm your hardware is serviceable.

• massimo
Posted February 28, 2014 at 12:41 pm |

I’ve tried this https://github.com/pololu/minimu-9-ahrs-arduinoand it works fine

• Posted February 28, 2014 at 12:51 pm |

What about the LPS331? Have you tested that?

Perhaps try commenting out the LPS331 code in ahrs.ino and see if it will run OK without it.

• massimo
Posted February 28, 2014 at 1:06 pm |

Nope, That Doesn’t work either

• Posted February 28, 2014 at 1:17 pm |

I’m sorry but I don’t know what to suggest. It works fine on my Arduino Nano. Please let me know if you find the issue.

9. massimo
Posted February 28, 2014 at 1:18 pm |

ok, thanks anyway!

10. Dave
Posted April 18, 2014 at 4:23 am |

Hi,

But wehn I try to run the code with Nano 3.0 and the same sensor board I get AltIMU-10 AHRS & Alt
http://www.CamelSoftware.com
AltIMU-10 AHRS & Alt
http://www.CamelSoftware.com
AltIMU-10 AHRS & Alt
http://www.CamelSoftware.com
AltIMU-10 AHRS & Alt
http://www.CamelSoftware.com
AltIMU-10 AHRS & Alt
http://www.CamelSoftware.com
AltIMU-10 AHRS & Alt
http://www.CamelSoftware.com
AltIMU-10 AHRS & Alt
http://www.CamelSoftware.com
AltIMU-10 AHRS & Alt
http://www.CamelSoftware.com
AltIMU-10 AHRS & Alt
http://www.CamelSoftware.com
AltIMU-10 AHRS & Alt
http://www.CamelSoftware.com
AltIMU-10 AHRS & Alt

repeatedly…
I didn t change anything but I didn t get any sensor reading.

What could be?

Thanks again

• Posted April 18, 2014 at 8:29 am |

It looks like the Arduino is crashing and resetting for some reason, possibly in the setup() function.

11. Harry
Posted May 6, 2014 at 9:28 pm |

I’m trying to get firetail to compile on arduino 1.0.3 and got passed the ahrs.cpp by renaming to ahrs.ino Now it’s complaining that the LPS331 does not name a type in the airdata.cpp . I’m guessing that the ‘type’ is supposed be LPS331 lps; and must be defined somewhere. I hope this is a simple error. Thanks, if you can help Camel.

• Posted May 7, 2014 at 12:16 pm |

Hello, firetail won’t compile with Arduino 1.0.3. You could use v1.5.6 and compile it for the Arduino Due, but you would need to use the Makefile rather than the IDE. If you are on Windows, it might be easiest to use MSYS2 to establish a ‘linux-like’ environment and go from there.

I know it’s a pain in the neck at the moment. There are some changes soon that will make it a lot easier to get up & running.

• Posted May 7, 2014 at 12:18 pm |

By the way, I’ve recently started a forum for questions like this. If you would like to sign up and post this question there, that’d be fantastic. It’s at http://www.firetailuav.com/forum

Cheers!

12. Harry
Posted May 7, 2014 at 5:51 pm |

Thanks, Camel. I like what you’re doing here and want to try things out. I couldn’t join your forum though. It won’t accept the code I see and type. I tried about 10 times.

• Posted May 7, 2014 at 6:02 pm |

Ok, I’lll temporarily disable the security code.

13. Ryan
Posted August 13, 2014 at 7:15 am |

I’m trying to get this to work with an Uno with an IMU from Adafruit that uses the same accelerometer and magnetometer as the AltIMU-10 (https://www.adafruit.com/products/1714). I thought that since it effectively uses the same components it should be easy to get this to work, but I’m having some similar problems to what some of the others were having. After removing the lines referring to the altimeter I could run the code but would only get

AltIMU-10 AHRS & Alt
http://www.CamelSoftware.com

as the Arduino crashed and reset repeatedly. I found that commenting out this line of code in setup()

stopped it from crashing, but then all it gives me is this:

AltIMU-10 AHRS & Alt
http://www.CamelSoftware.com
euler: nan nan nan
euler: nan nan nan
euler: nan nan nan

Do you have any suggestions for something that I could try to get it working?

• Posted August 17, 2014 at 5:17 pm |

I’m sorry, I’ve got no idea. All I can say is that it worked for me (very unhelpful, I know). If you find the problem, please let me know.

As a side note, the AltIMU-10 v3 (which is what I used to make this) has now been replaced by the v4 which has much better sensors. I’m pretty certain this Arduino sketch won’t work for the v4.

• Posted September 1, 2014 at 3:32 pm |

Ok, I had a look through today. The problem was a silly error in the read_mag_cal function. It’s now fixed and works fine!

14. Jaime Oliva
Posted September 27, 2014 at 3:19 am |

Congratulations and thanks for your work. I am running it in an Arduino Yún but there is something I don’t understand: Even if the IMU is completely static, the values are increasing continuously.

What might it be?

• Posted September 27, 2014 at 9:53 am |

Firstly, the code measures gyro bias when the board is powered up so it has to be held perfectly still for the first couple of seconds. If it’s not that, I would try to print out the raw values from the sensors just to make sure they look OK. Cheers!

• JAIME OLIVA
Posted September 28, 2014 at 5:57 am |

It’s not an issue coming from your code, because we’ve been testing several IMUs with different sketches and we always get the same result: X, Y and Z values are increasing/decreasing (I don’t mean that there is a deviation, that is normal ) continuously.

No matter what we do, if the IMU stays static, it keeps on sending values that increase themselves.

15. Peter Allinger
Posted May 5, 2015 at 5:28 pm |

Hi, I was just wondering if this code already works for a ALTIMU-10 v4 and maybe with a Teensy3.1? I´m using the latest Arduino IDE and I get a lot of compilation errors for various boards such as Teensy and Arduino Nano, Mega,…

16. Posted September 1, 2014 at 11:15 am |

I’ll be making a new, better (working?) AHRS soon. It will work with the AltIMU-4. It’ll use all of the tricks that Firetail uses (centrifugal force compensation, proper calibration techniques and so on).

17. Posted January 11, 2015 at 3:34 am |

I was looking at what it might take to port the current ahrs code from Firetail into an independent Arduino project. I’ll have to admit I’m stumped – I’m just not enough of a C++ coder to disentangle everything. This is unfortunate because I really need centrifugal force compensation and solid calibration. Any idea if you will be working on this in the near future?