I recently completed the implementation of a new feature for ArduPlane & ArduCopter. I have to thank Jan Scherrer for the idea and especially thank Andrew Tridgell for pushing me to improve the original implementation. The new feature is automatic compass declination.
Currently you are supposed to look up the magnetic declination value for your flight location and input that value into the Mission Planner so the compass heading can be corrected. If you are like me and constantly flying at new locations, it is a bit annoying to have to look this up every single time. In this post, Jan asked why it couldn't just be automatic... I did not have an answer so I decided to see if I could make that happen.
The automatic declination code is enabled by default. There is a new configuration parameter you can set in the Mission Planner if you wish to disable/re-enable it. See below:
To enable, set COMPASS_AUTODEC to 1. To disable, set COMPASS_AUTODEC to 0.
After any parameter changes you need to click the "Write Params" button.
With automatic declination enabled, the declination will be approximated to the declination at your GPS Lat/Lng coordinates as soon as a 3D fix is acquired. So far from our tests we show a +/- error of .5 degrees depending on your location. Andrew Tridgell reported accuracy of within .3 and I am seeing accuracy within .05.
The latest code that includes the automatic declination is available in the git repository. I should warn you that while most of the code in there is already tested, some of it may not be because that git repository is constantly in flux. If you feel confident, give it a try! The more testers the better. Otherwise, if you prefer to wait, this feature will be included in the next official release.
What is magnetic declination? Good question...Read this
I ended up finding another person that had put together a look up data table that used bi-linear interpolation to pull an approximate declination based on lat/lng GPS coordinates. This was definitely a good start. So I took that code and adapted it for our code base (of course giving proper credit to the original author). The problem with the original implementation was that the look up table took up a massive amount of space ( > 5kb ) and the way I had originally written it, that look up table was being stored in RAM instead of flash memory. Without going into too much detail on the difference between those two types of memory, the first is memory that is volatile memory that is used to store variables during code execution. It is fast, but resources are limited. Because the look up table is a static set of data that gets used infrequently, it is a much better option to store it in the flash memory (using PROGMEM). You can read about it all here.
Ok... so how to go from a 5kb to a lot less. We are dealing with integer values which vary in size. The first thing we need to do is make sure that we are using the smallest possible integer size for the values that we have in the look up table. The range of values in the original table -99 to 179. So out of the standard int sizes (int8_t, int16_t and int32_t) int16_t is the smallest we could use with a supported range of -32768 to 32767. The next smallest size, int8_t, supports a range of -128 to 127. You can see that the the lower end of the look up values will fit (-99 is greater than -128), but the upper range will not (179 is greater than the allowed 127).
Turns out there are a lot of clever ways to compress data. Here are a few that I considered, some of which I used.
1. Shift all the values -52 bringing the highest value in the look up table to 127 which is within int8_t. Oops that doesn't work. Now the lower end exceeds the lower limit of int8_t.
2. Store the first value in each row and then the differences between each consecutive value instead of the values themselves. This is a very promising approach because the difference between each value is quite small, meaning we can use a smaller integer size to store the value.
3. Pack the sign bits in a different array.
Lets take the highest value in the array (179) and see what that means in terms of a 16bit integer. 179 is represented by the following:
You can see that the upper 8 bits are not used (all zeros), but unfortunately we need at least one more bit to represent the sign of the lower end values. All of our values can fit into 9 bits, but when you create a 9 bit integer it is automatically padded up to 16 bits. What we could do is take all the extra sign bits out of the original array and put them in their own array. That means all of the absolutevalues fit in 8bits (a good improvement over the 16bits before, half the space). So how do we pack those sign bits? Remember that I said that the number of bits is automatically rounded up to the nearest byte. That means that if we tried to declare a 1 bit int for the sign and store it, it would actually take up 8 bits. Take the following:
00000001 - this would represent negative
00000000 - this would represent positive
All we need is that one value (1 or 0) to say that it is either negative or positive. So what we can do to avoid wasting those other 7 bits is to store 7 more signs in that one byte.
Take these values -99, 75, 43, -23, 175, 132, -45, -32
So that would be: 10010011 (1s for negative signs, zeros for the positive signs)
Now we are taking advantage of the full byte (8 bits) and not losing any space because of the padded bits.
Using a combination of the above methods allowed me to pack the values into around 1.6kb. When you compress the values, the retrieval strategy must change. Normally you would access values in an array using this simple syntax. MyArray which means give me the value in the 13th row, 3rd column. If we are splitting the array into their starting values, consecutive delta values, and signs, we will need to pull those values out in a different way. If you are interested in learning how it works, feel free to check out the code from the git repository. The logic is located in the AP_Declination.cpp file in the libraries folder.
As always, questions are welcome!
@Andke: Interesting thought, but I highly doubt anyone will fly far enough out that they would encounter a different magnetic declination... at least not a change in declination that is greater than the +/- .5 margin of error that is already present in the look up.
Great feature, but could it lookup the correct declination now and then, rather than just on boot ? - for those longer flights ?
Adam, the idea is that auto-calibration would correct the installation error. I'm all for it!
I wonder if this could be done by commanding pitch forward, looking at the DCM to make sure we are pitched foward with no roll, and then look at what the compass shows. We should see the compass vector tipped forward with no roll? Or perhaps just let it fly and see what heading it flies in?
Or, actually... didn't we already talk about the idea of, maybe after calibrating the compass, having the pilot pick up the craft, hang it from it's tail, and see where the compass vector points? Actually I think that was discussed with the idea of orienting the IMU relative to the airframe, but maybe it could work for compass too?
great feature ! I'm impressed on the accuracy.
I'm wondering how well will it manage geomagnetic anomalies. I live very close to three of those areas (see map).
Good explanation, was great to read how it progressed along the way... Good Job, thanks for your hard work.
No problem guys!
Now, what I have been thinking about is would it be possible to fine-tune the compass declination on the fly, since many fliers experience circling in loiter, which could be a result of a compass declination value that is still ever so slightly off. With this we have a great starting value, but could it be further adjusted slightly if the APM detects this "circling" behaviour, so that the circling could be reduced to its minimum?
Nice work! Thanks Adam