/*
	compass.c

	Routines for the vector compass module.

	Pinouts are specified as global variables.  Must call compass_init
	before attempting to read the compass.  It takes approx .26 seconds
	to take a reading, so if you're turning at the time ... :-(  

	v1.0, Doug Rinckes, 2001
	doug.rinckes@iname.com

*/

/* digital outputs on the expansion board */
int sclk = 5;	/* green wire */
int pcss = 6;	/* white wire */
int reset = 7;	/* brown wire */

/* digital inputs on the handyboard */
int sdo = 14;	/* blue wire */
int eoc = 15;	/* purple wire */


void compass_init()
 {
	set_digital_out(pcss);
	set_digital_out(sclk);
	set_digital_out(reset);
	clear_digital_out(reset);
	msleep(15L);
	set_digital_out(reset);
	msleep(5L);
	/*
	   this long wait is to prevent attempting to read the compass 
	   until it is fully reset. 
	*/
	msleep(100L);
}

int compass_read()
{
	int bit;
	int heading = 0;
	int eoc_return;

	/* set sclk and pcss high, then drop pcss for a short time */
	set_digital_out(sclk);
	set_digital_out(pcss);
	msleep(5L);
	clear_digital_out(pcss);
	msleep(2L);
	set_digital_out(pcss);

	/* eoc should drop... */
	eoc_return = digital(eoc);
	if (eoc_return == 1) {
		/* hmm, eoc didn't drop.  try poking it again */
		set_digital_out(pcss);
		msleep(5L);
		clear_digital_out(pcss);
		msleep(2L);
		set_digital_out(pcss);
		eoc_return = digital(eoc);
		/* if eoc still didn't drop, bomb out, otherwise continue */
		if (eoc_return == 1) {
			return -1;
		}
	}

	/* ok now wait until eoc goes high - indicates that the compass is ready */
	while (digital(eoc) != 1) {
		;
	}

	/* so eoc is now high, and the compass can be read */
	/* drop pcss while data is being clocked out */

	clear_digital_out(pcss);

	/* clock 7 times to clock out seven bits which aren't used.  */
	for (bit = 1;bit <= 7;bit ++) {
		clear_digital_out(sclk);
		set_digital_out(sclk);
	}

	/* get the next bit */
	clear_digital_out(sclk);
	set_digital_out(sclk);
	if (digital(sdo) == 1) {
		heading=256;
	}

	/* now clock out the next 8 bits */
	msleep(5L);
	for (bit = 7;bit > 0;bit --) {
		clear_digital_out(sclk);
		set_digital_out(sclk);
		if (digital(sdo) != 0) {
			heading +=(int)((2.^(float)(bit))+1.);
		}
	}
	/*
	   clock out final bit (bit 0). causes floating point overflow if
	   done in previous loop.
	*/
	clear_digital_out(sclk);
	set_digital_out(sclk);
	if (digital(sdo) != 0) {
		heading +=1;
	}

	clear_digital_out(sclk);
	set_digital_out(pcss);
	set_digital_out(sclk);

	return heading;
}