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

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

#define EnaHC		5
#define EnaVC		6

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

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

// Print sign on message
aPrint_String("Test 44. Daventech 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
   }

// detect Daventech Horizontal compass
	aDig_Config(EnaVC,	ADIG_OUTPUT);
	aDig_Config(EnaHC,	ADIG_OUTPUT);
	aDig_Write(EnaVC, 0); aDig_Write(EnaHC, 1); aCore_Sleep(500);	// min 300
	if (aI2C_ReadChar((unsigned char)0x0C0, 0) != -1) {
		vh = aI2C_ReadChar((char)0x0C0, 0);
		}
	aDig_Write(EnaHC, 0);	
	
// detect Daventech Vertical compass
	aDig_Write(EnaVC, 1); aCore_Sleep(500);
	if (aI2C_ReadChar((unsigned char)0x0C0, 0) != -1) {
		vv = aI2C_ReadChar((char)0x0C0, 0);
		}
	aDig_Write(EnaVC, 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(' ');

	// get compass info
	if (vh>0) {
		aDig_Write(EnaHC, 1); aCore_Sleep(1000);
		X = aI2C_ReadInt((unsigned char)0x0C0, 4);
		Y = aI2C_ReadInt((unsigned char)0x0C0, 6);
		aPrint_IntDec(X); aPrint_Char(' ');
		aPrint_IntDec(Y); aPrint_Char(' ');
		aDig_Write(EnaHC, 0);	
		} else { X=0; Y=0; }
	if (vv>0) {
		aDig_Write(EnaVC, 1); aCore_Sleep(1000);
		Z =  aI2C_ReadInt((unsigned char)0x0C0, 4);
		Y2 = aI2C_ReadInt((unsigned char)0x0C0, 6);
		aPrint_IntDec(Z); aPrint_Char(' ');
		aPrint_IntDec(Y2); aPrint_Char('\n');
		aDig_Write(EnaVC, 0);	
		} else Z=0;
	
	aCore_Sleep(1500);

	}
}      


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);
}