Joe Miller
/
CompassCal
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); } }