// ----------------------------------------------------
// TEST application for
// Cruise Control application for Panther Electro-Steer
//
// -- TEST6 --  does accelerometer sees roll and pitch?
//
// Copyright 2005,2006, Mike Noel, All Rights Reserved
// ----------------------------------------------------

#include <aCore.tea>
#include <aA2D.tea>
#include <aPrint.tea>
#include "aMath.tea"

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

void main () {
//
int a2d0, a2d1, a2d2, a2d3;

// Print sign on message
aPrint_String("Test 6, Acceleromter Roll/Pitch\n"); aCore_Sleep(1000);
aPrint_String("       '4 22 0' to kill...\n"); aCore_Sleep(1000);


// detect Accelerometer (skip first readings)
	a2d0=aA2D_ReadInt(0); a2d1=aA2D_ReadInt(1); a2d2=aA2D_ReadInt(2);
	a2d3=aA2D_ReadInt(3); a2d3=aA2D_ReadInt(3);
	if (a2d0 < 100) { // typical ~ 335 with, 0 w/o
		aPrint_String("  Acceleromter not found!\n");
		return;
		}

// report relative roll/pitch from initial angle until '4 22 0'...
while(1) {

	a2d0 = aA2D_ReadInt(0); 	// a2d-0 is accel 'x', aka roll (+ roll starboard gunel down)
	a2d1 = aA2D_ReadInt(1);		// a2d-1 is accel 'y', aka pitch (+ pitch bow up)
	a2d2 = aA2D_ReadInt(2);		// a2d-2 is accel 'z', aka 'down'
	a2d3 = aA2D_ReadInt(3);		// a2d-3 is ref voltage

	// ovresample...
	a2d0 = a2d0 + aA2D_ReadInt(0); a2d0 = a2d0 + aA2D_ReadInt(0); a2d0 = a2d0 + aA2D_ReadInt(0);
	a2d1 = a2d1 + aA2D_ReadInt(1); a2d1 = a2d1 + aA2D_ReadInt(1); a2d1 = a2d1 + aA2D_ReadInt(1);
	a2d2 = a2d2 + aA2D_ReadInt(2); a2d2 = a2d2 + aA2D_ReadInt(2); a2d2 = a2d2 + aA2D_ReadInt(2);
	a2d3 = a2d3 + aA2D_ReadInt(3); a2d3 = a2d3 + aA2D_ReadInt(3); a2d3 = a2d3 + aA2D_ReadInt(3);

	aPrint_IntDec(a2d0); aPrint_Char(' '); aPrint_IntDec(a2d1); aPrint_Char(' '); 
	aPrint_IntDec(a2d2); aPrint_Char(' '); aPrint_IntDec(a2d3); aPrint_Char(' '); 
	aPrint_IntDec(a2d0-(a2d3/2)); aPrint_Char(' ');
	aPrint_IntDec(a2d1-(a2d3/2)); aPrint_Char(' '); 
	aPrint_Char('\n'); 
	aCore_Sleep(3000);

	}
}