![](/media/cache/profiles/a733fa9b25f33689e2adbe72199f0e62.jpg.50x50_q85.jpg)
Tilt Compensated Compass using 3-axis magnetometer and 3-axis accelerometer.
main.cpp@0:d1c0b5304b6b, 2009-12-03 (annotated)
- Committer:
- JoeMiller
- Date:
- Thu Dec 03 06:08:14 2009 +0000
- Revision:
- 0:d1c0b5304b6b
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JoeMiller | 0:d1c0b5304b6b | 1 | // Tilt compensated compass |
JoeMiller | 0:d1c0b5304b6b | 2 | // PNI 11096 based mags and a LIS3LV02 accel |
JoeMiller | 0:d1c0b5304b6b | 3 | // This version uses Calibration data stored in a file named cal.txt |
JoeMiller | 0:d1c0b5304b6b | 4 | |
JoeMiller | 0:d1c0b5304b6b | 5 | #include "mbed.h" |
JoeMiller | 0:d1c0b5304b6b | 6 | |
JoeMiller | 0:d1c0b5304b6b | 7 | SPI spi(p5,p6,p7); |
JoeMiller | 0:d1c0b5304b6b | 8 | Serial pc (USBTX,USBRX); |
JoeMiller | 0:d1c0b5304b6b | 9 | LocalFileSystem local("local"); |
JoeMiller | 0:d1c0b5304b6b | 10 | |
JoeMiller | 0:d1c0b5304b6b | 11 | DigitalOut myled(LED1); // mosi, miso, sclk |
JoeMiller | 0:d1c0b5304b6b | 12 | DigitalOut SSnAcc(p8); // Select Accelerometer |
JoeMiller | 0:d1c0b5304b6b | 13 | DigitalOut RstMag(p9); // reset input to MicroMag3 |
JoeMiller | 0:d1c0b5304b6b | 14 | DigitalIn DrdyMag(p10); // Mag data Ready |
JoeMiller | 0:d1c0b5304b6b | 15 | DigitalOut SSnMag(p11); // Select Mag |
JoeMiller | 0:d1c0b5304b6b | 16 | |
JoeMiller | 0:d1c0b5304b6b | 17 | #define x 1 |
JoeMiller | 0:d1c0b5304b6b | 18 | #define y 2 |
JoeMiller | 0:d1c0b5304b6b | 19 | #define z 3 |
JoeMiller | 0:d1c0b5304b6b | 20 | #define MagCommand 0x40 |
JoeMiller | 0:d1c0b5304b6b | 21 | #define pi 3.14159 |
JoeMiller | 0:d1c0b5304b6b | 22 | #define Rad2Deg 360/(2* pi) |
JoeMiller | 0:d1c0b5304b6b | 23 | |
JoeMiller | 0:d1c0b5304b6b | 24 | int main() { |
JoeMiller | 0:d1c0b5304b6b | 25 | float gxOff, gxGain, gyOff, gyGain, gzOff, gzGain; |
JoeMiller | 0:d1c0b5304b6b | 26 | float mxOff, mxGain, myOff, myGain, mzOff, mzGain; |
JoeMiller | 0:d1c0b5304b6b | 27 | |
JoeMiller | 0:d1c0b5304b6b | 28 | pc.baud(115200); |
JoeMiller | 0:d1c0b5304b6b | 29 | SSnMag = 1; SSnAcc=1; // deselect everything |
JoeMiller | 0:d1c0b5304b6b | 30 | float mx, my, mz; // Magnetic field vectors |
JoeMiller | 0:d1c0b5304b6b | 31 | |
JoeMiller | 0:d1c0b5304b6b | 32 | pc.printf("Opening File...\n"); // Drive should be marked as removed |
JoeMiller | 0:d1c0b5304b6b | 33 | FILE *fp = fopen("/local/cal.txt", "r"); |
JoeMiller | 0:d1c0b5304b6b | 34 | if(!fp) { |
JoeMiller | 0:d1c0b5304b6b | 35 | fprintf(stderr, "File could not be opened!\n"); |
JoeMiller | 0:d1c0b5304b6b | 36 | exit(1); |
JoeMiller | 0:d1c0b5304b6b | 37 | } |
JoeMiller | 0:d1c0b5304b6b | 38 | |
JoeMiller | 0:d1c0b5304b6b | 39 | pc.printf("Reading Cal Data...\n"); |
JoeMiller | 0:d1c0b5304b6b | 40 | fscanf(fp,"%f %f %f %f %f %f ",&gxOff, &gxGain, &gyOff, &gyGain, &gzOff, &gzGain); |
JoeMiller | 0:d1c0b5304b6b | 41 | fscanf(fp,"%f %f %f %f %f %f ",&mxOff, &mxGain, &myOff, &myGain, &mzOff, &mzGain); |
JoeMiller | 0:d1c0b5304b6b | 42 | |
JoeMiller | 0:d1c0b5304b6b | 43 | pc.printf("Closing File........."); |
JoeMiller | 0:d1c0b5304b6b | 44 | fclose(fp); |
JoeMiller | 0:d1c0b5304b6b | 45 | pc.printf("Closed\n"); |
JoeMiller | 0:d1c0b5304b6b | 46 | |
JoeMiller | 0:d1c0b5304b6b | 47 | pc.printf("\ngxOff:%f gxGain:%f\ngyOff:%f gyGain%f\ngzOff:%f gzGain:%f\n",gxOff, gxGain, gyOff, gyGain, gzOff, gzGain); |
JoeMiller | 0:d1c0b5304b6b | 48 | pc.printf("\nmxOff:%f mxGain:%f\nmyOff:%f myGain%f\nmzOff:%f mzGain:%f\n\n",mxOff, mxGain, myOff, myGain, mzOff, mzGain); |
JoeMiller | 0:d1c0b5304b6b | 49 | |
JoeMiller | 0:d1c0b5304b6b | 50 | |
JoeMiller | 0:d1c0b5304b6b | 51 | //spi.frequency(100000); // no need to go very fast |
JoeMiller | 0:d1c0b5304b6b | 52 | // setup the accel |
JoeMiller | 0:d1c0b5304b6b | 53 | spi.format(8,3); // the accel expects cpol=1,cpha=1 |
JoeMiller | 0:d1c0b5304b6b | 54 | SSnAcc = 0; |
JoeMiller | 0:d1c0b5304b6b | 55 | spi.write(0x20); // Address the Ctrl_Reg1 register..... |
JoeMiller | 0:d1c0b5304b6b | 56 | spi.write(0x47); // and set 'active mode bit' and all 3 axes 'enable' bits |
JoeMiller | 0:d1c0b5304b6b | 57 | SSnAcc = 1; |
JoeMiller | 0:d1c0b5304b6b | 58 | wait (0.01); |
JoeMiller | 0:d1c0b5304b6b | 59 | |
JoeMiller | 0:d1c0b5304b6b | 60 | //main loop - continuously read Accel and mag data then process and display |
JoeMiller | 0:d1c0b5304b6b | 61 | while (!pc.readable()) { |
JoeMiller | 0:d1c0b5304b6b | 62 | spi.format(8,3); // the accel expects cpol=1,cpha=1 |
JoeMiller | 0:d1c0b5304b6b | 63 | |
JoeMiller | 0:d1c0b5304b6b | 64 | SSnAcc = 0; |
JoeMiller | 0:d1c0b5304b6b | 65 | spi.write(0xA9); // Read raw X data |
JoeMiller | 0:d1c0b5304b6b | 66 | signed char xraw = spi.write(0x0); |
JoeMiller | 0:d1c0b5304b6b | 67 | SSnAcc = 1; |
JoeMiller | 0:d1c0b5304b6b | 68 | |
JoeMiller | 0:d1c0b5304b6b | 69 | SSnAcc = 0; |
JoeMiller | 0:d1c0b5304b6b | 70 | spi.write(0xAB); // Read raw Y data |
JoeMiller | 0:d1c0b5304b6b | 71 | signed char yraw = spi.write(0x0); |
JoeMiller | 0:d1c0b5304b6b | 72 | SSnAcc = 1; |
JoeMiller | 0:d1c0b5304b6b | 73 | |
JoeMiller | 0:d1c0b5304b6b | 74 | SSnAcc = 0; |
JoeMiller | 0:d1c0b5304b6b | 75 | spi.write(0xAD); // Read raw Z data |
JoeMiller | 0:d1c0b5304b6b | 76 | signed char zraw = spi.write(0x0); |
JoeMiller | 0:d1c0b5304b6b | 77 | SSnAcc = 1; |
JoeMiller | 0:d1c0b5304b6b | 78 | |
JoeMiller | 0:d1c0b5304b6b | 79 | float gx = ((float)xraw-gxOff)*gxGain; // scale and offset using calibration coef |
JoeMiller | 0:d1c0b5304b6b | 80 | float gy = ((float)yraw-gyOff)*gyGain; // |
JoeMiller | 0:d1c0b5304b6b | 81 | float gz = ((float)zraw-gzOff)*gzGain; // |
JoeMiller | 0:d1c0b5304b6b | 82 | |
JoeMiller | 0:d1c0b5304b6b | 83 | spi.format(8,0); // the MicroMag3 expects cpol=0, cpha=0 |
JoeMiller | 0:d1c0b5304b6b | 84 | |
JoeMiller | 0:d1c0b5304b6b | 85 | SSnMag = 0; // Select MicroMag 3 |
JoeMiller | 0:d1c0b5304b6b | 86 | |
JoeMiller | 0:d1c0b5304b6b | 87 | RstMag = 1; RstMag = 0; // Mag reset pulse. this creates ~1.1uS pulse |
JoeMiller | 0:d1c0b5304b6b | 88 | spi.write(MagCommand + x); // send request for X axis mag value |
JoeMiller | 0:d1c0b5304b6b | 89 | while(!DrdyMag); // wait for it... |
JoeMiller | 0:d1c0b5304b6b | 90 | mx =spi.write(0)*0x100; mx = mx + spi.write(0); // I could not get the spi.format(16,0) to work |
JoeMiller | 0:d1c0b5304b6b | 91 | // so I am constructing the word from 2 bytes |
JoeMiller | 0:d1c0b5304b6b | 92 | if ( mx > 0x7fff) // convert to signed value |
JoeMiller | 0:d1c0b5304b6b | 93 | mx = mx - 0x10000; |
JoeMiller | 0:d1c0b5304b6b | 94 | mx = (mx - mxOff)*mxGain; |
JoeMiller | 0:d1c0b5304b6b | 95 | |
JoeMiller | 0:d1c0b5304b6b | 96 | RstMag = 1; RstMag = 0; // get Y axis mag value |
JoeMiller | 0:d1c0b5304b6b | 97 | spi.write(MagCommand + y); |
JoeMiller | 0:d1c0b5304b6b | 98 | while(!DrdyMag); |
JoeMiller | 0:d1c0b5304b6b | 99 | my =spi.write(0)*0x100; my = my + spi.write(0); |
JoeMiller | 0:d1c0b5304b6b | 100 | if ( my > 0x7fff) |
JoeMiller | 0:d1c0b5304b6b | 101 | my = my - 0x10000; |
JoeMiller | 0:d1c0b5304b6b | 102 | my = (my - myOff)*myGain; |
JoeMiller | 0:d1c0b5304b6b | 103 | |
JoeMiller | 0:d1c0b5304b6b | 104 | RstMag = 1; RstMag = 0; //get Z axis mag value |
JoeMiller | 0:d1c0b5304b6b | 105 | spi.write(MagCommand + z); |
JoeMiller | 0:d1c0b5304b6b | 106 | while(!DrdyMag); |
JoeMiller | 0:d1c0b5304b6b | 107 | mz =spi.write(0)*0x100; mz = mz + spi.write(0); |
JoeMiller | 0:d1c0b5304b6b | 108 | if ( mz > 0x7fff) |
JoeMiller | 0:d1c0b5304b6b | 109 | mz = mz - 0x10000; |
JoeMiller | 0:d1c0b5304b6b | 110 | mz = (mz - mzOff)*mzGain; |
JoeMiller | 0:d1c0b5304b6b | 111 | |
JoeMiller | 0:d1c0b5304b6b | 112 | SSnMag = 0; // Deselect Mag |
JoeMiller | 0:d1c0b5304b6b | 113 | |
JoeMiller | 0:d1c0b5304b6b | 114 | // Axis adjustments. This section adjusts some of the Axes to |
JoeMiller | 0:d1c0b5304b6b | 115 | // make the system into a NEU (North East Up) body frame. |
JoeMiller | 0:d1c0b5304b6b | 116 | mx = -mx; // makes mx = + when x is pointing due North (body frame point North) |
JoeMiller | 0:d1c0b5304b6b | 117 | my = -my; // makes my = + when y is pointing due North (body frame pointing East) |
JoeMiller | 0:d1c0b5304b6b | 118 | mz = -mz; // makes mz = + when z is pointing due North (body frame is up-side-down) |
JoeMiller | 0:d1c0b5304b6b | 119 | gx = -gx; // makes gx = + when x is pointing down (body frame is pointing down) |
JoeMiller | 0:d1c0b5304b6b | 120 | // gy is already + when y is pointing down (body frame's right wing is down) |
JoeMiller | 0:d1c0b5304b6b | 121 | gz = -gz; // makes gz = + when z is pointing down (body frame is up-side-down) |
JoeMiller | 0:d1c0b5304b6b | 122 | // I'm using the vector method of tilt compensation rather than a cosine matrix. The vector method does not involve |
JoeMiller | 0:d1c0b5304b6b | 123 | // a lot of trig. Basically what is going on is that horizontal plane is constructed using Cross products of g and m. |
JoeMiller | 0:d1c0b5304b6b | 124 | // Then by using Dot products, the body coordinate frame is projected onto the newly contructed horizontal XY vectors. |
JoeMiller | 0:d1c0b5304b6b | 125 | // for more information search: "A New Solution Algorithm of Magnetic Amizuth", or "A combined electronic compass/clinometer" |
JoeMiller | 0:d1c0b5304b6b | 126 | float hx = (gz*gz * mx) - (gx*gz*mz) - (gx *gy * my) + (gy*gy * mx); //horizontal X mag vector |
JoeMiller | 0:d1c0b5304b6b | 127 | float hy = (gy * mz) - (gz * my); //horizontal Y mag vector |
JoeMiller | 0:d1c0b5304b6b | 128 | float heading = atan2(hy,hx)*Rad2Deg; |
JoeMiller | 0:d1c0b5304b6b | 129 | |
JoeMiller | 0:d1c0b5304b6b | 130 | // {+45}-------{ 0 }-------{-45} this is the output of atan2(hy,hx) |
JoeMiller | 0:d1c0b5304b6b | 131 | // | +-----[ +x]-----+ | it must be adjusted to convert it |
JoeMiller | 0:d1c0b5304b6b | 132 | // | | | | to navigational directions |
JoeMiller | 0:d1c0b5304b6b | 133 | // | | | | |
JoeMiller | 0:d1c0b5304b6b | 134 | // {+90} [+y] [0,0] [-y] {-90} (Note: you get positive y when body frame |
JoeMiller | 0:d1c0b5304b6b | 135 | // | | | | faces West in an NEU system because |
JoeMiller | 0:d1c0b5304b6b | 136 | // | | | | NEU refers to sensor direction when the |
JoeMiller | 0:d1c0b5304b6b | 137 | // | +-----[ -x]-----+ | body frame is at rest in its reference or identity orientation) |
JoeMiller | 0:d1c0b5304b6b | 138 | // {+135}-----{+/-180}------{-135} |
JoeMiller | 0:d1c0b5304b6b | 139 | if (heading > 0) |
JoeMiller | 0:d1c0b5304b6b | 140 | heading = 360 - heading; |
JoeMiller | 0:d1c0b5304b6b | 141 | else |
JoeMiller | 0:d1c0b5304b6b | 142 | heading = -heading; |
JoeMiller | 0:d1c0b5304b6b | 143 | // {335}------{360/0}------{45} |
JoeMiller | 0:d1c0b5304b6b | 144 | // | +-----[ N ]-----+ | |
JoeMiller | 0:d1c0b5304b6b | 145 | // | | | | navigational directions |
JoeMiller | 0:d1c0b5304b6b | 146 | // | | | | |
JoeMiller | 0:d1c0b5304b6b | 147 | // {270} [W] [0,0] [E] {90} |
JoeMiller | 0:d1c0b5304b6b | 148 | // | | | | |
JoeMiller | 0:d1c0b5304b6b | 149 | // | | | | |
JoeMiller | 0:d1c0b5304b6b | 150 | // | +------[S]------+ | |
JoeMiller | 0:d1c0b5304b6b | 151 | // {225}-------{180}-------{135} |
JoeMiller | 0:d1c0b5304b6b | 152 | |
JoeMiller | 0:d1c0b5304b6b | 153 | float pitch = atan(-gx/sqrt(gy*gy+gz*gz))*Rad2Deg; //invert gx because +pitch is up. range is +/-90 degrees |
JoeMiller | 0:d1c0b5304b6b | 154 | float roll = atan(gy/sqrt(gx*gx+gz*gz))*Rad2Deg; // right side down is +roll |
JoeMiller | 0:d1c0b5304b6b | 155 | if (gz > 0) // unfold atan's limited 2 quadrant space to extend roll to 4 quadrants (+/-180) |
JoeMiller | 0:d1c0b5304b6b | 156 | if ( roll > 0) |
JoeMiller | 0:d1c0b5304b6b | 157 | roll = 180 - roll; |
JoeMiller | 0:d1c0b5304b6b | 158 | else |
JoeMiller | 0:d1c0b5304b6b | 159 | roll = -180 - roll; |
JoeMiller | 0:d1c0b5304b6b | 160 | |
JoeMiller | 0:d1c0b5304b6b | 161 | pc.printf("\ngx: %f, gy: %f, gz: %f\nmx: %f, my: %f, mz: %f\n",gx,gy,gz,mx,my,mz); |
JoeMiller | 0:d1c0b5304b6b | 162 | pc.printf("hx: %f, hy: %f\n",hx,hy); |
JoeMiller | 0:d1c0b5304b6b | 163 | pc.printf("heading: %f, pitch: %f, roll: %f\n",heading,pitch,roll); |
JoeMiller | 0:d1c0b5304b6b | 164 | wait(1); |
JoeMiller | 0:d1c0b5304b6b | 165 | } |
JoeMiller | 0:d1c0b5304b6b | 166 | } |