Erik -
Never actually tested in practise
This library has only been 'hand tested', it never was actually included in a quadcopter. It is now published so it might help someone, but please verify it works for you before you crash your setup (that is of course for every library you use). It is a while ago I made this, so everything that follows might be slightly different than I remember :D.
Inputs are SI units (probably), so gyro data should be in rad/s. Magnetometer and accelerometer only uses normalized vectors. You will require the following library which isn't included in this one: http://mbed.org/users/BlazeX/code/GTMath/. I am fairly certain things like normalizing a vector twice happens currently, so it can be more efficient.
Basic functionality
The library doesn't use quaternions, since they are hard, but instead two 3D vectors. Those last 2 floats aren't going to fill your memory. One vector is the in the length of the aircraft/device/etc ('heading'), the other one points up ('top'). Together they define the angle of your craft.
The currently measured vectors by the accelerometer and magnetometer are defined. The top simply calculated from the accelerometer data. For the heading the magnetometer data is used, which is moved to be at 90 degrees from the top (this is required since unless you live at the equator that won't be the case). This directly makes sure they have the 90 degree angle between them they are supposed to have.
At the same time the gyroscope offset (later more) is removed from the gyroscope data, and that is used to rotate the old heading and top vectors to new values according to your gyroscope data.
We calculate the difference between the gyroscope measurements and the accelero/magneto measurements. We call this the offset of the gyroscope. Now with a certain weight factor we combine the two measurement types into a final result, which is also used for the next gyroscope measurement. This already cancels part of the gyroscope drift.
The second part is that we average out the gyroscope offset measurements, and the result of that is used to compensate new gyroscope measurements.
IMUCalc.h@1:51c902d63dda, 2014-01-22 (annotated)
- Committer:
- Sissors
- Date:
- Wed Jan 22 21:19:18 2014 +0000
- Revision:
- 1:51c902d63dda
- Parent:
- 0:4dc7e56179ff
v0.1
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| Sissors | 0:4dc7e56179ff | 1 | /*Seriously no one cares about copyright stuff |
| Sissors | 0:4dc7e56179ff | 2 | |
| Sissors | 0:4dc7e56179ff | 3 | But if you really want the license: If you make lots of money with my code, send me money, ty |
| Sissors | 0:4dc7e56179ff | 4 | |
| Sissors | 0:4dc7e56179ff | 5 | Now usefull stuff*/ |
| Sissors | 0:4dc7e56179ff | 6 | |
| Sissors | 0:4dc7e56179ff | 7 | |
| Sissors | 0:4dc7e56179ff | 8 | #ifndef IMUCALC_H |
| Sissors | 0:4dc7e56179ff | 9 | #define IMUCALC_H |
| Sissors | 0:4dc7e56179ff | 10 | |
| Sissors | 0:4dc7e56179ff | 11 | /** |
| Sissors | 0:4dc7e56179ff | 12 | * Includes |
| Sissors | 0:4dc7e56179ff | 13 | */ |
| Sissors | 0:4dc7e56179ff | 14 | #include "mbed.h" |
| Sissors | 0:4dc7e56179ff | 15 | #include "math.h" |
| Sissors | 0:4dc7e56179ff | 16 | #include "GTMath.h" |
| Sissors | 0:4dc7e56179ff | 17 | |
| Sissors | 0:4dc7e56179ff | 18 | #ifndef M_PI |
| Sissors | 0:4dc7e56179ff | 19 | #define M_PI 3.14159265358979323846 |
| Sissors | 0:4dc7e56179ff | 20 | #endif |
| Sissors | 0:4dc7e56179ff | 21 | |
| Sissors | 0:4dc7e56179ff | 22 | class IMUCalc { |
| Sissors | 0:4dc7e56179ff | 23 | public: |
| Sissors | 1:51c902d63dda | 24 | IMUCalc( void ); |
| Sissors | 1:51c902d63dda | 25 | |
| Sissors | 1:51c902d63dda | 26 | /** |
| Sissors | 1:51c902d63dda | 27 | * Calculates new headings |
| Sissors | 1:51c902d63dda | 28 | * |
| Sissors | 1:51c902d63dda | 29 | * @param accdat - pointer to float array with length 3 with acceleration data |
| Sissors | 1:51c902d63dda | 30 | * @param gyrodat - pointer to float array with length 3 with gyroscope data |
| Sissors | 1:51c902d63dda | 31 | * @param magdat - pointer to float array with length 3 with magnetic data |
| Sissors | 1:51c902d63dda | 32 | * @param timestep - float with timestep in seconds |
| Sissors | 1:51c902d63dda | 33 | */ |
| Sissors | 1:51c902d63dda | 34 | void runCalc(float *accdat, float *gyrodat, float *magdat, float timestep); |
| Sissors | 1:51c902d63dda | 35 | |
| Sissors | 1:51c902d63dda | 36 | /** |
| Sissors | 1:51c902d63dda | 37 | * Returns the yaw |
| Sissors | 1:51c902d63dda | 38 | * |
| Sissors | 1:51c902d63dda | 39 | * @param return - float with yaw in rads |
| Sissors | 1:51c902d63dda | 40 | */ |
| Sissors | 1:51c902d63dda | 41 | float getYaw( void ); |
| Sissors | 1:51c902d63dda | 42 | |
| Sissors | 1:51c902d63dda | 43 | /** |
| Sissors | 1:51c902d63dda | 44 | * Returns the pitch |
| Sissors | 1:51c902d63dda | 45 | * |
| Sissors | 1:51c902d63dda | 46 | * @param return - float with pitch in rads |
| Sissors | 1:51c902d63dda | 47 | */ |
| Sissors | 1:51c902d63dda | 48 | float getPitch( void ); |
| Sissors | 1:51c902d63dda | 49 | |
| Sissors | 1:51c902d63dda | 50 | /** |
| Sissors | 1:51c902d63dda | 51 | * Returns the roll |
| Sissors | 1:51c902d63dda | 52 | * |
| Sissors | 1:51c902d63dda | 53 | * @param return - float with roll in rads |
| Sissors | 1:51c902d63dda | 54 | */ |
| Sissors | 1:51c902d63dda | 55 | float getRoll( void ); |
| Sissors | 1:51c902d63dda | 56 | |
| Sissors | 1:51c902d63dda | 57 | /** |
| Sissors | 1:51c902d63dda | 58 | * Returns the calculated offset of the gyro |
| Sissors | 1:51c902d63dda | 59 | * |
| Sissors | 1:51c902d63dda | 60 | * @param return - Vector3 with gyro offsets |
| Sissors | 1:51c902d63dda | 61 | */ |
| Sissors | 1:51c902d63dda | 62 | Vector3 getGyroOffset( void ); |
| Sissors | 0:4dc7e56179ff | 63 | |
| Sissors | 0:4dc7e56179ff | 64 | |
| Sissors | 0:4dc7e56179ff | 65 | |
| Sissors | 0:4dc7e56179ff | 66 | //private: |
| Sissors | 1:51c902d63dda | 67 | /** |
| Sissors | 1:51c902d63dda | 68 | * Rotates magnetic vector to be in plane |
| Sissors | 1:51c902d63dda | 69 | * |
| Sissors | 1:51c902d63dda | 70 | * @param magdat - Vector3 with magnetic data |
| Sissors | 1:51c902d63dda | 71 | * @param ground - Vector3 with data which direction the ground is |
| Sissors | 1:51c902d63dda | 72 | * |
| Sissors | 1:51c902d63dda | 73 | * @param return - Vector3 with new magnetic vector |
| Sissors | 1:51c902d63dda | 74 | */ |
| Sissors | 1:51c902d63dda | 75 | Vector3 rotateMag(Vector3 magdat, Vector3 ground); |
| Sissors | 0:4dc7e56179ff | 76 | |
| Sissors | 0:4dc7e56179ff | 77 | /** |
| Sissors | 0:4dc7e56179ff | 78 | * Calculates the angle between two vectors |
| Sissors | 0:4dc7e56179ff | 79 | * |
| Sissors | 0:4dc7e56179ff | 80 | * @param vectorA - first vector |
| Sissors | 0:4dc7e56179ff | 81 | * @param vectorB - second vector |
| Sissors | 1:51c902d63dda | 82 | * @param return - vector with which to rotate vectorA to get vectorB |
| Sissors | 0:4dc7e56179ff | 83 | */ |
| Sissors | 0:4dc7e56179ff | 84 | static Vector3 angleBetween(Vector3 vectorA, Vector3 VectorB); |
| Sissors | 1:51c902d63dda | 85 | |
| Sissors | 1:51c902d63dda | 86 | /** |
| Sissors | 1:51c902d63dda | 87 | * Rotate a vector around an axis |
| Sissors | 1:51c902d63dda | 88 | * |
| Sissors | 1:51c902d63dda | 89 | * @param vector - vector to rotate |
| Sissors | 1:51c902d63dda | 90 | * @param axis - axis to rotate it around |
| Sissors | 1:51c902d63dda | 91 | * @param angle - angle to rotate the vectors |
| Sissors | 1:51c902d63dda | 92 | * @param return - resulting vector |
| Sissors | 1:51c902d63dda | 93 | */ |
| Sissors | 1:51c902d63dda | 94 | static Vector3 rotateVector(Vector3 vector, Vector3 axis, float angle); |
| Sissors | 0:4dc7e56179ff | 95 | |
| Sissors | 0:4dc7e56179ff | 96 | |
| Sissors | 1:51c902d63dda | 97 | Vector3 heading; |
| Sissors | 1:51c902d63dda | 98 | Vector3 top; |
| Sissors | 1:51c902d63dda | 99 | Vector3 gyroOffset; |
| Sissors | 1:51c902d63dda | 100 | |
| Sissors | 1:51c902d63dda | 101 | float absGain; |
| Sissors | 1:51c902d63dda | 102 | bool initialRun; |
| Sissors | 1:51c902d63dda | 103 | |
| Sissors | 0:4dc7e56179ff | 104 | }; |
| Sissors | 0:4dc7e56179ff | 105 | |
| Sissors | 0:4dc7e56179ff | 106 | #endif |