/*
	GYRO.CC

	Gyroscope class decyphers the gyroscope sensor installed on the balancing
	robot, Ballybot.

	If an analog inlinometer is installed on the robot, this class uses the
	inclinometer to recalibrate the gyro.

	Alistair Sutherland
	suthe-aj@ee.uwa.edu.au
	CIIPS, 2003
*/

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "gyro.h"

// Number of samples needed to get a filtered inclinometer reading
#define ANGLE_ERROR_SAMPLES 5
#define INCLINE_SAMPLES  10

// variance threshold to recalibrate
#define RECALIBRATE_VARIANCE 0.05
#define MIN_TIME_BETWEEN_RECALIBRATE 100
#define MAX_RECALIBRATE_CORRECTION 4500 
#define MAX_RECALIBRATEVEL_CORRECTION 2


// These need to be global for the timer sample function to access...
int g_iMode;
int g_iZeroVelocity;			// Raw Gyro reading that is quated to 0deg/sec
int g_iAngularVelocity;			// Gyro estimate of angular velocity (last raw reading)
int g_iAngle;					// Gyro estimate of angle
int g_iSampleTime;				// System time (tics) sample last taken
int g_iInclineZero;				// The raw AD inclinometer reading at 0deg
int g_iTimeLastCalibrated;		// System time (tics) last calibrated
int g_iMaxAngle;				// The raw gyro estimate of 90deg

int g_iPWMInclinometer;
int g_iPWMInclineZero;

int g_iGyroAngleCorrection;			// After recalibration, this is the angle error to be corrected
int g_iGyroAngleCorrectionDelta;	// the error is corrected in this size chunks - not all at once

int g_iRecordInclineIdx;						// Last few inclinometer readings are stored,
int g_rgiRecordInclineTime[INCLINE_SAMPLES];	// in case we want to recalibrate the Gyroscope.
int g_rgiRecordIncline[INCLINE_SAMPLES];

#define DEBUG_SAMPLES 1000
int g_iDebugSampleCount;
int g_rgiDebugSampleTime[DEBUG_SAMPLES];
int g_rgiDebugSampleIncline[DEBUG_SAMPLES];
int g_rgiDebugSampleGyro[DEBUG_SAMPLES];
int g_rgiDebugSampleCorrect[DEBUG_SAMPLES];
int g_iDebugRecording;

// Prototype
void OnlineRecalibrate(float fMeanIncline);
void DownloadResult();
void TimerSample();


CGyro::CGyro()
{
	// I would like to simply call CGyro(NORMAL_MODE) here, but this does not work.
	g_iMode = NORMAL_MODE;

	g_iRecordInclineIdx = 0;

	// Start with no timer...
	m_hSampleTimer = 0;

	// Need to initialise the SERVO driving Gyro...
	m_hServo = SERVOInit(SERVO11);
	SERVOSet(m_hServo, 127);

	// Initialise, Release & Re-initialise the gyroscope sensor
	tpuinit();
	tpurelease();
	tpuinit();

	// Initialise state values
	InitialiseState();

	// init debug
	g_iDebugSampleCount = 0;
}

CGyro::CGyro(int iMode)
{
	g_iMode = iMode;
	int i;

	g_iRecordInclineIdx = 0;
	g_iDebugRecording = DEBUG_RECORDING;
	for (i=0;i<DEBUG_SAMPLES;i++)
	{
		g_rgiDebugSampleTime[i] =0;
		g_rgiDebugSampleIncline[i]=0;
		g_rgiDebugSampleGyro[i]=0;
		g_rgiDebugSampleCorrect[i]=0;
	}

	// Start with no timer...
	m_hSampleTimer = 0;

	// Need to initialise the SERVO driving Gyro...
	m_hServo = SERVOInit(SERVO11);
	SERVOSet(m_hServo, 127);

	// Initialise, Release & Re-initialise the gyroscope sensor
	tpuinit();
	tpurelease();
	tpuinit();

	// Initialise state values
	InitialiseState();

	// init debug
	g_iDebugSampleCount = 0;
}

CGyro::~CGyro()
{
	// Release the gyroscope sensors
	tpurelease();

	// Release the servo
	SERVORelease(SERVO11);

	// Release the timer
	OSDetachTimer( m_hSampleTimer );
}


/////////////////////////////////////////////////////////
//
// TIMER SAMPLE
//
// Handler function for sample timer - gets reading from
// Gyro and updates state variables
//
void TimerSample()
{
	int iAngVel;
	int iTimeNow, iElapsed;
	int i;
	int iRawADReading;
	float fMeanIncline;
	float fVarianceIncline;

	// Check if there is a new sample:
	iAngVel = tpuread13();
	
	if (iAngVel > -1)
	{
		iAngVel = iAngVel;

		// Get the elapsed time
		iTimeNow = OSGetCount();
		iElapsed = iTimeNow - g_iSampleTime;

		// Correct elapsed time if rolled over!
		if (iElapsed < 0) iElapsed += 0xFFFFFFFF; // ROLL OVER

		// Correct the angular velocity
		iAngVel -= g_iZeroVelocity;

		// Calculate angular displacement
		g_iAngle += (g_iAngularVelocity * iElapsed);
		g_iAngularVelocity = -iAngVel;
		g_iSampleTime = iTimeNow;

		// Read inclinometer (a few times to drain residual values)
		iRawADReading = OSGetAD(INCLINE_CHANNEL);
		iRawADReading = OSGetAD(INCLINE_CHANNEL);

		// If we are recording, and we have started...store data
		if (g_iTimeLastCalibrated > 0)
		{
			if (g_iMode == DEBUG_RAW_MODE && g_iDebugRecording == DEBUG_RECORDING)
			{
				if (g_iDebugSampleCount >= DEBUG_SAMPLES) DownloadResult();
				g_rgiDebugSampleTime[g_iDebugSampleCount] = iTimeNow;
				g_rgiDebugSampleIncline[g_iDebugSampleCount] = iRawADReading;
				g_rgiDebugSampleGyro[g_iDebugSampleCount] = g_iAngle;
				g_iDebugSampleCount++;
			}

			else
			{
				// record sensors & residual angle correction
				if (g_iMode == DEBUG_ONLINE_MODE && g_iDebugRecording == DEBUG_RECORDING)
				{
					// Wrap around recording...
					if (g_iDebugSampleCount >= DEBUG_SAMPLES) g_iDebugSampleCount = 0;

					g_rgiDebugSampleTime[g_iDebugSampleCount] = iTimeNow;
					g_rgiDebugSampleIncline[g_iDebugSampleCount] = iRawADReading - g_iInclineZero;
					g_rgiDebugSampleGyro[g_iDebugSampleCount] = g_iAngle;
					g_rgiDebugSampleCorrect[g_iDebugSampleCount] = g_iGyroAngleCorrection;
					g_iDebugSampleCount++;
				}
				
				g_rgiRecordInclineTime[g_iRecordInclineIdx] = g_iSampleTime;
				g_rgiRecordIncline[g_iRecordInclineIdx++]  = iRawADReading - g_iInclineZero;
				if (g_iRecordInclineIdx >= INCLINE_SAMPLES) 
				{
					g_iRecordInclineIdx = 0;

					// Work out mean of samples
					fMeanIncline = 0.0;
					for (i=0; i<INCLINE_SAMPLES; i++)
					{
						fMeanIncline += g_rgiRecordIncline[i];
					}
					fMeanIncline /= INCLINE_SAMPLES;

					// Work out variance
					fVarianceIncline = 0.0;
					for (i=0; i<INCLINE_SAMPLES; i++)
					{
						fVarianceIncline += ((g_rgiRecordIncline[i] - fMeanIncline) * (g_rgiRecordIncline[i] - fMeanIncline));
					}
					fVarianceIncline /= (INCLINE_SAMPLES-1);

					if (fVarianceIncline < RECALIBRATE_VARIANCE &&
						(g_iSampleTime - g_iTimeLastCalibrated) > MIN_TIME_BETWEEN_RECALIBRATE &&
						g_iMode != GYRO_ONLY_MODE) 
						OnlineRecalibrate(fMeanIncline);
				}
			}
		}
	}

	// If there is a correction factor remaining to apply, apply it!
	if (g_iGyroAngleCorrection != 0)
	{
		g_iGyroAngleCorrection -= g_iGyroAngleCorrectionDelta;
		g_iAngle -= g_iGyroAngleCorrectionDelta;
		if (((g_iGyroAngleCorrectionDelta > 0) && (g_iGyroAngleCorrection < 0)) || 
		    ((g_iGyroAngleCorrectionDelta < 0) && (g_iGyroAngleCorrection > 0)))  g_iGyroAngleCorrection = 0;
	}
}


//////////////////////////////////////////////////////////
//
// INITIALISE STATE
//
// Recalibrates zero state, and takes the first state
// sample, assuming angle is zero.
//
void CGyro::InitialiseState()
{
	// May need to release sample timer...
	if (m_hSampleTimer)
	{
		OSDetachTimer( m_hSampleTimer );
		m_hSampleTimer = 0;
	}

	// Pause for a bit
	OSWait(INIT_DELAY);

	// Initialise state values...
	g_iAngle = 0;
	g_iZeroVelocity = 0;
	g_iTimeLastCalibrated = 0;
	g_iGyroAngleCorrection = 0;

	ZeroAngVel();

	g_iSampleTime = OSGetCount();


	LCDPrintf("Sampling...\n");

	// Create the sampling timer:
	m_hSampleTimer = OSAttachTimer(1, TimerSample);
}

///////////////////////////////////////////////////////////
//
// GET TIME SINCE CALIBRATED
//
// Returns the number of ticks elapsed since the last recalibration,
// or -1 if the Gyro has never been calibrated.
//
int CGyro::GetTimeSinceCalibration()
{
	int iTimeNow;

	// Get the elapsed time
	iTimeNow = OSGetCount();

	if (g_iTimeLastCalibrated>0)
	{
		return (iTimeNow - g_iTimeLastCalibrated);
	}
	else
	{
		return (-1);
	}
}

//////////////////////////////////////////////////////////
//
// RECALIBRATE
//
// Recalibrates zero state, and finds max and zero angles.
//
void CGyro::Recalibrate()
{
	int iKey;
	int i;

	iKey = 0;

	// Get MaxAngle & ZeroAngle
	LCDPrintf("Lie on face & KEY");
	iKey = KEYGet();
	InitialiseState();

	LCDPrintf("Stand up & KEY");
	iKey = KEYGet();

	g_iMaxAngle = -g_iAngle;
	g_iAngle = 0;

	// Wake up AD channel
	OSGetAD(INCLINE_CHANNEL);
	LCDPrintf("\nReset Inclinometer...\n");

	// Get the zero-inclinometer reading:
	g_iInclineZero = 0;
	for (i=0;i<INCLINE_SAMPLES; i++)
	{
		g_iInclineZero += OSGetAD(INCLINE_CHANNEL);
		LCDPrintf(".");
	}
	LCDPrintf("\n");

	g_iInclineZero /= INCLINE_SAMPLES; // Inclinometer units


	// Remember time calibrated:
	g_iTimeLastCalibrated = OSGetCount();

}

void CGyro::AdjustMaxAngle(int iGUnits)
{
	g_iMaxAngle += iGUnits;
	LCDPrintf(".%d.\n", g_iMaxAngle);
}

//////////////////////////////////////////////////////////
//
// ONLINE RECALIBRATE
//
// Recalibrates gyro, based on inclinometer readings
//
void OnlineRecalibrate( float fMeanIncline )
{
	int iIncline;
	int iGyroAngleError;
	int iGyroAngleVelError;
	int iTimeElapsed;

	// can only calibrate if a decent time has elapsed since the last calibration AND
	//                       the inclinometer is within its linear range (+/- 40).
	if (g_iTimeLastCalibrated > 0 && fMeanIncline > -40 && fMeanIncline < 40)
	{
		// Determine elapsed time:
		iTimeElapsed = g_iTimeLastCalibrated;

		// Remember time calibrated:
		g_iTimeLastCalibrated = OSGetCount();
		
		iTimeElapsed = g_iTimeLastCalibrated - iTimeElapsed;

		// g_iAngle is the gyro measured angle, it should be what the inclinometer says!
		iIncline = (int)(fMeanIncline * g_iMaxAngle) / 132;
		iGyroAngleError = g_iAngle - iIncline;

		// Set angle to correct value
		if (iGyroAngleError >  MAX_RECALIBRATE_CORRECTION) iGyroAngleError = MAX_RECALIBRATE_CORRECTION;
		if (iGyroAngleError < -MAX_RECALIBRATE_CORRECTION) iGyroAngleError = -MAX_RECALIBRATE_CORRECTION;

		g_iGyroAngleCorrectionDelta = iGyroAngleError / 20;
		g_iGyroAngleCorrection = iGyroAngleError;

		// Error in angle is due to an incorrect angular velocity
		// Error in angular velocity is due to an incorrect g_iZeroVelocity
		iGyroAngleVelError = iGyroAngleError / iTimeElapsed;
		int iAngVelCorrect = iGyroAngleVelError ; // / 8;
		if (iAngVelCorrect<-MAX_RECALIBRATEVEL_CORRECTION) iAngVelCorrect=-MAX_RECALIBRATEVEL_CORRECTION;
		if (iAngVelCorrect> MAX_RECALIBRATEVEL_CORRECTION) iAngVelCorrect= MAX_RECALIBRATEVEL_CORRECTION;
		g_iZeroVelocity -= iAngVelCorrect;

	}
}


///////////////////////////////////////////////////////////
//
// ZERO ANG VEL
//
// Records the current reading of angular velocity from the
// Gyro, and assumes this value corresponds to 0 rad/sec.
//
// Function also "zero's" the inclinometer reading of angle
//
void CGyro::ZeroAngVel()
{
	int iRawVel;

	int cPWM6;

	cPWM6 = 0;
	g_iPWMInclinometer = 0;
	g_iZeroVelocity = 0;

	// Take samples until one of the PWM channels has enough...
	while (cPWM6 < INIT_SAMPLES)
	{
		iRawVel = tpuread13();

		if (iRawVel > -1)
		{
			cPWM6++;
			g_iZeroVelocity += iRawVel;
		}
	}

	g_iZeroVelocity = g_iZeroVelocity / INIT_SAMPLES;
	g_iAngularVelocity = 0;
}

/////////////////////////////////////////////////////////
//
// GET GYRO STATE FUNCTIONS
//

///////////////////////////////////////////////////////////
//
// GET ANGLE
//
// If the sensor has been calibrated
//   GetAngle returns the Gyro estimate of Angle in RADIANS
// Else
//   GetAngle returns the Gyro estimate of Angle in "GUnits" from Zero
//
float CGyro::GetAngle()
{
	if (g_iTimeLastCalibrated > 0)
	{
		return ((g_iAngle * PI) / (g_iMaxAngle * 2.0));
	}
	else
	{
		return ((float)g_iAngle);
	}
}

///////////////////////////////////////////////////////////
//
// GET ANGLULARVEL
//
// If the sensor has been calibrated
//   GetAngle returns the Gyro estimate of Angular Velocity in RADIANS/Second
// Else
//   GetAngle returns the Gyro estimate of Angular Velocity in "GUnits/Tick"
//
float CGyro::GetAngularVel()
{
	float fResult;

	if (g_iTimeLastCalibrated > 0)
	{						// Note :   g_iAngularVelocity = GUnits/Tick
		fResult = (float)g_iAngularVelocity*(float)100.0;		  // GUnits/Second
		fResult = ((fResult * PI) / (g_iMaxAngle * (float)2.0)); // Radians/Second
		return (fResult);
	}
	else
	{
		return ((float)g_iAngularVelocity);
	}
}

void CGyro::DisplayAngularVelocity()
{
	LCDPrintf(" %d\n", g_iAngularVelocity);
}
//
//////////////////////////////////////////////////////////


///////////////////////////////////////////////////
//
// DOWNLOAD RESULT
// use this to send recordings of sensor samples to serial port.
// the values to download must be first stored in global arrays, and are 
// populated if the gyro mode is DEBUG_ONLINE_MODE
//
//
void CGyro::DownloadData()
{
	if (g_iMode != DEBUG_ONLINE_MODE) return;
	DownloadResult();
}
void CGyro::StopRecording()
{
	g_iDebugRecording = DEBUG_NOTRECORDING;
}

void DownloadResult()
{
	int i;
	char sz[80];


	// Ready?
	LCDPrintf("ready to send?");
	KEYGet();

	// Open serial port
	OSInitRS232(SER115200, RTSCTS, SERIAL1);

	// Download gyro angle
	TransmitLine("GYRO_ANGLE");
	for (i=0; i<DEBUG_SAMPLES; i++)
	{
		sprintf(sz, "%d\t%d\t%d\t%d", 
			g_rgiDebugSampleTime[i], g_rgiDebugSampleGyro[i],g_rgiDebugSampleIncline[i],g_rgiDebugSampleCorrect[i]);
		TransmitLine(sz);
	}

	LCDPrintf("Done.\n");
	KEYGet();
}


///////////////////////////////////////////////////
//
// Send data back to host computer for processing
//

int TransmitLine(char * sz)
{
	int i;
	int c;
	int ierr;

	c = strlen(sz);
	for (i=0; i<c; i++)
	{
		if ((ierr = OSSendCharRS232(sz[i], SERIAL1))) break;
	}
	if (!ierr) ierr = OSSendCharRS232('\n', SERIAL1);
	return ierr;
}