/*************************************************************************
$Archive: /pacs/OnBoard/CsCtrl1.c $
$Revision: 1.11 $  $Date: 2009/04/23 13:51:12 $  
$Author: amazy $ 
$Log: CsCtrl1.c,v $
Revision 1.11  2009/04/23 13:51:12  amazy
6.029

 * 
 * 6     9/10/08 12:05 Amazy
 * 
 * 29    3/14/06 4:31p Pacs Egse
 * Version 6.001
 * Cleaned and commented
*************************************************************************/

#ifdef SIMULATOR
#include "virtuosoSim.h"
#else // SIMULATOR
#include "v_macro.h"
#include "node1.h"
#include "k_struct.h"
#endif

#include "params.h"
#include "error.h"

#ifndef SIMULATOR
K_ARGS	gSemaCS1;
#endif //SIMULATOR

int gCurrentCalSrc = 0;
int gCalSrc500msPeriodCounter = 0;

//****************************************************************
// MACRO
//****************************************************************

//macro to signal an error in Cal Src 1 Controller Task
#define SetError(newError) \
{\
	SetGlobalError(newError);\
	LOCK_WRITE_RES_PARAMS;\
	SET_BITS(gpCS1Controller->TaskStatus, K_BMASK_TASK_STATUS_ERROR_STATUS | K_BMASK_TASK_STATUS_ERROR_CODE, K_BMASK_TASK_STATUS_TASK_IN_ERROR + newError); \
	UNLOCK_WRITE_RES_PARAMS;\
}\

//****************************************************************
// FUNCTION DECLARATION
//****************************************************************

extern void* memset(void *, int, size_t);

void InitializeCsControllerParams(CsControllerParams* p_controller);
void ExecuteCSController(CsControllerParams* p_controller);
void InitializeCS1Ctrl();
void CS1Ctrl();

// commands functions
BOOL CommandSwitchOnCS1Ctrl(CommandParameter paramNotUsed);
BOOL CommandSwitchOffCS1Ctrl(CommandParameter paramNotUsed);
BOOL CommandEnableCS1Ctrl(CommandParameter paramNotUsed);
BOOL CommandDisableCS1Ctrl(CommandParameter paramNotUsed);
BOOL CommandSetCS1Temp(CommandParameter param);
BOOL CommandSetCS1Voltage(CommandParameter param);


//****************************************************************
// FUNCTION IMPLEMENTATION
//****************************************************************

/*
FUNCTION : void InitializeCsControllerParams(CsControllerParams* p_controller)
******************************************************************************

AUTHOR : Amazy

Initializes the calibration source controller

PARAMS:
p_controller:	the Calibration source controller to initialize
*/
void InitializeCsControllerParams(CsControllerParams* p_controller)
{
	// init task variables
	p_controller->TaskStatus = K_BMASK_TASK_STATUS_ALIVE | K_BMASK_TASK_STATUS_NO_ERROR_IN_TASK;

	// init pid variables
	p_controller->Output = 0;
	p_controller->Accumulator = 0;
	p_controller->Kp = 1000000;
	p_controller->Ki = 5000;
	p_controller->MaxAccumulatorLimit = 3277;
	p_controller->MinAccumulatorLimit = 1857;
	p_controller->OutputThreshold = 327;
	p_controller->OutputLimit = 0x7FFF;
	p_controller->DacOffset = 0;
	p_controller->Error = 0;
	p_controller->Target = 0;
	p_controller->CurrentResistorValue = -1;
	p_controller->PeriodCounter = 0;

	// init measures variables
	p_controller->Measure1 = 0;
	p_controller->Measure2 = 0;
	p_controller->Measure3 = 0;
	p_controller->Measure4 = 0;
	p_controller->Measure5 = 0;
	p_controller->Measure6 = 0;
	p_controller->Measure7 = 0;
	p_controller->Measure8 = 0;
	p_controller->Voltage0 = 0;
	p_controller->Voltage5neg = 0;
	p_controller->Voltage5pos = 0;
	p_controller->VoltageDacOut = 0;
	p_controller->VoltageSmallGainSensor = 0;
	p_controller->VoltageBigGainSensor = 0;
	p_controller->CurrentSmallGainSensor = 0;
	p_controller->CurrentBigGainSensor = 0;
	p_controller->PosVoltage0 = 0;
	p_controller->PosVoltage5neg = 0;
	p_controller->PosVoltage5pos = 0;
	p_controller->PosVoltageDacOut = 0;
	p_controller->PosVoltageSmallGainSensor = 0;
	p_controller->PosVoltageBigGainSensor = 0;
	p_controller->PosCurrentSmallGainSensor = 0;
	p_controller->PosCurrentBigGainSensor = 0;
	p_controller->NegVoltage0 = 0;
	p_controller->NegVoltage5neg = 0;
	p_controller->NegVoltage5pos = 0;
	p_controller->NegVoltageDacOut = 0;
	p_controller->NegVoltageSmallGainSensor = 0;
	p_controller->NegVoltageBigGainSensor = 0;
	p_controller->NegCurrentSmallGainSensor = 0;
	p_controller->NegCurrentBigGainSensor = 0;
}


/*
FUNCTION : void InitializeCS1Ctrl()
***********************************

AUTHOR : AMazy

USE : 
Initialize Calibration source 1 Controller task.
*/
void InitializeCS1Ctrl()
{
#ifndef SIMULATOR
	//initialize the structure that will be used to signal the semaphore to trigger the task
	gSemaCS1.Srce = 0;
	gSemaCS1.Comm = SIGNALS;
	gSemaCS1.Args.s1.sema = SEMA_TIMER_CS_1;
#endif //SIMULATOR

	// initialize status
	LOCK_WRITE_RES_PARAMS;
	InitializeCsControllerParams(gpCS1Controller);
	UNLOCK_WRITE_RES_PARAMS;
}


/*
TASK_FUNCTION : void CS1Ctrl()
******************************

AUTHOR : AMazy

This function implements the Calibration Source 1 Controller
It is triggerd by a semaphore signaled at regular interval (500ms) in the interrupt routine
*/
void CS1Ctrl()
{
	InitializeCS1Ctrl();

	while (1)
	{
		//wait to be awaken every 500ms if the CS Controller is switched on
		KS_SemaTestW(SEMA_TIMER_CS_1);

		if (TEST_ONE_BIT(gpCS1Controller->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON))
		{
			ExecuteCSController(gpCS1Controller);
		}
	}
}

/*
FUNCTION : void ExecuteCSController(CsControllerParams* p_controller)
*********************************************************************

AUTHOR : AMazy

Perform the control of the calibration source.
This function is called every 500ms.

PARAMS:
p_controller:	the Calibration source controller to run
*/
void ExecuteCSController(CsControllerParams* p_controller)
{
	int* pPosMeasure;	// a pointer to navigate through measures
	int* pNegMeasure;	// a pointer to navigate through measures
	int* pDifMeasure;	// a pointer to navigate through measures
	int* pHkMeasure;	// a pointer to navigate through measures
	
	float floatResistorValue = 0.0f;
	int i =0;

	int periodCounterPositivePulse;
	// CS1 and CS2 do not generate their pulse at the same time
	if (p_controller == gpCS1Controller)
	{
		periodCounterPositivePulse = 0;
	}
	else
	{
		periodCounterPositivePulse = 2;
	}

	//every twenty seconds, reset the period counter
	if (gCalSrc500msPeriodCounter >= K_CS_PERIOD_COUNT)
	{
		gCalSrc500msPeriodCounter = 0;
	}

	// PID AND OUTPUT GENERATION
	////////////////////////////
	
	//every 20 seconds, we execute the PID algorithm and compute the output
	if ((gCalSrc500msPeriodCounter == (periodCounterPositivePulse + 4)) && (p_controller->CurrentResistorValue > 0))
	{
		//PID block
		if (TEST_ONE_BIT(p_controller->TaskStatus, K_BMASK_PID_CTRL_COMMAND_ENABLE_PID))
		{
			//compute the error
			p_controller->Error = ((float)(p_controller->Target - p_controller->CurrentResistorValue))/1000000.0f;
			
			//accumulate the error
			p_controller->Accumulator += p_controller->Error*20.0f; //((float)(PERIOD_COUNT/2)));
			
			// clip the accumulator
			if (p_controller->Accumulator > ((float)(p_controller->MaxAccumulatorLimit))/1000.0f)
			{
				p_controller->Accumulator = ((float)(p_controller->MaxAccumulatorLimit))/1000.0f;
			}
			if (p_controller->Accumulator < ((float)(p_controller->MinAccumulatorLimit))/1000.0f)
			{
				p_controller->Accumulator = ((float)(p_controller->MinAccumulatorLimit))/1000.0f;
			}
			
			// the PID itself
			p_controller->Output = (int)(p_controller->Error*(float)p_controller->Kp + p_controller->Accumulator*(float)p_controller->Ki);
			
			// clip the output
			if (p_controller->Output > p_controller->OutputLimit)
			{
				p_controller->Output = p_controller->OutputLimit;
			}

			if (p_controller->Output < p_controller->OutputThreshold)
			{
				// the output is too small, we enter the 'measure only' mode
				p_controller->Output = 0;
				SET_BIT(p_controller->TaskStatus, K_BMASK_CS_CTRL_STATUS_MEASURE_ONLY);
			}
			else
			{
				// the output is big enough, we stay in 'regulation' mode and start with a negative voltage
				p_controller->Output = -p_controller->Output;
				CLEAR_BIT(p_controller->TaskStatus, K_BMASK_CS_CTRL_STATUS_MEASURE_ONLY);
			}
		}
	}

	// if we are in 'measure only' mode
	if (TEST_ONE_BIT(p_controller->TaskStatus, K_BMASK_CS_CTRL_STATUS_MEASURE_ONLY))
	{
		// during the first 2 periods, generate a negative and positive pulse to make the measure
		if (gCalSrc500msPeriodCounter == periodCounterPositivePulse)
		{
			p_controller->Output = p_controller->OutputThreshold;
		}
		else if (gCalSrc500msPeriodCounter == (periodCounterPositivePulse + 1))
		{
			p_controller->Output = -p_controller->OutputThreshold;
		}
		else
		{
			p_controller->Output = 0;
		}
	}
	else 
	{
		//if we are in 'regulation' mode, every 500ms, change the sign of the output
		if ((gCalSrc500msPeriodCounter%2) == 0)
		{
			if (p_controller->Output < 0)
			{
				p_controller->Output = -p_controller->Output;
			}
		}
		else
		{
			if (p_controller->Output > 0)
			{
				p_controller->Output = -p_controller->Output;
			}
		}
	}

	// RESISTOR MEASUREMENT
	///////////////////////
	
	memcpy(&(p_controller->HkVoltage0), &(p_controller->Measure1), 8*sizeof(int));

	//the ISR accumulates 256 measures, in the hk, we store the averaged value
	pHkMeasure = &(p_controller->HkVoltage0);
	for (i=0; i < 8; i++)
	{
		*pHkMeasure++>>=8;
	}
	
	//every 20 seconds, we copy the measures
	if (gCalSrc500msPeriodCounter == (periodCounterPositivePulse + 1))
	{
		//copy the measure taken with a positive voltage into the array
		memcpy(&(p_controller->PosVoltage0), &(p_controller->Measure1), 8*sizeof(int));
	}
	else if (gCalSrc500msPeriodCounter == (periodCounterPositivePulse + 2))
	{
		//copy the measure taken with a negative voltage into the array
		memcpy(&(p_controller->NegVoltage0), &(p_controller->Measure1), 8*sizeof(int));
		pDifMeasure = &(p_controller->Voltage0);
		pPosMeasure = &(p_controller->PosVoltage0);
		pNegMeasure = &(p_controller->NegVoltage0);

		// we use the difference between pos and neg measure to cancel the ADC offset
		for (i=0; i < 8; i++)
		{
			*pDifMeasure++ = *pPosMeasure++ - *pNegMeasure++;
		}

		// which gain do we have to use to have the best resolution ?
		if (TEST_ONE_BIT(p_controller->TaskStatus, K_BMASK_CS_CTRL_STATUS_USING_BIG_GAIN))
		{
			// is it time to start using the small gain ?
			if ((p_controller->VoltageBigGainSensor > K_CS_BIG_GAIN_VALUE_THRESHOLD) 
				|| (p_controller->CurrentBigGainSensor > K_CS_BIG_GAIN_VALUE_THRESHOLD))
			{
				// signal that we will be using the small gain in the future
				CLEAR_BIT(p_controller->TaskStatus, K_BMASK_CS_CTRL_STATUS_USING_BIG_GAIN);
				
				// and make the first computation with the small gain formula
				floatResistorValue = (float)p_controller->VoltageSmallGainSensor / (float)p_controller->CurrentSmallGainSensor;
			}
			else
			{
				// compute the resistor value with the big gain formula
				floatResistorValue = (float)p_controller->VoltageBigGainSensor / (float)p_controller->CurrentBigGainSensor;
			}
		}
		else
		{
			// is it time to start using the big gain ?
			if ((p_controller->VoltageSmallGainSensor < K_CS_SMALL_GAIN_VALUE_THRESHOLD) 
				&& (p_controller->CurrentSmallGainSensor < K_CS_SMALL_GAIN_VALUE_THRESHOLD))
			{
				// signal that we will be using the big gain in the future
				SET_BIT(p_controller->TaskStatus, K_BMASK_CS_CTRL_STATUS_USING_BIG_GAIN);

				// and make the first computation with the small gain formula
				floatResistorValue = (float)p_controller->VoltageBigGainSensor / (float)p_controller->CurrentBigGainSensor;
			}
			else
			{
				// compute the resistor value with the big gain formula
				floatResistorValue = (float)p_controller->VoltageSmallGainSensor / (float)p_controller->CurrentSmallGainSensor;
			}
		}

		//if the controller is in simulated mode, copy the target in the current resistor value
		if ((p_controller == gpCS1Controller) && TEST_ONE_BIT(gSelectControllersMode, K_BMASK_CTRL_MODE_CS1))
		{
			p_controller->CurrentResistorValue  = p_controller->Target;
		}
		else if ((p_controller == gpCS2Controller) && TEST_ONE_BIT(gSelectControllersMode, K_BMASK_CTRL_MODE_CS2))
		{
			p_controller->CurrentResistorValue  = p_controller->Target;
		}
		else //nominal "real" mode
		{
			// convert the float resistor value into the integer value (unit = 100ľohms)
			p_controller->CurrentResistorValue = (int)(floatResistorValue*1000000);
		}
	}

	//reset the measures such that the ISR starts accumulating from 0 again.
	memset(&(p_controller->Measure1), 0, 8*sizeof(int));
}



/*
CMD_FUNCTION : BOOL CommandSwitchOnCS1Ctrl(CommandParameter paramNotUsed)
*************************************************************************

AUTHOR : Amazy

USE :
Switch on Calibration source 1 controller
*/
BOOL CommandSwitchOnCS1Ctrl(CommandParameter paramNotUsed)
{
	int i=0;

	// change the status and start in 'measure only' mode
	LOCK_WRITE_RES_PARAMS;
	SET_BIT(gpCS1Controller->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON | K_BMASK_CS_CTRL_STATUS_MEASURE_ONLY);
	UNLOCK_WRITE_RES_PARAMS;

	// reset variables
	gpCS1Controller->Output = 0;		
	gpCS1Controller->PeriodCounter = 0;
	gpCS1Controller->Accumulator = 0.0f;

	// validate the houskeeping related to the Calibration source
	VALIDATE_HK(DMC_CS1_RES_VALUE);
	VALIDATE_HK(DMC_CS1_OUTPUT);
	VALIDATE_HK(DMC_CS1_TARGET);

	for (i=DMC_CS1_VOLT_0V; i <= DMC_CS1_CUR_BG; i++)
	{
		VALIDATE_HK(i);
	}

	SequencerSendAck(DMC_SWON_BB_1_CONT);
	return FALSE;
}

/*
CMD_FUNCTION : BOOL CommandSwitchOffCS1Ctrl(CommandParameter paramNotUsed)
**************************************************************************

AUTHOR : Amazy

USE :
Switch off Calibration source 1 controller
*/
BOOL CommandSwitchOffCS1Ctrl(CommandParameter paramNotUsed)
{
	int i=0;

	// make sure output is zero
	gpCS1Controller->Output = 0;

	// change the status
	LOCK_WRITE_RES_PARAMS;
	CLEAR_BIT(gpCS1Controller->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON | K_BMASK_CS_CTRL_STATUS_MEASURE_ONLY | K_BMASK_PID_CTRL_STATUS_PID_ENABLED);
	UNLOCK_WRITE_RES_PARAMS;
	
	// invalidate the houskeeping related to the Calibration source
	INVALIDATE_HK(DMC_CS1_RES_VALUE);
	INVALIDATE_HK(DMC_CS1_OUTPUT);
	INVALIDATE_HK(DMC_CS1_TARGET);

	for (i=DMC_CS1_VOLT_0V; i <= DMC_CS1_CUR_BG; i++)
	{
		INVALIDATE_HK(i);
	}

	SequencerSendAck(DMC_SWOF_BB_1_CONT);
	return FALSE;
}

/*
CMD_FUNCTION : BOOL CommandSetCS1Temp(CommandParameter param)
*************************************************************

AUTHOR : Amazy

USE :
Set the target temperature for Calibration source 1

PARAMS :
Uint: target resistor
*/
BOOL CommandSetCS1Temp(CommandParameter param)
{
	// change the target
	gpCS1Controller->Target = param.Uint;

	SequencerSendAck(DMC_SET_TEMP_BB_1);
	return FALSE;
}

/*
CMD_FUNCTION : BOOL CommandSetCS1Voltage(CommandParameter param)
****************************************************************

AUTHOR : Amazy

USE :
Set the voltage directly for calibration source 1

PARAMS :
Int voltage.
*/
BOOL CommandSetCS1Voltage(CommandParameter param)
{
	// the command is available only if the PID is disabled
	if (TEST_ONE_BIT(gpCS1Controller->TaskStatus, K_BMASK_PID_CTRL_STATUS_PID_ENABLED))
	{	
		SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND);
		SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND);
		return FALSE;
	}

	// directly set the output to the value requested
	gpCS1Controller->Output = param.Int;

	if (param.Int != 0) 
	{
		// if we apply a voltage, don't need to be in measure mode
		CLEAR_BIT(gpCS1Controller->TaskStatus, K_BMASK_CS_CTRL_STATUS_MEASURE_ONLY);
	}
	else
	{
		// if no voltage is applied, go back in measure only mode
		SET_BIT(gpCS1Controller->TaskStatus, K_BMASK_CS_CTRL_STATUS_MEASURE_ONLY);
	}

	SequencerSendAck(DMC_SET_BB_1_VOLTAGE);
	return FALSE;
}


/*
CMD_FUNCTION : BOOL CommandEnableCS1Ctrl(CommandParameter paramNotUsed)
***********************************************************************

AUTHOR : Amazy

USE :
Enable the CS1 controller
*/
BOOL CommandEnableCS1Ctrl(CommandParameter paramNotUsed)
{
	// the CS must already be powered on
	if (!TEST_ONE_BIT(gpCS1Controller->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON | K_BMASK_CS_CTRL_STATUS_MEASURE_ONLY))
	{	
		SequencerSendNack(0, COULD_NOT_EXECUTE_COMMAND);
		SetSequencerError(ERR_SEQUENCER_COULD_NOT_EXECUTE_COMMAND);
		return FALSE;
	}

	// change the status
	SET_BIT(gpCS1Controller->TaskStatus, K_BMASK_PID_CTRL_STATUS_PID_ENABLED);

	SequencerSendAck(DMC_ENABLE_BB_1_CONT);
	return FALSE;
}

/*
CMD_FUNCTION : BOOL CommandDisableCS1Ctrl(CommandParameter paramNotUsed)
************************************************************************

AUTHOR : Amazy

USE :
Disable the CS1 controller
*/
BOOL CommandDisableCS1Ctrl(CommandParameter paramNotUsed)
{
	//change the status
	LOCK_WRITE_RES_PARAMS;
	CLEAR_BIT(gpCS1Controller->TaskStatus, K_BMASK_PID_CTRL_STATUS_PID_ENABLED);
	SET_BIT(gpCS1Controller->TaskStatus, K_BMASK_CS_CTRL_STATUS_MEASURE_ONLY);
	UNLOCK_WRITE_RES_PARAMS;

	SequencerSendAck(DMC_DISABLE_BB_1_CONT);
	return FALSE;
}