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.

Download

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

“read\n” – Requests a reading / 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


48 Comments

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

    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 | Permalink

      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 | Permalink

        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 | Permalink

          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 | Permalink

    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 | Permalink

    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?

    Thanks for your help.

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

      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 | Permalink

    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 | Permalink

      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 | Permalink

    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

    Please help me :(

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

      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 | Permalink

        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?

        https://www.dropbox.com/s/fiqzx6zmz0n4pmm/quadcopter.rar

        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 | Permalink

          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 | Permalink

    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 | Permalink

      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 | Permalink

    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 | Permalink

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

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

        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 | Permalink

          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 | Permalink

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

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

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

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

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

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

            Yeah AltIMU-10 is fine.

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

            Apparently crashes and reset at lps331.init();

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

    Apparently crashes and reset at lps331.init();

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

      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 | Permalink

        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 | Permalink

          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 | Permalink

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

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

            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 | Permalink

            Nope, That Doesn’t work either

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

            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 | Permalink

    ok, thanks anyway!

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

    Hi,
    Your code and your worl are great! Congratulation!!!

    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 | Permalink

      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 | Permalink

    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 | Permalink

      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 | Permalink

      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 | Permalink

    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.

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

    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()

    read_mag_cal(&lsm303.m_min, &lsm303.m_max);

    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 | Permalink

      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 | Permalink

      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 | Permalink

    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 | Permalink

      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 | Permalink

        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. Posted September 1, 2014 at 11:15 am | Permalink

    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).

One Trackback

  1. […] supported by the bluetooth, and with the volume reading code added.   I originally tried to use Camel Software’s Quaternion-based AHRS using AltIMU-10 & Arduino, but found – like others – that it crashed the nano when trying to read EEPROM. With my […]

Leave a Reply

Your email address will not be published. Required fields are marked *

You may use these HTML tags and attributes: <a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <strike> <strong>

Current ye@r *