main.cpp

00001 // Tilt compensated compass
00002 // PNI 11096 based mags and a LIS3LV02 accel
00003 // This version uses Calibration data stored in a file named cal.txt
00004 
00005 #include "mbed.h"
00006 
00007 SPI spi(p5,p6,p7);
00008 Serial pc (USBTX,USBRX);
00009 LocalFileSystem local("local");
00010 
00011 DigitalOut myled(LED1);      // mosi, miso, sclk
00012 DigitalOut SSnAcc(p8);      // Select Accelerometer
00013 DigitalOut RstMag(p9);     // reset input to MicroMag3
00014 DigitalIn  DrdyMag(p10);  // Mag data Ready
00015 DigitalOut SSnMag(p11);  // Select Mag
00016 
00017 #define x 1
00018 #define y 2
00019 #define z 3
00020 #define MagCommand 0x40
00021 #define pi 3.14159
00022 #define Rad2Deg 360/(2* pi) 
00023 
00024 int main() {
00025   float gxOff, gxGain, gyOff, gyGain, gzOff, gzGain;
00026   float mxOff, mxGain, myOff, myGain, mzOff, mzGain;
00027   
00028   pc.baud(115200);
00029   SSnMag = 1; SSnAcc=1;   // deselect everything 
00030   float mx, my, mz;         // Magnetic field vectors
00031   
00032   pc.printf("Opening File...\n"); // Drive should be marked as removed
00033     FILE *fp = fopen("/local/cal.txt", "r");
00034     if(!fp) {
00035         fprintf(stderr, "File could not be opened!\n");
00036         exit(1);
00037     }
00038     
00039     pc.printf("Reading Cal Data...\n");    
00040     fscanf(fp,"%f %f %f %f %f %f ",&gxOff, &gxGain, &gyOff, &gyGain, &gzOff, &gzGain);
00041     fscanf(fp,"%f %f %f %f %f %f ",&mxOff, &mxGain, &myOff, &myGain, &mzOff, &mzGain);
00042     
00043     pc.printf("Closing File.........");
00044     fclose(fp);
00045     pc.printf("Closed\n");
00046 
00047     pc.printf("\ngxOff:%f  gxGain:%f\ngyOff:%f  gyGain%f\ngzOff:%f  gzGain:%f\n",gxOff, gxGain, gyOff, gyGain, gzOff, gzGain);
00048     pc.printf("\nmxOff:%f  mxGain:%f\nmyOff:%f  myGain%f\nmzOff:%f  mzGain:%f\n\n",mxOff, mxGain, myOff, myGain, mzOff, mzGain);
00049 
00050   
00051   //spi.frequency(100000);    // no need to go very fast 
00052                       // setup the accel  
00053   spi.format(8,3);    // the accel expects cpol=1,cpha=1
00054   SSnAcc = 0;
00055   spi.write(0x20);    // Address the Ctrl_Reg1 register.....
00056   spi.write(0x47);    // and set 'active mode bit' and all 3 axes 'enable' bits
00057   SSnAcc = 1;
00058   wait (0.01);
00059     
00060   //main loop - continuously read Accel and mag data then process and display
00061   while (!pc.readable()) {
00062       spi.format(8,3);    // the accel expects cpol=1,cpha=1
00063        
00064       SSnAcc = 0;        
00065       spi.write(0xA9);   // Read raw X data
00066       signed char xraw =  spi.write(0x0);
00067       SSnAcc = 1;
00068 
00069       SSnAcc = 0;
00070       spi.write(0xAB);   // Read raw Y data
00071       signed char yraw =  spi.write(0x0);
00072       SSnAcc = 1;
00073 
00074       SSnAcc = 0;
00075       spi.write(0xAD);   // Read raw Z data
00076       signed char zraw = spi.write(0x0);
00077       SSnAcc = 1;
00078 
00079       float gx = ((float)xraw-gxOff)*gxGain; // scale and offset using calibration coef
00080       float gy = ((float)yraw-gyOff)*gyGain; // 
00081       float gz = ((float)zraw-gzOff)*gzGain; // 
00082 
00083       spi.format(8,0);      // the MicroMag3 expects cpol=0, cpha=0
00084 
00085       SSnMag = 0;           // Select MicroMag 3
00086 
00087       RstMag = 1; RstMag = 0;         // Mag reset pulse. this creates ~1.1uS pulse
00088       spi.write(MagCommand + x);      // send request for X axis mag value
00089       while(!DrdyMag);                // wait for it...
00090       mx =spi.write(0)*0x100; mx = mx + spi.write(0); // I could not get the spi.format(16,0) to work 
00091                                                       // so I am constructing the word from 2 bytes            
00092       if ( mx > 0x7fff)               // convert to signed value
00093           mx = mx - 0x10000;
00094       mx = (mx - mxOff)*mxGain; 
00095          
00096       RstMag = 1; RstMag = 0; // get Y axis mag value
00097       spi.write(MagCommand + y);
00098       while(!DrdyMag);
00099       my =spi.write(0)*0x100; my = my + spi.write(0);
00100       if ( my > 0x7fff) 
00101           my = my - 0x10000; 
00102       my = (my - myOff)*myGain; 
00103          
00104       RstMag = 1; RstMag = 0; //get Z axis mag value
00105       spi.write(MagCommand + z);
00106       while(!DrdyMag);
00107       mz =spi.write(0)*0x100; mz = mz + spi.write(0);
00108       if ( mz > 0x7fff) 
00109           mz = mz - 0x10000; 
00110       mz = (mz - mzOff)*mzGain; 
00111 
00112       SSnMag = 0; // Deselect Mag
00113 
00114 // Axis adjustments. This section adjusts some of the Axes to  
00115 // make the system into a NEU (North East Up) body frame. 
00116       mx = -mx;    // makes mx = + when x is pointing due North (body frame point North)
00117       my = -my;    // makes my = + when y is pointing due North (body frame pointing East)
00118       mz = -mz;    // makes mz = + when z is pointing due North (body frame is up-side-down)
00119       gx = -gx;    // makes gx = + when x is pointing down    (body frame is pointing down)
00120                    // gy is already + when y is pointing down (body frame's right wing is down)
00121       gz = -gz;    // makes gz = + when z is pointing down    (body frame is up-side-down)
00122 // I'm using the vector method of tilt compensation rather than a cosine matrix. The vector method does not involve 
00123 // a lot of trig. Basically what is going on is that horizontal plane is constructed using Cross products of g and m.
00124 // Then by using Dot products, the body coordinate frame is projected onto the newly contructed horizontal XY vectors.
00125 // for more information search: "A New Solution Algorithm of Magnetic Amizuth", or "A combined electronic compass/clinometer" 
00126       float hx = (gz*gz * mx) - (gx*gz*mz) - (gx *gy * my) + (gy*gy * mx);    //horizontal X mag vector
00127       float hy = (gy * mz) - (gz * my);                                       //horizontal Y mag vector
00128       float heading = atan2(hy,hx)*Rad2Deg;  
00129             
00130 //  {+45}-------{ 0 }-------{-45}  this is the output of atan2(hy,hx)
00131 //      | +-----[ +x]-----+ |      it must be adjusted to convert it
00132 //      | |               | |      to navigational directions   
00133 //      | |               | | 
00134 //  {+90} [+y]  [0,0]  [-y] {-90} (Note: you get positive y when body frame
00135 //      | |               | |      faces West in an NEU system because
00136 //      | |               | |      NEU refers to sensor direction when the
00137 //      | +-----[ -x]-----+ |      body frame is at rest in its reference or identity orientation)
00138 // {+135}-----{+/-180}------{-135}
00139       if (heading > 0) 
00140           heading = 360 - heading;
00141       else 
00142           heading = -heading;
00143 //  {335}------{360/0}------{45}  
00144 //      | +-----[ N ]-----+ |     
00145 //      | |               | |      navigational directions   
00146 //      | |               | | 
00147 //  {270} [W]   [0,0]   [E] {90} 
00148 //      | |               | |     
00149 //      | |               | | 
00150 //      | +------[S]------+ |
00151 //  {225}-------{180}-------{135}
00152       
00153       float pitch = atan(-gx/sqrt(gy*gy+gz*gz))*Rad2Deg; //invert gx because +pitch is up. range is +/-90 degrees
00154       float roll = atan(gy/sqrt(gx*gx+gz*gz))*Rad2Deg;   // right side down is +roll
00155       if (gz > 0)                // unfold atan's limited 2 quadrant space to extend roll to 4 quadrants (+/-180)
00156           if ( roll > 0) 
00157               roll = 180 - roll;
00158           else
00159               roll = -180 - roll;
00160       
00161       pc.printf("\ngx: %f, gy: %f, gz: %f\nmx: %f, my: %f, mz: %f\n",gx,gy,gz,mx,my,mz);
00162       pc.printf("hx: %f, hy: %f\n",hx,hy);
00163       pc.printf("heading: %f, pitch: %f, roll: %f\n",heading,pitch,roll);
00164       wait(1);
00165     }
00166 }