Diff: main.cpp

Revision:

0:d1c0b5304b6b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Dec 03 06:08:14 2009 +0000
@@ -0,0 +1,166 @@
+// Tilt compensated compass
+// PNI 11096 based mags and a LIS3LV02 accel
+// This version uses Calibration data stored in a file named cal.txt
+
+#include "mbed.h"
+
+SPI spi(p5,p6,p7);
+Serial pc (USBTX,USBRX);
+LocalFileSystem local("local");
+
+DigitalOut myled(LED1);      // mosi, miso, sclk
+DigitalOut SSnAcc(p8);      // Select Accelerometer
+DigitalOut RstMag(p9);     // reset input to MicroMag3
+DigitalIn  DrdyMag(p10);  // Mag data Ready
+DigitalOut SSnMag(p11);  // Select Mag
+
+#define x 1
+#define y 2
+#define z 3
+#define MagCommand 0x40
+#define pi 3.14159
+#define Rad2Deg 360/(2* pi) 
+
+int main() {
+  float gxOff, gxGain, gyOff, gyGain, gzOff, gzGain;
+  float mxOff, mxGain, myOff, myGain, mzOff, mzGain;
+  
+  pc.baud(115200);
+  SSnMag = 1; SSnAcc=1;   // deselect everything 
+  float mx, my, mz;         // Magnetic field vectors
+  
+  pc.printf("Opening File...\n"); // Drive should be marked as removed
+    FILE *fp = fopen("/local/cal.txt", "r");
+    if(!fp) {
+        fprintf(stderr, "File could not be opened!\n");
+        exit(1);
+    }
+    
+    pc.printf("Reading Cal Data...\n");    
+    fscanf(fp,"%f %f %f %f %f %f ",&gxOff, &gxGain, &gyOff, &gyGain, &gzOff, &gzGain);
+    fscanf(fp,"%f %f %f %f %f %f ",&mxOff, &mxGain, &myOff, &myGain, &mzOff, &mzGain);
+    
+    pc.printf("Closing File.........");
+    fclose(fp);
+    pc.printf("Closed\n");
+
+    pc.printf("\ngxOff:%f  gxGain:%f\ngyOff:%f  gyGain%f\ngzOff:%f  gzGain:%f\n",gxOff, gxGain, gyOff, gyGain, gzOff, gzGain);
+    pc.printf("\nmxOff:%f  mxGain:%f\nmyOff:%f  myGain%f\nmzOff:%f  mzGain:%f\n\n",mxOff, mxGain, myOff, myGain, mzOff, mzGain);
+
+  
+  //spi.frequency(100000);    // no need to go very fast 
+                      // setup the accel  
+  spi.format(8,3);    // the accel expects cpol=1,cpha=1
+  SSnAcc = 0;
+  spi.write(0x20);    // Address the Ctrl_Reg1 register.....
+  spi.write(0x47);    // and set 'active mode bit' and all 3 axes 'enable' bits
+  SSnAcc = 1;
+  wait (0.01);
+    
+  //main loop - continuously read Accel and mag data then process and display
+  while (!pc.readable()) {
+      spi.format(8,3);    // the accel expects cpol=1,cpha=1
+       
+      SSnAcc = 0;        
+      spi.write(0xA9);   // Read raw X data
+      signed char xraw =  spi.write(0x0);
+      SSnAcc = 1;
+
+      SSnAcc = 0;
+      spi.write(0xAB);   // Read raw Y data
+      signed char yraw =  spi.write(0x0);
+      SSnAcc = 1;
+
+      SSnAcc = 0;
+      spi.write(0xAD);   // Read raw Z data
+      signed char zraw = spi.write(0x0);
+      SSnAcc = 1;
+
+      float gx = ((float)xraw-gxOff)*gxGain; // scale and offset using calibration coef
+      float gy = ((float)yraw-gyOff)*gyGain; // 
+      float gz = ((float)zraw-gzOff)*gzGain; // 
+
+      spi.format(8,0);      // the MicroMag3 expects cpol=0, cpha=0
+
+      SSnMag = 0;           // Select MicroMag 3
+
+      RstMag = 1; RstMag = 0;         // Mag reset pulse. this creates ~1.1uS pulse
+      spi.write(MagCommand + x);      // send request for X axis mag value
+      while(!DrdyMag);                // wait for it...
+      mx =spi.write(0)*0x100; mx = mx + spi.write(0); // I could not get the spi.format(16,0) to work 
+                                                      // so I am constructing the word from 2 bytes            
+      if ( mx > 0x7fff)               // convert to signed value
+          mx = mx - 0x10000;
+      mx = (mx - mxOff)*mxGain; 
+         
+      RstMag = 1; RstMag = 0; // get Y axis mag value
+      spi.write(MagCommand + y);
+      while(!DrdyMag);
+      my =spi.write(0)*0x100; my = my + spi.write(0);
+      if ( my > 0x7fff) 
+          my = my - 0x10000; 
+      my = (my - myOff)*myGain; 
+         
+      RstMag = 1; RstMag = 0; //get Z axis mag value
+      spi.write(MagCommand + z);
+      while(!DrdyMag);
+      mz =spi.write(0)*0x100; mz = mz + spi.write(0);
+      if ( mz > 0x7fff) 
+          mz = mz - 0x10000; 
+      mz = (mz - mzOff)*mzGain; 
+
+      SSnMag = 0; // Deselect Mag
+
+// Axis adjustments. This section adjusts some of the Axes to  
+// make the system into a NEU (North East Up) body frame. 
+      mx = -mx;    // makes mx = + when x is pointing due North (body frame point North)
+      my = -my;    // makes my = + when y is pointing due North (body frame pointing East)
+      mz = -mz;    // makes mz = + when z is pointing due North (body frame is up-side-down)
+      gx = -gx;    // makes gx = + when x is pointing down    (body frame is pointing down)
+                   // gy is already + when y is pointing down (body frame's right wing is down)
+      gz = -gz;    // makes gz = + when z is pointing down    (body frame is up-side-down)
+// I'm using the vector method of tilt compensation rather than a cosine matrix. The vector method does not involve 
+// a lot of trig. Basically what is going on is that horizontal plane is constructed using Cross products of g and m.
+// Then by using Dot products, the body coordinate frame is projected onto the newly contructed horizontal XY vectors.
+// for more information search: "A New Solution Algorithm of Magnetic Amizuth", or "A combined electronic compass/clinometer" 
+      float hx = (gz*gz * mx) - (gx*gz*mz) - (gx *gy * my) + (gy*gy * mx);    //horizontal X mag vector
+      float hy = (gy * mz) - (gz * my);                                       //horizontal Y mag vector
+      float heading = atan2(hy,hx)*Rad2Deg;  
+            
+//  {+45}-------{ 0 }-------{-45}  this is the output of atan2(hy,hx)
+//      | +-----[ +x]-----+ |      it must be adjusted to convert it
+//      | |               | |      to navigational directions   
+//      | |               | | 
+//  {+90} [+y]  [0,0]  [-y] {-90} (Note: you get positive y when body frame
+//      | |               | |      faces West in an NEU system because
+//      | |               | |      NEU refers to sensor direction when the
+//      | +-----[ -x]-----+ |      body frame is at rest in its reference or identity orientation)
+// {+135}-----{+/-180}------{-135}
+      if (heading > 0) 
+          heading = 360 - heading;
+      else 
+          heading = -heading;
+//  {335}------{360/0}------{45}  
+//      | +-----[ N ]-----+ |     
+//      | |               | |      navigational directions   
+//      | |               | | 
+//  {270} [W]   [0,0]   [E] {90} 
+//      | |               | |     
+//      | |               | | 
+//      | +------[S]------+ |
+//  {225}-------{180}-------{135}
+      
+      float pitch = atan(-gx/sqrt(gy*gy+gz*gz))*Rad2Deg; //invert gx because +pitch is up. range is +/-90 degrees
+      float roll = atan(gy/sqrt(gx*gx+gz*gz))*Rad2Deg;   // right side down is +roll
+      if (gz > 0)                // unfold atan's limited 2 quadrant space to extend roll to 4 quadrants (+/-180)
+          if ( roll > 0) 
+              roll = 180 - roll;
+          else
+              roll = -180 - roll;
+      
+      pc.printf("\ngx: %f, gy: %f, gz: %f\nmx: %f, my: %f, mz: %f\n",gx,gy,gz,mx,my,mz);
+      pc.printf("hx: %f, hy: %f\n",hx,hy);
+      pc.printf("heading: %f, pitch: %f, roll: %f\n",heading,pitch,roll);
+      wait(1);
+    }
+}