TCCompass2 - Tilt Compensated Compass using 3-axis magnetomete…

Users » JoeMiller » Code » TCCompass2

Joe Miller / Mbed 2 deprecated TCCompass2

Tilt Compensated Compass using 3-axis magnetometer and 3-axis accelerometer.

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

Committer:: JoeMiller
Date:: Thu Dec 03 06:08:14 2009 +0000
Revision:: 0:d1c0b5304b6b

Who changed what in which revision?

User	Revision	Line number	New contents of line
JoeMiller	0:d1c0b5304b6b	1	// Tilt compensated compass
JoeMiller	0:d1c0b5304b6b	2	// PNI 11096 based mags and a LIS3LV02 accel
JoeMiller	0:d1c0b5304b6b	3	// This version uses Calibration data stored in a file named cal.txt
JoeMiller	0:d1c0b5304b6b	4
JoeMiller	0:d1c0b5304b6b	5	#include "mbed.h"
JoeMiller	0:d1c0b5304b6b	6
JoeMiller	0:d1c0b5304b6b	7	SPI spi(p5,p6,p7);
JoeMiller	0:d1c0b5304b6b	8	Serial pc (USBTX,USBRX);
JoeMiller	0:d1c0b5304b6b	9	LocalFileSystem local("local");
JoeMiller	0:d1c0b5304b6b	10
JoeMiller	0:d1c0b5304b6b	11	DigitalOut myled(LED1); // mosi, miso, sclk
JoeMiller	0:d1c0b5304b6b	12	DigitalOut SSnAcc(p8); // Select Accelerometer
JoeMiller	0:d1c0b5304b6b	13	DigitalOut RstMag(p9); // reset input to MicroMag3
JoeMiller	0:d1c0b5304b6b	14	DigitalIn DrdyMag(p10); // Mag data Ready
JoeMiller	0:d1c0b5304b6b	15	DigitalOut SSnMag(p11); // Select Mag
JoeMiller	0:d1c0b5304b6b	16
JoeMiller	0:d1c0b5304b6b	17	#define x 1
JoeMiller	0:d1c0b5304b6b	18	#define y 2
JoeMiller	0:d1c0b5304b6b	19	#define z 3
JoeMiller	0:d1c0b5304b6b	20	#define MagCommand 0x40
JoeMiller	0:d1c0b5304b6b	21	#define pi 3.14159
JoeMiller	0:d1c0b5304b6b	22	#define Rad2Deg 360/(2* pi)
JoeMiller	0:d1c0b5304b6b	23
JoeMiller	0:d1c0b5304b6b	24	int main() {
JoeMiller	0:d1c0b5304b6b	25	float gxOff, gxGain, gyOff, gyGain, gzOff, gzGain;
JoeMiller	0:d1c0b5304b6b	26	float mxOff, mxGain, myOff, myGain, mzOff, mzGain;
JoeMiller	0:d1c0b5304b6b	27
JoeMiller	0:d1c0b5304b6b	28	pc.baud(115200);
JoeMiller	0:d1c0b5304b6b	29	SSnMag = 1; SSnAcc=1; // deselect everything
JoeMiller	0:d1c0b5304b6b	30	float mx, my, mz; // Magnetic field vectors
JoeMiller	0:d1c0b5304b6b	31
JoeMiller	0:d1c0b5304b6b	32	pc.printf("Opening File...\n"); // Drive should be marked as removed
JoeMiller	0:d1c0b5304b6b	33	FILE *fp = fopen("/local/cal.txt", "r");
JoeMiller	0:d1c0b5304b6b	34	if(!fp) {
JoeMiller	0:d1c0b5304b6b	35	fprintf(stderr, "File could not be opened!\n");
JoeMiller	0:d1c0b5304b6b	36	exit(1);
JoeMiller	0:d1c0b5304b6b	37	}
JoeMiller	0:d1c0b5304b6b	38
JoeMiller	0:d1c0b5304b6b	39	pc.printf("Reading Cal Data...\n");
JoeMiller	0:d1c0b5304b6b	40	fscanf(fp,"%f %f %f %f %f %f ",&gxOff, &gxGain, &gyOff, &gyGain, &gzOff, &gzGain);
JoeMiller	0:d1c0b5304b6b	41	fscanf(fp,"%f %f %f %f %f %f ",&mxOff, &mxGain, &myOff, &myGain, &mzOff, &mzGain);
JoeMiller	0:d1c0b5304b6b	42
JoeMiller	0:d1c0b5304b6b	43	pc.printf("Closing File.........");
JoeMiller	0:d1c0b5304b6b	44	fclose(fp);
JoeMiller	0:d1c0b5304b6b	45	pc.printf("Closed\n");
JoeMiller	0:d1c0b5304b6b	46
JoeMiller	0:d1c0b5304b6b	47	pc.printf("\ngxOff:%f gxGain:%f\ngyOff:%f gyGain%f\ngzOff:%f gzGain:%f\n",gxOff, gxGain, gyOff, gyGain, gzOff, gzGain);
JoeMiller	0:d1c0b5304b6b	48	pc.printf("\nmxOff:%f mxGain:%f\nmyOff:%f myGain%f\nmzOff:%f mzGain:%f\n\n",mxOff, mxGain, myOff, myGain, mzOff, mzGain);
JoeMiller	0:d1c0b5304b6b	49
JoeMiller	0:d1c0b5304b6b	50
JoeMiller	0:d1c0b5304b6b	51	//spi.frequency(100000); // no need to go very fast
JoeMiller	0:d1c0b5304b6b	52	// setup the accel
JoeMiller	0:d1c0b5304b6b	53	spi.format(8,3); // the accel expects cpol=1,cpha=1
JoeMiller	0:d1c0b5304b6b	54	SSnAcc = 0;
JoeMiller	0:d1c0b5304b6b	55	spi.write(0x20); // Address the Ctrl_Reg1 register.....
JoeMiller	0:d1c0b5304b6b	56	spi.write(0x47); // and set 'active mode bit' and all 3 axes 'enable' bits
JoeMiller	0:d1c0b5304b6b	57	SSnAcc = 1;
JoeMiller	0:d1c0b5304b6b	58	wait (0.01);
JoeMiller	0:d1c0b5304b6b	59
JoeMiller	0:d1c0b5304b6b	60	//main loop - continuously read Accel and mag data then process and display
JoeMiller	0:d1c0b5304b6b	61	while (!pc.readable()) {
JoeMiller	0:d1c0b5304b6b	62	spi.format(8,3); // the accel expects cpol=1,cpha=1
JoeMiller	0:d1c0b5304b6b	63
JoeMiller	0:d1c0b5304b6b	64	SSnAcc = 0;
JoeMiller	0:d1c0b5304b6b	65	spi.write(0xA9); // Read raw X data
JoeMiller	0:d1c0b5304b6b	66	signed char xraw = spi.write(0x0);
JoeMiller	0:d1c0b5304b6b	67	SSnAcc = 1;
JoeMiller	0:d1c0b5304b6b	68
JoeMiller	0:d1c0b5304b6b	69	SSnAcc = 0;
JoeMiller	0:d1c0b5304b6b	70	spi.write(0xAB); // Read raw Y data
JoeMiller	0:d1c0b5304b6b	71	signed char yraw = spi.write(0x0);
JoeMiller	0:d1c0b5304b6b	72	SSnAcc = 1;
JoeMiller	0:d1c0b5304b6b	73
JoeMiller	0:d1c0b5304b6b	74	SSnAcc = 0;
JoeMiller	0:d1c0b5304b6b	75	spi.write(0xAD); // Read raw Z data
JoeMiller	0:d1c0b5304b6b	76	signed char zraw = spi.write(0x0);
JoeMiller	0:d1c0b5304b6b	77	SSnAcc = 1;
JoeMiller	0:d1c0b5304b6b	78
JoeMiller	0:d1c0b5304b6b	79	float gx = ((float)xraw-gxOff)*gxGain; // scale and offset using calibration coef
JoeMiller	0:d1c0b5304b6b	80	float gy = ((float)yraw-gyOff)*gyGain; //
JoeMiller	0:d1c0b5304b6b	81	float gz = ((float)zraw-gzOff)*gzGain; //
JoeMiller	0:d1c0b5304b6b	82
JoeMiller	0:d1c0b5304b6b	83	spi.format(8,0); // the MicroMag3 expects cpol=0, cpha=0
JoeMiller	0:d1c0b5304b6b	84
JoeMiller	0:d1c0b5304b6b	85	SSnMag = 0; // Select MicroMag 3
JoeMiller	0:d1c0b5304b6b	86
JoeMiller	0:d1c0b5304b6b	87	RstMag = 1; RstMag = 0; // Mag reset pulse. this creates ~1.1uS pulse
JoeMiller	0:d1c0b5304b6b	88	spi.write(MagCommand + x); // send request for X axis mag value
JoeMiller	0:d1c0b5304b6b	89	while(!DrdyMag); // wait for it...
JoeMiller	0:d1c0b5304b6b	90	mx =spi.write(0)*0x100; mx = mx + spi.write(0); // I could not get the spi.format(16,0) to work
JoeMiller	0:d1c0b5304b6b	91	// so I am constructing the word from 2 bytes
JoeMiller	0:d1c0b5304b6b	92	if ( mx > 0x7fff) // convert to signed value
JoeMiller	0:d1c0b5304b6b	93	mx = mx - 0x10000;
JoeMiller	0:d1c0b5304b6b	94	mx = (mx - mxOff)*mxGain;
JoeMiller	0:d1c0b5304b6b	95
JoeMiller	0:d1c0b5304b6b	96	RstMag = 1; RstMag = 0; // get Y axis mag value
JoeMiller	0:d1c0b5304b6b	97	spi.write(MagCommand + y);
JoeMiller	0:d1c0b5304b6b	98	while(!DrdyMag);
JoeMiller	0:d1c0b5304b6b	99	my =spi.write(0)*0x100; my = my + spi.write(0);
JoeMiller	0:d1c0b5304b6b	100	if ( my > 0x7fff)
JoeMiller	0:d1c0b5304b6b	101	my = my - 0x10000;
JoeMiller	0:d1c0b5304b6b	102	my = (my - myOff)*myGain;
JoeMiller	0:d1c0b5304b6b	103
JoeMiller	0:d1c0b5304b6b	104	RstMag = 1; RstMag = 0; //get Z axis mag value
JoeMiller	0:d1c0b5304b6b	105	spi.write(MagCommand + z);
JoeMiller	0:d1c0b5304b6b	106	while(!DrdyMag);
JoeMiller	0:d1c0b5304b6b	107	mz =spi.write(0)*0x100; mz = mz + spi.write(0);
JoeMiller	0:d1c0b5304b6b	108	if ( mz > 0x7fff)
JoeMiller	0:d1c0b5304b6b	109	mz = mz - 0x10000;
JoeMiller	0:d1c0b5304b6b	110	mz = (mz - mzOff)*mzGain;
JoeMiller	0:d1c0b5304b6b	111
JoeMiller	0:d1c0b5304b6b	112	SSnMag = 0; // Deselect Mag
JoeMiller	0:d1c0b5304b6b	113
JoeMiller	0:d1c0b5304b6b	114	// Axis adjustments. This section adjusts some of the Axes to
JoeMiller	0:d1c0b5304b6b	115	// make the system into a NEU (North East Up) body frame.
JoeMiller	0:d1c0b5304b6b	116	mx = -mx; // makes mx = + when x is pointing due North (body frame point North)
JoeMiller	0:d1c0b5304b6b	117	my = -my; // makes my = + when y is pointing due North (body frame pointing East)
JoeMiller	0:d1c0b5304b6b	118	mz = -mz; // makes mz = + when z is pointing due North (body frame is up-side-down)
JoeMiller	0:d1c0b5304b6b	119	gx = -gx; // makes gx = + when x is pointing down (body frame is pointing down)
JoeMiller	0:d1c0b5304b6b	120	// gy is already + when y is pointing down (body frame's right wing is down)
JoeMiller	0:d1c0b5304b6b	121	gz = -gz; // makes gz = + when z is pointing down (body frame is up-side-down)
JoeMiller	0:d1c0b5304b6b	122	// I'm using the vector method of tilt compensation rather than a cosine matrix. The vector method does not involve
JoeMiller	0:d1c0b5304b6b	123	// a lot of trig. Basically what is going on is that horizontal plane is constructed using Cross products of g and m.
JoeMiller	0:d1c0b5304b6b	124	// Then by using Dot products, the body coordinate frame is projected onto the newly contructed horizontal XY vectors.
JoeMiller	0:d1c0b5304b6b	125	// for more information search: "A New Solution Algorithm of Magnetic Amizuth", or "A combined electronic compass/clinometer"
JoeMiller	0:d1c0b5304b6b	126	float hx = (gzgz mx) - (gxgzmz) - (gx gy my) + (gygy mx); //horizontal X mag vector
JoeMiller	0:d1c0b5304b6b	127	float hy = (gy * mz) - (gz * my); //horizontal Y mag vector
JoeMiller	0:d1c0b5304b6b	128	float heading = atan2(hy,hx)*Rad2Deg;
JoeMiller	0:d1c0b5304b6b	129
JoeMiller	0:d1c0b5304b6b	130	// {+45}-------{ 0 }-------{-45} this is the output of atan2(hy,hx)
JoeMiller	0:d1c0b5304b6b	131	// \| +-----[ +x]-----+ \| it must be adjusted to convert it
JoeMiller	0:d1c0b5304b6b	132	// \| \| \| \| to navigational directions
JoeMiller	0:d1c0b5304b6b	133	// \| \| \| \|
JoeMiller	0:d1c0b5304b6b	134	// {+90} [+y] [0,0] [-y] {-90} (Note: you get positive y when body frame
JoeMiller	0:d1c0b5304b6b	135	// \| \| \| \| faces West in an NEU system because
JoeMiller	0:d1c0b5304b6b	136	// \| \| \| \| NEU refers to sensor direction when the
JoeMiller	0:d1c0b5304b6b	137	// \| +-----[ -x]-----+ \| body frame is at rest in its reference or identity orientation)
JoeMiller	0:d1c0b5304b6b	138	// {+135}-----{+/-180}------{-135}
JoeMiller	0:d1c0b5304b6b	139	if (heading > 0)
JoeMiller	0:d1c0b5304b6b	140	heading = 360 - heading;
JoeMiller	0:d1c0b5304b6b	141	else
JoeMiller	0:d1c0b5304b6b	142	heading = -heading;
JoeMiller	0:d1c0b5304b6b	143	// {335}------{360/0}------{45}
JoeMiller	0:d1c0b5304b6b	144	// \| +-----[ N ]-----+ \|
JoeMiller	0:d1c0b5304b6b	145	// \| \| \| \| navigational directions
JoeMiller	0:d1c0b5304b6b	146	// \| \| \| \|
JoeMiller	0:d1c0b5304b6b	147	// {270} [W] [0,0] [E] {90}
JoeMiller	0:d1c0b5304b6b	148	// \| \| \| \|
JoeMiller	0:d1c0b5304b6b	149	// \| \| \| \|
JoeMiller	0:d1c0b5304b6b	150	// \| +------[S]------+ \|
JoeMiller	0:d1c0b5304b6b	151	// {225}-------{180}-------{135}
JoeMiller	0:d1c0b5304b6b	152
JoeMiller	0:d1c0b5304b6b	153	float pitch = atan(-gx/sqrt(gygy+gzgz))*Rad2Deg; //invert gx because +pitch is up. range is +/-90 degrees
JoeMiller	0:d1c0b5304b6b	154	float roll = atan(gy/sqrt(gxgx+gzgz))*Rad2Deg; // right side down is +roll
JoeMiller	0:d1c0b5304b6b	155	if (gz > 0) // unfold atan's limited 2 quadrant space to extend roll to 4 quadrants (+/-180)
JoeMiller	0:d1c0b5304b6b	156	if ( roll > 0)
JoeMiller	0:d1c0b5304b6b	157	roll = 180 - roll;
JoeMiller	0:d1c0b5304b6b	158	else
JoeMiller	0:d1c0b5304b6b	159	roll = -180 - roll;
JoeMiller	0:d1c0b5304b6b	160
JoeMiller	0:d1c0b5304b6b	161	pc.printf("\ngx: %f, gy: %f, gz: %f\nmx: %f, my: %f, mz: %f\n",gx,gy,gz,mx,my,mz);
JoeMiller	0:d1c0b5304b6b	162	pc.printf("hx: %f, hy: %f\n",hx,hy);
JoeMiller	0:d1c0b5304b6b	163	pc.printf("heading: %f, pitch: %f, roll: %f\n",heading,pitch,roll);
JoeMiller	0:d1c0b5304b6b	164	wait(1);
JoeMiller	0:d1c0b5304b6b	165	}
JoeMiller	0:d1c0b5304b6b	166	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

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

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

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning