Dependencies:   mbed

main.cpp

Committer:
JoeMiller
Date:
2009-12-03
Revision:
0:4969ef3a1629

File content as of revision 0:4969ef3a1629:

#include "mbed.h"
LocalFileSystem local("local");

SPI spi(p5,p6,p7);
Serial pc (USBTX,USBRX);

DigitalOut xLED(LED1);      // mosi, miso, sclk
DigitalOut yLED(LED2);      // mosi, miso, sclk
DigitalOut zLED(LED3);      // 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

int main() {
  xLED = yLED = zLED = 0;
  pc.baud(115200);
  SSnMag = 1; SSnAcc=1;   // deselect everything 
  int mx, my, mz;         // Magnetic field vectors
  float mxH = 0.0, mxL=0.0, myH = 0.0, myL = 0.0, mzH = 0.0, mzL = 0.0;
  signed char gxH = 0, gxL = 0, gyH = 0, gyL = 0, gzH = 0, gzL = 0;
  float mxOff, mxGain, myOff, myGain, mzOff, mzGain;
  float gxOff, gxGain, gyOff, gyGain, gzOff, gzGain;
  
  //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 (1) {
      spi.format(8,3);    // the accel expects cpol=1,cpha=1

//----- CAL gz      
      pc.printf("\n\r\n\rLay unit flat right side up then press anykey");
      while (pc.getc() == NULL);
      
      SSnAcc = 0;
      spi.write(0xAD);   // Read raw Z data
      gzH = spi.write(0x0);
      SSnAcc = 1;
      zLED = 1;
      wait(1.0);
      pc.printf("\n\rLay unit flat up-side DOWN then press anykey");
      while (pc.getc() == NULL);
      
      SSnAcc = 0;
      spi.write(0xAD);   // Read raw Z data
      gzL = spi.write(0x0);
      SSnAcc = 1;
      zLED = 0;
      
      wait(1.0);
      pc.printf("\n\rPlace unit on vertical surface, press any key, rotate slowly 360-deg\n\r  then press any key again when complete");
      while (pc.getc() == NULL);
      wait(1.0);
  
      while (!pc.readable()) {
          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;
          
          if (xraw > gxH) {gxH = xraw; xLED = !xLED;}
          if (xraw < gxL) {gxL = xraw; xLED = !xLED;}
          if (yraw > gyH) {gyH = yraw; yLED = !yLED;}
          if (yraw < gyL) {gyL = yraw; yLED = !yLED;}          
      }
      pc.getc(); // remove character from buffer
      
      gxOff = (float)(gxH + gxL)/2.0; gxGain = 2.0/(float)(gxH - gxL);
      gyOff = (float)(gyH + gyL)/2.0; gyGain = 2.0/(float)(gyH - gyL);
      gzOff = (float)(gzH + gzL)/2.0; gzGain = 2.0/(float)(gzH - gzL);
      
      pc.printf("\n\rgxOff:%f  gxGain:%f\n\rgyOff:%f  gyGain%f\n\rgzOff:%f  gzGain:%f\n\r",gxOff, gxGain, gyOff, gyGain, gzOff, gzGain);
      
      spi.format(8,0);      // the MicroMag3 expects cpol=0, cpha=0

      SSnMag = 0;           // Select MicroMag 3

      wait(1.0);
      pc.printf("\n\rPress any key to GO\n\r");
      pc.printf("Then -Wave unit around to find min and max mag values for all three axes\n\r");
      pc.printf("      LED1-3 will toggle when new high/low values for X,Y,Z respectively are found\n\r");
      pc.printf("      press any key to stop when you cannont make anymore lights blink\n\r");
      
      while (pc.getc() == NULL);
      
          // Throw out first reading - sometimes it is bad
          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 

      while(!pc.readable()) {
          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; 
         
          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; 
         
          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; 
          
          if (mx > mxH) { mxH = mx; if(xLED ==1)xLED=0; else xLED = 1;}    
          if (mx < mxL) { mxL = mx; if(xLED ==1)xLED=0; else xLED = 1;}    
          if (my > myH) { myH = my; if(yLED ==1)yLED=0; else yLED = 1;}    
          if (my < myL) { myL = my; if(yLED ==1)yLED=0; else yLED = 1;}    
          if (mz > mzH) { mzH = mz; if(zLED ==1)zLED=0; else zLED = 1;}    
          if (mz < mzL) { mzL = mz; if(zLED ==1)zLED=0; else zLED = 1;}    
              
      }        
      pc.getc(); // remove character from buffer

      SSnMag = 1; // Deselect Mag

      mxOff = (mxH + mxL)/2; mxGain = 1/(mxH - mxL);
      myOff = (myH + myL)/2; myGain = 1/(myH - myL);
      mzOff = (mzH + mzL)/2; mzGain = 1/(mzH - mzL);
      
      pc.printf("\rmxOff:%f  mxGain:%f\rmyOff:%f  myGain%f\rmzOff:%f  mzGain:%f\r\n\n",mxOff, mxGain, myOff, myGain, mzOff, mzGain);


    pc.printf("Opening File...\n\r"); // Drive should be marked as removed
    FILE *fp = fopen("/local/cal.txt", "w");
    if(!fp) {
        fprintf(stderr, "File /local/test.txt could not be opened!\n");
        exit(1);
    }
    
    pc.printf("Writing Data...\n\r");    
    fprintf(fp,"%f %f %f %f %f %f ",gxOff, gxGain, gyOff, gyGain, gzOff, gzGain);
    fprintf(fp,"%f %f %f %f %f %f ",mxOff, mxGain, myOff, myGain, mzOff, mzGain);
    fprintf(fp,"\ngxH = %d, gxL=%d,\n gyH = %d, gyL = %d,\n gzH = %d, gzL = %d\n",gxH,gxL,gyH,gyL,gzH,gzL);
    fprintf(fp,"\nmxH = %f, mxL=%f,\n myH = %f, myL = %f,\n mzH = %f, mzL = %f\n",mxH,mxL,myH,myL,mzH,mzL);
    
    pc.printf("Closing File...\n\r");
    fclose(fp);

    pc.printf("\n\rgxH = %d, gxL=%d,\r gyH = %d, gyL = %d,\r gzH = %d, gzL = %d\n\r",gxH,gxL,gyH,gyL,gzH,gzL);
    pc.printf("\n\rmxH = %f, mxL=%f,\r myH = %f, myL = %f,\r mzH = %f, mzL = %f\n\r",mxH,mxL,myH,myL,mzH,mzL);



    exit(0); 
}

}