I'm trying to make a simple tilt-compensated compass with the FXOS8700 accelerometer + compass chip, but I'm stuck at how to compensate for the tilt of the sensor. The sensor is hooked up properly and giving data that is in the 20 - 65 uT range, so everything is working fine there but my heading calculation function has some problems, as it is not giving the data in the right range (my output is very roughly 20 - 60 degrees), when it should be giving data in the +-90 degree range. Here are the two lines that calculate the error of the x and y magnetic sensor readings:
The variables ax, ay, and az (as floats) are the accelerometer measurements in gs, and the variables mx and my (also as floats) are the magnetometer readings in uT.
You probably need to calibrate the magnetometer, and perhaps also the accelerometer.
Here are two procedures, in order of increasing accuracy and usefulness:
you might use double iso float, for UNO it is the same but for DUE you can get higher precision.
PaulS is right about the pow()
as az*az appears twice you might use a local var for speed up
typo ? ax where ay was meant?
double temp = az * az;
double x_err = sqrt(ax*ax + temp) - ax;
double y_err = sqrt(ay * ay + temp) - ay; // there was -AX in your post above
pow(ax, 2) is a damned expensive way to compute ax * ax.
I was about to say the same thing. pow(x,n) is equivalent to exp(log(x)*n), although hopefully it would use some sort of fast base-2 log. Still expensive as all hell.
Ok, the more I try to work this out, the more difficult it gets .
There are a couple of basic tools for the job.
to convert any vector into a unit vector, divide each component by the length of the vector. This comes down to a multiplication by the inverse square root of the squares of the components. There is a well-known algorithm to calculate a fast inverse root, but I don't see that it's part of the arduino library. Here's a link Quake’s fast inverse square root
If you take the dot product of a vector and a unit vector, you will get a projection: how long the first vector is with respect to that unit vector.
To project a vector onto a pane, subtract from the vector the projection of that vector onto the normal of the plane. This will allow us to project the tilted compass onto the ground plane.
if you take the cross product of two vectors, then the result is a vector at right angles to both, whose length is the product of the lengths of the two. So if you have a pair of unit vectors, then the cross product will be a vector at right angles to both. The significance of this is that if you want to rotate the tilted compass so that it lies level with the ground, then this is the axis around which you want to rotate.
So with these tools, if the compass reads theta degrees and the acellerometer is pointing "down", how to find where north is relative to the flat ground plane?
It will be something like:
normalise the 'down' vector to get hat-down.
convert your compass bearing into a vector {cos, sin, 0}.
subtract hat-down * (hat-down ∙ compass) from compass to get the compass heading projected onto the flat plane. No need to normalise it.
compute hat-down ⨯ {0,0,-1} to get the axis around which you need to move the horizontal plane to match the plane of your device, call it hat-roll
rotate the projected compass heading around hat-roll to move it onto the z=0 device plane. This bit, frankly, is the bit I am not sure how to do
with z=0, use atan2 on the rotated projection to get the bearing
Seems complicated, but in the end it will come down to a bunch of multiplications and addittions.
.