/********************************************************************************************
*
* Embedded Studio (C) 2008                                   
*                                                                                
* File: ES_RvdtDrv.c
* Desc: Rotary Variable Differential Transformer (RVDT) Driver Functions
*
*********************************************************************************************/

#include "ES_RvdtDrv.h"

/********************************************************************************************
 member data
 ********************************************************************************************/
RvdtDataStruct rvdtData;
RvdtStatusStruct rvdtStatus;

/********************************************************************************************
 private function prototypes
 ********************************************************************************************/
bool RVDT_ValidatePos(void);
bool RVDT_ValidateVsum(void);

/********************************************************************************************
 extern function prototypes
 ********************************************************************************************/
extern bool RVDT_SetExcFreq();  //extern function that sets excitation frequency 
extern int ADC_GetRvdtVa();    //extern ADC function that gets RVDT Va 
extern int ADC_GetRvdtVb();    //extern ADC function that gets RVDT Vb 

/*******************************************************************************************
 * Function: RVDT_Init
 *
 * Desc: This function initializes RVDT data variables and sets excitation frequency
 *
 * Params: None
 *
 * Returns: bool
 *
 * Notes: None
 *
 *******************************************************************************************/
bool RVDT_Init(void)
{
	//init rvdtData
	rvdtData.Va = 0.0;
	rvdtData.Vb = 0.0;
	rvdtData.Vsum = 0.0;
	rvdtData.Vdiff = 0.0;
	rvdtData.Position = 0.0;
	rvdtData.PosPrev = RVDT_POS_MID_STROKE;
	
	//init rvdtStatus
	rvdtStatus.SumFaultOnCounter = 0;
	rvdtStatus.SumFaultOffCounter = 0;
	rvdtStatus.HiLimitFaultOnCounter = 0;
	rvdtStatus.HiLimitFaultOffCounter = 0;
	rvdtStatus.LoLimitFaultOnCounter = 0;
	rvdtStatus.LoLimitFaultOffCounter = 0;
	rvdtStatus.Status.All = 0;

	//set excitation
    return RVDT_SetExcFreq();
}

/*******************************************************************************************
 * Function: RVDT_ValidatePos
 *
 * Desc: This function checks if position value exceeds low and high limits
 *
 * Params: None
 *
 * Returns: bool
 *
 * Notes: None
 *
 *******************************************************************************************/
bool RVDT_ValidatePos(void)
{  	
	// validate position high limit
	if(rvdtData.Position > RVDT_POS_LIMIT_HI)
	{
	    // Increment persistance counter if out of range
	    rvdtData.HiLimitFaultOnCounter++;

	    // Limit persistance counter if out of range and latch fault
	    if(rvdtData.HiLimitFaultOnCounter > RVDT_FAULT_ON_COUNT_MAX)
	    {
			rvdtData.HiLimitFaultOnCounter = 0;
			rvdtData.HiLimitFaultOffCounter = RVDT_FAULT_OFF_COUNT_MAX;
			rvdtData.Status.Bits.HiLimitFault = 1;
	    }
	}
	else
	{
		if (rvdtData.Status.Bits.HiLimitFault == 1)
		{
			if(rvdtData.HiLimitFaultOffCounter > 0)
    	    {
    			rvdtData.HiLimitFaultOffCounter--;
    	    }
    	    else
    	    {
    	    	rvdtData.Status.Bits.HiLimitFault = 0;
    	    }
		}
	}
		
	//validate low limit
	if(rvdtData.Position < RVDT_POS_LIMIT_LO)
	{
	    // Increment persistance counter if out of range
	    rvdtData.LoLimitFaultOnCounter++;

	    // Limit persistance counter if out of range and latch fault
	    if(rvdtData.LoLimitFaultOnCounter > RVDT_FAULT_ON_COUNT_MAX)
	    {
			rvdtData.LoLimitFaultOnCounter = 0;
			rvdtData.LoLimitFaultOffCounter = RVDT_FAULT_OFF_COUNT_MAX;
			rvdtData.Status.Bits.LoLimitFault = 1;
	    }
	}
	else
	{
		if (rvdtData.Status.Bits.LoLimitFault == 1)
		{
			if(rvdtData.LoLimitFaultOffCounter > 0)
    	    {
    			rvdtData.LoLimitFaultOffCounter--;
    	    }
    	    else
    	    {
    	    	rvdtData.Status.Bits.LoLimitFault = 0;
    	    }
		}
	}
    		
	//check if any fault bit was set
	if (rvdtData.Status.All == 0)
	{
		//update PosPrev if the current position is valid
		rvdtData.PosPrev = rvdtData.Position;
	}
	else
	{
		//use the previous position value
	    rvdtData.Position = rvdtData.PosPrev;
	}
}

/*******************************************************************************************
 * Function: RVDT_ValidateVsum
 *
 * Desc: This function checks if Vsum exceeds its limits
 *
 * Params: None
 *
 * Returns: bool
 *
 * Notes: None
 *
 *******************************************************************************************/
bool RVDT_ValidateVsum(void)
{
	// validate voltage sum
	if (rvdtData.Vsum == 0.0)
	{
		rvdtData.Status.Bits.SumFault = 1;
	}
	else if( (rvdtData.Vsum > RVDT_SUM_LIMIT_HI) || (rvdtData.Vsum < RVDT_SUM_LIMIT_LO) )
	{
	    // increment persistance counter if out of range 
	    rvdtData.SumFaultOnCounter++;

	    // limit persistance counter if out of range and latch fault
	    if(rvdtData.SumFaultOnCounter > RVDT_FAULT_ON_COUNT_MAX)
	    {
			rvdtData.SumFaultOnCounter = 0;
			rvdtData.SumFaultOffCounter = RVDT_FAULT_OFF_COUNT_MAX;
		    rvdtData.Status.Bits.SumFault = 1;
	    }
	}
	else
	{
  	    // decrement persistance counter if it has been set
		if (rvdtData.Status.Bits.SumFault == 1)
		{
    	    if(rvdtData.SumFaultOffCounter > 0)
    	    {
    			rvdtData.SumFaultOffCounter--;
    	    }
    	    else
    	    {
    	    	rvdtData.Status.Bits.SumFault = 0;
    	    }
		}
	}
	
	return (!rvdtData.Status.Bits.SumFault);
}

/*******************************************************************************************
 * Function: RVDT_GetData
 *
 * Desc: This function computes and validates RDVT data
 *
 * Params: None
 *
 * Returns: None
 *
 * Notes:
 *
 *******************************************************************************************/
void RVDT_GetData(void)
{
	// declare variables for ADC values of Va and Vb
	int nVa, nVb;
	
	//get ADC values of Va and Vb
	nVa = ADC_GetRvdtVa();
	nVb = ADC_GetRvdtVb();
	
	//convert to voltage from ADC value
	rvdtData.Va = (float)(nVa * ADC_RVDT_COUNTS_2_VOLTS);
	rvdtData.Vb = (float)(nVb * ADC_RVDT_COUNTS_2_VOLTS);

	//calculate Vsum and Vdiff
	rvdtData.Vsum = rvdtData.Va + rvdtData.Vb;
	rvdtData.Vdiff = rvdtData.Va - rvdtData.Vb;
	
	if(RVDT_ValidateVsum())
	{
	    rvdtData.Position = ((rvdtData.Vdiff / rvdtData.Vsum) * RVDT_POS_GAIN) + RVDT_POS_OFFSET;
		
		//validate position
		RVDT_ValidatePos();
	}
	else
	{
	    //use previous position value if Vsum faults
		rvdtData.Position = rvdtData.PosPrev;
	}
}

/*******************************************************************************************
 * Function: RVDT_GetPosition
 *
 * Desc: This function gets the position value from RVDT
 *
 * Params: None
 *
 * Returns: float position
 *
 * Notes:
 *
 *******************************************************************************************/
FLOAT32 RVDT_GetPosition(void)
{
	return rvdtData.Position;
}

/*******************************************************************************************
 * Function: RVDT_IsDataValid
 *
 * Desc: This function returns data valid status
 *
 * Params: None
 *
 * Returns: bool
 *
 * Notes:
 *
 *******************************************************************************************/
bool RVDT_IsDataValid(void)
{
	return (rvdtData.Status.All == 0);
}
/*******************************************
 End of File
 *******************************************/