CompassCal -

Users » JoeMiller » Code » CompassCal

Joe Miller / Mbed 2 deprecated CompassCal

main.cpp@0:4969ef3a1629, 2009-12-03 (annotated)

Committer:: JoeMiller
Date:: Thu Dec 03 06:08:40 2009 +0000
Revision:: 0:4969ef3a1629

Who changed what in which revision?

User	Revision	Line number	New contents of line
JoeMiller	0:4969ef3a1629	1	#include "mbed.h"
JoeMiller	0:4969ef3a1629	2	LocalFileSystem local("local");
JoeMiller	0:4969ef3a1629	3
JoeMiller	0:4969ef3a1629	4	SPI spi(p5,p6,p7);
JoeMiller	0:4969ef3a1629	5	Serial pc (USBTX,USBRX);
JoeMiller	0:4969ef3a1629	6
JoeMiller	0:4969ef3a1629	7	DigitalOut xLED(LED1); // mosi, miso, sclk
JoeMiller	0:4969ef3a1629	8	DigitalOut yLED(LED2); // mosi, miso, sclk
JoeMiller	0:4969ef3a1629	9	DigitalOut zLED(LED3); // mosi, miso, sclk
JoeMiller	0:4969ef3a1629	10	DigitalOut SSnAcc(p8); // Select Accelerometer
JoeMiller	0:4969ef3a1629	11	DigitalOut RstMag(p9); // reset input to MicroMag3
JoeMiller	0:4969ef3a1629	12	DigitalIn DrdyMag(p10); // Mag data Ready
JoeMiller	0:4969ef3a1629	13	DigitalOut SSnMag(p11); // Select Mag
JoeMiller	0:4969ef3a1629	14
JoeMiller	0:4969ef3a1629	15	#define x 1
JoeMiller	0:4969ef3a1629	16	#define y 2
JoeMiller	0:4969ef3a1629	17	#define z 3
JoeMiller	0:4969ef3a1629	18	#define MagCommand 0x40
JoeMiller	0:4969ef3a1629	19
JoeMiller	0:4969ef3a1629	20	int main() {
JoeMiller	0:4969ef3a1629	21	xLED = yLED = zLED = 0;
JoeMiller	0:4969ef3a1629	22	pc.baud(115200);
JoeMiller	0:4969ef3a1629	23	SSnMag = 1; SSnAcc=1; // deselect everything
JoeMiller	0:4969ef3a1629	24	int mx, my, mz; // Magnetic field vectors
JoeMiller	0:4969ef3a1629	25	float mxH = 0.0, mxL=0.0, myH = 0.0, myL = 0.0, mzH = 0.0, mzL = 0.0;
JoeMiller	0:4969ef3a1629	26	signed char gxH = 0, gxL = 0, gyH = 0, gyL = 0, gzH = 0, gzL = 0;
JoeMiller	0:4969ef3a1629	27	float mxOff, mxGain, myOff, myGain, mzOff, mzGain;
JoeMiller	0:4969ef3a1629	28	float gxOff, gxGain, gyOff, gyGain, gzOff, gzGain;
JoeMiller	0:4969ef3a1629	29
JoeMiller	0:4969ef3a1629	30	//spi.frequency(100000); // no need to go very fast
JoeMiller	0:4969ef3a1629	31	// setup the accel
JoeMiller	0:4969ef3a1629	32	spi.format(8,3); // the accel expects cpol=1,cpha=1
JoeMiller	0:4969ef3a1629	33	SSnAcc = 0;
JoeMiller	0:4969ef3a1629	34	spi.write(0x20); // Address the Ctrl_Reg1 register.....
JoeMiller	0:4969ef3a1629	35	spi.write(0x47); // and set 'active mode bit' and all 3 axes 'enable' bits
JoeMiller	0:4969ef3a1629	36	SSnAcc = 1;
JoeMiller	0:4969ef3a1629	37	wait (0.01);
JoeMiller	0:4969ef3a1629	38
JoeMiller	0:4969ef3a1629	39	//main loop - continuously read Accel and mag data then process and display
JoeMiller	0:4969ef3a1629	40	while (1) {
JoeMiller	0:4969ef3a1629	41	spi.format(8,3); // the accel expects cpol=1,cpha=1
JoeMiller	0:4969ef3a1629	42
JoeMiller	0:4969ef3a1629	43	//----- CAL gz
JoeMiller	0:4969ef3a1629	44	pc.printf("\n\r\n\rLay unit flat right side up then press anykey");
JoeMiller	0:4969ef3a1629	45	while (pc.getc() == NULL);
JoeMiller	0:4969ef3a1629	46
JoeMiller	0:4969ef3a1629	47	SSnAcc = 0;
JoeMiller	0:4969ef3a1629	48	spi.write(0xAD); // Read raw Z data
JoeMiller	0:4969ef3a1629	49	gzH = spi.write(0x0);
JoeMiller	0:4969ef3a1629	50	SSnAcc = 1;
JoeMiller	0:4969ef3a1629	51	zLED = 1;
JoeMiller	0:4969ef3a1629	52	wait(1.0);
JoeMiller	0:4969ef3a1629	53	pc.printf("\n\rLay unit flat up-side DOWN then press anykey");
JoeMiller	0:4969ef3a1629	54	while (pc.getc() == NULL);
JoeMiller	0:4969ef3a1629	55
JoeMiller	0:4969ef3a1629	56	SSnAcc = 0;
JoeMiller	0:4969ef3a1629	57	spi.write(0xAD); // Read raw Z data
JoeMiller	0:4969ef3a1629	58	gzL = spi.write(0x0);
JoeMiller	0:4969ef3a1629	59	SSnAcc = 1;
JoeMiller	0:4969ef3a1629	60	zLED = 0;
JoeMiller	0:4969ef3a1629	61
JoeMiller	0:4969ef3a1629	62	wait(1.0);
JoeMiller	0:4969ef3a1629	63	pc.printf("\n\rPlace unit on vertical surface, press any key, rotate slowly 360-deg\n\r then press any key again when complete");
JoeMiller	0:4969ef3a1629	64	while (pc.getc() == NULL);
JoeMiller	0:4969ef3a1629	65	wait(1.0);
JoeMiller	0:4969ef3a1629	66
JoeMiller	0:4969ef3a1629	67	while (!pc.readable()) {
JoeMiller	0:4969ef3a1629	68	SSnAcc = 0;
JoeMiller	0:4969ef3a1629	69	spi.write(0xA9); // Read raw X data
JoeMiller	0:4969ef3a1629	70	signed char xraw = spi.write(0x0);
JoeMiller	0:4969ef3a1629	71	SSnAcc = 1;
JoeMiller	0:4969ef3a1629	72
JoeMiller	0:4969ef3a1629	73	SSnAcc = 0;
JoeMiller	0:4969ef3a1629	74	spi.write(0xAB); // Read raw Y data
JoeMiller	0:4969ef3a1629	75	signed char yraw = spi.write(0x0);
JoeMiller	0:4969ef3a1629	76	SSnAcc = 1;
JoeMiller	0:4969ef3a1629	77
JoeMiller	0:4969ef3a1629	78	if (xraw > gxH) {gxH = xraw; xLED = !xLED;}
JoeMiller	0:4969ef3a1629	79	if (xraw < gxL) {gxL = xraw; xLED = !xLED;}
JoeMiller	0:4969ef3a1629	80	if (yraw > gyH) {gyH = yraw; yLED = !yLED;}
JoeMiller	0:4969ef3a1629	81	if (yraw < gyL) {gyL = yraw; yLED = !yLED;}
JoeMiller	0:4969ef3a1629	82	}
JoeMiller	0:4969ef3a1629	83	pc.getc(); // remove character from buffer
JoeMiller	0:4969ef3a1629	84
JoeMiller	0:4969ef3a1629	85	gxOff = (float)(gxH + gxL)/2.0; gxGain = 2.0/(float)(gxH - gxL);
JoeMiller	0:4969ef3a1629	86	gyOff = (float)(gyH + gyL)/2.0; gyGain = 2.0/(float)(gyH - gyL);
JoeMiller	0:4969ef3a1629	87	gzOff = (float)(gzH + gzL)/2.0; gzGain = 2.0/(float)(gzH - gzL);
JoeMiller	0:4969ef3a1629	88
JoeMiller	0:4969ef3a1629	89	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);
JoeMiller	0:4969ef3a1629	90
JoeMiller	0:4969ef3a1629	91	spi.format(8,0); // the MicroMag3 expects cpol=0, cpha=0
JoeMiller	0:4969ef3a1629	92
JoeMiller	0:4969ef3a1629	93	SSnMag = 0; // Select MicroMag 3
JoeMiller	0:4969ef3a1629	94
JoeMiller	0:4969ef3a1629	95	wait(1.0);
JoeMiller	0:4969ef3a1629	96	pc.printf("\n\rPress any key to GO\n\r");
JoeMiller	0:4969ef3a1629	97	pc.printf("Then -Wave unit around to find min and max mag values for all three axes\n\r");
JoeMiller	0:4969ef3a1629	98	pc.printf(" LED1-3 will toggle when new high/low values for X,Y,Z respectively are found\n\r");
JoeMiller	0:4969ef3a1629	99	pc.printf(" press any key to stop when you cannont make anymore lights blink\n\r");
JoeMiller	0:4969ef3a1629	100
JoeMiller	0:4969ef3a1629	101	while (pc.getc() == NULL);
JoeMiller	0:4969ef3a1629	102
JoeMiller	0:4969ef3a1629	103	// Throw out first reading - sometimes it is bad
JoeMiller	0:4969ef3a1629	104	RstMag = 1; RstMag = 0; // Mag reset pulse. this creates ~1.1uS pulse
JoeMiller	0:4969ef3a1629	105	spi.write(MagCommand + x); // send request for X axis mag value
JoeMiller	0:4969ef3a1629	106	while(!DrdyMag); // wait for it...
JoeMiller	0:4969ef3a1629	107	mx =spi.write(0)*0x100; mx = mx + spi.write(0); // I could not get the spi.format(16,0) to work
JoeMiller	0:4969ef3a1629	108
JoeMiller	0:4969ef3a1629	109	while(!pc.readable()) {
JoeMiller	0:4969ef3a1629	110	RstMag = 1; RstMag = 0; // Mag reset pulse. this creates ~1.1uS pulse
JoeMiller	0:4969ef3a1629	111	spi.write(MagCommand + x); // send request for X axis mag value
JoeMiller	0:4969ef3a1629	112	while(!DrdyMag); // wait for it...
JoeMiller	0:4969ef3a1629	113	mx =spi.write(0)*0x100; mx = mx + spi.write(0); // I could not get the spi.format(16,0) to work
JoeMiller	0:4969ef3a1629	114	// so I am constructing the word from 2 bytes
JoeMiller	0:4969ef3a1629	115	if ( mx > 0x7fff) // convert to signed value
JoeMiller	0:4969ef3a1629	116	mx = mx - 0x10000;
JoeMiller	0:4969ef3a1629	117
JoeMiller	0:4969ef3a1629	118	RstMag = 1; RstMag = 0; // get Y axis mag value
JoeMiller	0:4969ef3a1629	119	spi.write(MagCommand + y);
JoeMiller	0:4969ef3a1629	120	while(!DrdyMag);
JoeMiller	0:4969ef3a1629	121	my =spi.write(0)*0x100; my = my + spi.write(0);
JoeMiller	0:4969ef3a1629	122	if ( my > 0x7fff)
JoeMiller	0:4969ef3a1629	123	my = my - 0x10000;
JoeMiller	0:4969ef3a1629	124
JoeMiller	0:4969ef3a1629	125	RstMag = 1; RstMag = 0; //get Z axis mag value
JoeMiller	0:4969ef3a1629	126	spi.write(MagCommand + z);
JoeMiller	0:4969ef3a1629	127	while(!DrdyMag);
JoeMiller	0:4969ef3a1629	128	mz =spi.write(0)*0x100; mz = mz + spi.write(0);
JoeMiller	0:4969ef3a1629	129	if ( mz > 0x7fff)
JoeMiller	0:4969ef3a1629	130	mz = mz - 0x10000;
JoeMiller	0:4969ef3a1629	131
JoeMiller	0:4969ef3a1629	132	if (mx > mxH) { mxH = mx; if(xLED ==1)xLED=0; else xLED = 1;}
JoeMiller	0:4969ef3a1629	133	if (mx < mxL) { mxL = mx; if(xLED ==1)xLED=0; else xLED = 1;}
JoeMiller	0:4969ef3a1629	134	if (my > myH) { myH = my; if(yLED ==1)yLED=0; else yLED = 1;}
JoeMiller	0:4969ef3a1629	135	if (my < myL) { myL = my; if(yLED ==1)yLED=0; else yLED = 1;}
JoeMiller	0:4969ef3a1629	136	if (mz > mzH) { mzH = mz; if(zLED ==1)zLED=0; else zLED = 1;}
JoeMiller	0:4969ef3a1629	137	if (mz < mzL) { mzL = mz; if(zLED ==1)zLED=0; else zLED = 1;}
JoeMiller	0:4969ef3a1629	138
JoeMiller	0:4969ef3a1629	139	}
JoeMiller	0:4969ef3a1629	140	pc.getc(); // remove character from buffer
JoeMiller	0:4969ef3a1629	141
JoeMiller	0:4969ef3a1629	142	SSnMag = 1; // Deselect Mag
JoeMiller	0:4969ef3a1629	143
JoeMiller	0:4969ef3a1629	144	mxOff = (mxH + mxL)/2; mxGain = 1/(mxH - mxL);
JoeMiller	0:4969ef3a1629	145	myOff = (myH + myL)/2; myGain = 1/(myH - myL);
JoeMiller	0:4969ef3a1629	146	mzOff = (mzH + mzL)/2; mzGain = 1/(mzH - mzL);
JoeMiller	0:4969ef3a1629	147
JoeMiller	0:4969ef3a1629	148	pc.printf("\rmxOff:%f mxGain:%f\rmyOff:%f myGain%f\rmzOff:%f mzGain:%f\r\n\n",mxOff, mxGain, myOff, myGain, mzOff, mzGain);
JoeMiller	0:4969ef3a1629	149
JoeMiller	0:4969ef3a1629	150
JoeMiller	0:4969ef3a1629	151	pc.printf("Opening File...\n\r"); // Drive should be marked as removed
JoeMiller	0:4969ef3a1629	152	FILE *fp = fopen("/local/cal.txt", "w");
JoeMiller	0:4969ef3a1629	153	if(!fp) {
JoeMiller	0:4969ef3a1629	154	fprintf(stderr, "File /local/test.txt could not be opened!\n");
JoeMiller	0:4969ef3a1629	155	exit(1);
JoeMiller	0:4969ef3a1629	156	}
JoeMiller	0:4969ef3a1629	157
JoeMiller	0:4969ef3a1629	158	pc.printf("Writing Data...\n\r");
JoeMiller	0:4969ef3a1629	159	fprintf(fp,"%f %f %f %f %f %f ",gxOff, gxGain, gyOff, gyGain, gzOff, gzGain);
JoeMiller	0:4969ef3a1629	160	fprintf(fp,"%f %f %f %f %f %f ",mxOff, mxGain, myOff, myGain, mzOff, mzGain);
JoeMiller	0:4969ef3a1629	161	fprintf(fp,"\ngxH = %d, gxL=%d,\n gyH = %d, gyL = %d,\n gzH = %d, gzL = %d\n",gxH,gxL,gyH,gyL,gzH,gzL);
JoeMiller	0:4969ef3a1629	162	fprintf(fp,"\nmxH = %f, mxL=%f,\n myH = %f, myL = %f,\n mzH = %f, mzL = %f\n",mxH,mxL,myH,myL,mzH,mzL);
JoeMiller	0:4969ef3a1629	163
JoeMiller	0:4969ef3a1629	164	pc.printf("Closing File...\n\r");
JoeMiller	0:4969ef3a1629	165	fclose(fp);
JoeMiller	0:4969ef3a1629	166
JoeMiller	0:4969ef3a1629	167	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);
JoeMiller	0:4969ef3a1629	168	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);
JoeMiller	0:4969ef3a1629	169
JoeMiller	0:4969ef3a1629	170
JoeMiller	0:4969ef3a1629	171
JoeMiller	0:4969ef3a1629	172	exit(0);
JoeMiller	0:4969ef3a1629	173	}
JoeMiller	0:4969ef3a1629	174
JoeMiller	0:4969ef3a1629	175	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	03 Dec 2009
Imports:	2
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	1
Followers:	1

main.cpp@0:4969ef3a1629, 2009-12-03 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning