// ----------------------------------------------------
// TEST application for
// Cruise Control application for Panther Electro-Steer
//
// -- TEST5 --  read & print Mindsensor compass info.
//
// Copyright 2005,2006, Mike Noel, All Rights Reserved
// ----------------------------------------------------

#include <aCore.tea>
#include <aI2C.tea>
#include <aPrint.tea>

// ********************* Test Routine ********************

void main () {
//
int i=0, vh=-1, vv=-1, tmp, X, Y, Z;

// Print sign on message
aPrint_String("Test 5. Mindscape Compass(s) "); aCore_Sleep(1000);
aPrint_String(" '4 22 0' to kill...\n");	aCore_Sleep(1000);

// need for following slowdown is confirmed...
  asm {   
   /* set to 100kHz for slow compass */
   pushlb 0
   popbm  aPortIICBaud
   }

vh = aI2C_ReadChar((char)0x0E0, 0);
vv = aI2C_ReadChar((char)0x0E8, 0);

aPrint_String("Compass versions, Horz "); 
PrtVer(vh);
aPrint_String(", Vert ");
PrtVer(vv);
aPrint_String("\n\n");
aCore_Sleep(10000);
if ((vv+vh)<0) return;

i = 0;
while (1) {

	i++;
	aPrint_IntDec(i); aPrint_Char(' ');

	// tell compass(s) to ready a new reading
	if (vh>0) aI2C_WriteChar((char)0x0E0, 0, 'I');	
	if (vv>0) aI2C_WriteChar((char)0x0E8, 0, 'I');	

	// get compass info
	if (vh>0) {
		X = RSwap((char)0x0E0, 12);
		Y = RSwap((char)0x0E0, 14);
		} else { X=0; Y=0; }
	aPrint_IntDec(X); aPrint_Char(' ');
	aPrint_IntDec(Y); aPrint_Char(' ');
	
	if (vv>0) {
		Z = RSwap((char)0x0E8, 12);
		Y = RSwap((char)0x0E8, 14);
		} else { Z=0; }
	aPrint_IntDec(Z); aPrint_Char(' ');
	aPrint_IntDec(Y); aPrint_Char('\n');
	
	aCore_Sleep(5000);
	}
}      


void PrtVer(int v) {
// print version number, or, if version negative, say it is not available
if (v>0) aPrint_IntDec(v); else aPrint_String("**NOT PRESENT**");
aCore_Sleep(1000);
}

int RSwap(char addr, char port) {
// read compass and swap bytes
int it, tmp;
	while (aI2C_ReadChar(addr, 0) < 0)  { aCore_Sleep(100); }
	it = aI2C_ReadInt(addr, port);
	tmp = (it >> 8) & 0xff;
	return(((it & 0xff) << 8) | tmp);
}