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

 * 
 * 12    3/14/06 4:32p 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	gSemaTempSensors;
#endif //SIMULATOR

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

//macro to signal an error in Temperature Sensor Task
#define SetError(newError) \
{\
	SetGlobalError(newError);\
	LOCK_WRITE_RES_PARAMS;\
	SET_BITS(gpTempSensors->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;\
}\

/****************************************************************
GLOBAL VARIABLES
****************************************************************/

enum
{
	INCREASING_PWM,
	WAIT_BEFORE_ADDING_NDIV_1,
	WAIT_BEFORE_ADDING_NDIV_0,
	WAIT_AFTER_NDIV_ADDED,
	WAIT_BEFORE_NEW_CYCLE_2,
	WAIT_BEFORE_NEW_CYCLE_1,
	WAIT_BEFORE_NEW_CYCLE_0,
	WAIT_BEFORE_POS_MEASURE,
	READY_FOR_POS_MEASURE,
	READY_FOR_NEG_MEASURE
};


/****************************************************************
FUNCTIONS DECLARATION
****************************************************************/

void InitializeTempSensors();
void SetResistorValue(int i, int resistorValue);
int	 GetTempSensorVoltage(int i);
int	 GetTempSensorCurrent(int i, int isLowGain);
void InitForNewMeasureCycle();
void TempSensors();

// commands functions
BOOL CommandSwitchOnTempSensors(CommandParameter paramNotUsed);
BOOL CommandSwitchOffTempSensors(CommandParameter paramNotUsed);

/****************************************************************
FUNCTIONS IMPLEMENTATION
****************************************************************/

/*
FUNCTION : void InitializeTempSensors()
***************************************

AUTHOR : Amazy

Initializes the temperature sensor task
*/
void InitializeTempSensors()
{
	// init task variables
	gpTempSensors->TaskStatus = K_BMASK_TASK_STATUS_ALIVE | K_BMASK_TASK_STATUS_NO_ERROR_IN_TASK;
	gpTempSensors->HkHighOrLowGain = 0;

#ifndef SIMULATOR
	//initialize the structure that will be used to signal the semaphore to trigger the task
	gSemaTempSensors.Srce = 0;
	gSemaTempSensors.Comm = SIGNALS;
	gSemaTempSensors.Args.s1.sema = SEMA_TEMP_SENSORS;
#endif //SIMULATOR

	InitForNewMeasureCycle();
}

/*
FUNCTION : void SetResistorValue(int i, int resistorValue)
**********************************************************

AUTHOR : Amazy

Helper function to store the resistor values

PARAMS:
i:				id of the sensor
resistorValue:	the resistor value of this sensor
*/
void SetResistorValue(int i, int resistorValue)
{
	switch (i)
	{
	case 0:
		gpTempSensors->ChopperTemperatureSensorResistorValue = resistorValue;
		return;
	case 1:
		gpTempSensors->BB1TemperatureSensorResistorValue = resistorValue;
		return;
	case 2:
		gpTempSensors->FpuTemperatureSensor1ResistorValue = resistorValue;
		return;
	case 3:
		gpTempSensors->FpuTemperatureSensor2ResistorValue = resistorValue;
		return;
	case 4:
		gpTempSensors->GratingTemperatureSensorResistorValue = resistorValue;
		return;
	case 5:
		gpTempSensors->FWSpecTemperatureSensorResistorValue = resistorValue;
		return;
	case 6:
		gpTempSensors->FWPhotoTemperatureSensorResistorValue = resistorValue;
		return;
	}
}

/*
FUNCTION : int GetTempSensorVoltage(int i)
******************************************

AUTHOR : Amazy

Helper function to get the sensor voltage

PARAMS:
i:		id of the sensor
*/
int GetTempSensorVoltage(int i)
{
	switch (i)
	{
	case 0:
		return gpMim->HkAnalogMeasures.ChopperTemperatureSensorVoltage;
	case 1:
		return gpMim->HkAnalogMeasures.BB1TemperatureSensorVoltage;
	case 2:
		return gpMim->HkAnalogMeasures.FpuTemperatureSensor1Voltage;
	case 3:
		return gpMim->HkAnalogMeasures.FpuTemperatureSensor2Voltage;
	case 4:
		return gpMim->HkAnalogMeasures.GratingTemperatureSensorVoltage;
	case 5:
		return gpMim->HkAnalogMeasures.FWSpecTemperatureSensorVoltage;
	case 6:
		return gpMim->HkAnalogMeasures.FWPhotoTemperatureSensorVoltage;			
	}

	return 0;
}

/*
FUNCTION : int GetTempSensorCurrent(int i, int isLowGain)
*********************************************************

AUTHOR : Amazy

Helper function to get the sensor current

PARAMS:
i:				id of the sensor
isLowGain:		chose between the low and high gain measures
*/
int GetTempSensorCurrent(int i, int isLowGain)
{
	if (isLowGain)
	{
		if (i < 4)
		{
			return gpMim->HkAnalogMeasures.TempSensorsSrc1CurrentLowGain;
		}
		else
		{
			return gpMim->HkAnalogMeasures.TempSensorsSrc2CurrentLowGain;
		}
	}
	else
	{
		if (i < 4)
		{
			return gpMim->HkAnalogMeasures.TempSensorsSrc1CurrentHighGain;
		}
		else
		{
			return gpMim->HkAnalogMeasures.TempSensorsSrc2CurrentHighGain;
		}
	}
}

/*
FUNCTION : void InitForNewMeasureCycle()
****************************************

AUTHOR : Amazy

Resets all variables before starting a new measure cycle
*/
void InitForNewMeasureCycle()
{
	int i=0;

	// reset the current injected in the sensors
	SET_BITS(gpFPGA->ControlRegister, K_BMASK_TIMING_HK, K_BMASK_TIMING_HK_ENB | K_BMASK_TIMING_HK_POSNEG);
	gpTempSensors->CurrentPWM = 10;

	// reset the measure status
	for (i=0; i < 7; ++i)
	{
		gpTempSensors->MeasuresStatus[i] = TEMP_SENS_NOT_MEASURED;
		gpTempSensors->TempMeasures[i] = 0;
	}

#ifndef SIMULATOR
	//write PWM to registers
	*((int*)TIMING_CTRL_REG) = gpFPGA->ControlRegister;
	*((int*)HK_PWM) = gpTempSensors->CurrentPWM;
#endif //SIMULATOR

}

/*
TASK_FUNCTION : void TempSensors()
**********************************

AUTHOR : Amazy

This is the function implementing the temperature sensor task.
It is triggered at regular interval by the interrupt routine.
The ISR provides a 500ms wake-up interval.

The current injected in the sensors is incremented until one of the sensor reaches 2mV.
Then, the current is inverted to have a positive and a negative measure (to cancel the offset errors).
Then, the current injected is incremented again until another sensor reaches 2mV.
When all the sensors have been measured (or when the current has reached its maximum value), the cycle starts again.
*/
void TempSensors()
{
	int i=0;
	int taskStatus = INCREASING_PWM;
	int currentHighGain;
	float resistorValue;

	InitializeTempSensors();

	while(1)
	{
		//should be woken up every 500ms when all measures are valid
		KS_SemaTestW(SEMA_TEMP_SENSORS);

		if (TEST_ONE_BIT(gpTempSensors->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON))
		{
			if (taskStatus == INCREASING_PWM)
			{
				for (i=0; i < 7; ++i)
				{
					if ((gpTempSensors->MeasuresStatus[i] == TEMP_SENS_NOT_MEASURED) && (GetTempSensorVoltage(i) > 4587))  // if voltage > 2mV (ADC Range = -10V/+10V, conversion: 1.4V/2mV)
					{
						gpTempSensors->MeasuresStatus[i] = TEMP_SENS_TO_BE_MEASURED;
						taskStatus = WAIT_BEFORE_POS_MEASURE;
					}
				}

				// if the status is still 'to be measured', we can increase the PWM value
				if (taskStatus == INCREASING_PWM)
				{
					// compute next PWM value
					gpTempSensors->CurrentPWM += gpTempSensors->CurrentPWM/8;

					// if we reached the maximum PWM value
					if (gpTempSensors->CurrentPWM > 511)
					{
						// if not yet done, change the gain
						if (!TEST_ONE_BIT(gpFPGA->ControlRegister, K_BMASK_TIMING_HK_NDIV25))
						{
#ifndef SIMULATOR
							gpTempSensors->CurrentPWM = 10;
							//write PWM to registers
							*((int*)HK_PWM) = gpTempSensors->CurrentPWM;
#endif //SIMULATOR

							//wait 1 sec and change NDIV25 to avoid the overshoot of the amplifier
							taskStatus = WAIT_BEFORE_ADDING_NDIV_1;
						}
						else //otherwise, it is not possible to increment PWM anymore
						{
							// all sensors that have not been measured are set to 'Error'
							for (i=0; i < 7; ++i)
							{
								if (gpTempSensors->MeasuresStatus[i] == TEMP_SENS_NOT_MEASURED)
								{
									gpTempSensors->TempMeasures[i] = 0xFFFFFFFF;
									SET_BITS(gpTempSensors->HkHighOrLowGain, (K_BMASK_TEMP_SENS_HK_STATUS << (i*2)), (K_BMASK_TEMP_SENS_HK_STATUS_ERROR << (i*2)));
								}
							}

							SetResistorValue(i, gpTempSensors->TempMeasures[i]);

#ifndef SIMULATOR
							gpTempSensors->CurrentPWM = 10;
							//write PWM to registers
							*((int*)HK_PWM) = gpTempSensors->CurrentPWM;
#endif //SIMULATOR
							taskStatus = WAIT_BEFORE_NEW_CYCLE_2;
						}
					}
					else
					{
#ifndef SIMULATOR
						//write PWM to registers
						*((int*)HK_PWM) = gpTempSensors->CurrentPWM;
#endif //SIMULATOR
					}
				}
			}
			else if (taskStatus == WAIT_BEFORE_ADDING_NDIV_1)
			{
				taskStatus = WAIT_BEFORE_ADDING_NDIV_0;
			}
			else if (taskStatus == WAIT_BEFORE_ADDING_NDIV_0)
			{
				SET_BIT(gpFPGA->ControlRegister, K_BMASK_TIMING_HK_NDIV25);
#ifndef SIMULATOR
				*((int*)TIMING_CTRL_REG) = gpFPGA->ControlRegister;	
#endif //SIMULATOR

				taskStatus = WAIT_AFTER_NDIV_ADDED;
			}
			else if (taskStatus == WAIT_AFTER_NDIV_ADDED)
			{
				taskStatus = INCREASING_PWM;
			}
			else if (taskStatus == WAIT_BEFORE_NEW_CYCLE_2)
			{
				taskStatus = WAIT_BEFORE_NEW_CYCLE_1;
			}
			else if (taskStatus == WAIT_BEFORE_NEW_CYCLE_1)
			{
				// put PWM back in starting conditions
				InitForNewMeasureCycle();

				taskStatus = WAIT_BEFORE_NEW_CYCLE_0;
			}
			else if (taskStatus == WAIT_BEFORE_NEW_CYCLE_0)
			{
				taskStatus = INCREASING_PWM;
			}
			else if (taskStatus == WAIT_BEFORE_POS_MEASURE)
			{
				//wait 500ms more before doing the measure
				taskStatus = READY_FOR_POS_MEASURE;
			}
			else if (taskStatus == READY_FOR_POS_MEASURE)  // at least one sensor is ready to be measured
			{
				for (i=0; i < 7; ++i)
				{
					if (gpTempSensors->MeasuresStatus[i] == TEMP_SENS_TO_BE_MEASURED)
					{
						//store the positive voltage for these sensors
						gpTempSensors->PosVoltages[i] = GetTempSensorVoltage(i);

						//store the positive current for these sensors.
						//if the High Gain current is near saturation, use the small gain
						currentHighGain = GetTempSensorCurrent(i, FALSE);
						if (currentHighGain > 30000)
						{
							gpTempSensors->PosCurrents[i] = GetTempSensorCurrent(i, TRUE);
							gpTempSensors->HighOrLowGain[i] = TEMP_SENS_LOW_GAIN;
						}
						else
						{
							gpTempSensors->PosCurrents[i] = currentHighGain;
							gpTempSensors->HighOrLowGain[i] = TEMP_SENS_HIGH_GAIN;
						}
					}
				}
				CLEAR_BIT(gpFPGA->ControlRegister, K_BMASK_TIMING_HK_POSNEG);
#ifndef SIMULATOR
				*((int*)TIMING_CTRL_REG) = gpFPGA->ControlRegister;	
#endif //SIMULATOR
				taskStatus = READY_FOR_NEG_MEASURE;
			}
			else if (taskStatus == READY_FOR_NEG_MEASURE)
			{
				int measuresCompleted = 0;

				for (i=0; i < 7; ++i)
				{
					if (gpTempSensors->MeasuresStatus[i] == TEMP_SENS_TO_BE_MEASURED)
					{
						//the measure for these sensors is now completed, store the negative voltage
						gpTempSensors->NegVoltages[i] = GetTempSensorVoltage(i);

						//for the current, use the same gain as for the negative measure
						if (gpTempSensors->HighOrLowGain[i] == TEMP_SENS_LOW_GAIN)
						{
							gpTempSensors->NegCurrents[i] = GetTempSensorCurrent(i, TRUE);
						}
						else
						{
							gpTempSensors->NegCurrents[i] = GetTempSensorCurrent(i, FALSE);
						}
		
						//mark the measure as completed
						gpTempSensors->MeasuresStatus[i] = TEMP_SENS_MEASURED;

						resistorValue = ((float)(gpTempSensors->PosVoltages[i] - gpTempSensors->NegVoltages[i]))/((float)(gpTempSensors->PosCurrents[i] - gpTempSensors->NegCurrents[i]));						

						if (gpTempSensors->HighOrLowGain[i] == TEMP_SENS_LOW_GAIN)
						{
							gpTempSensors->TempMeasures[i] = (int)(resistorValue*128.0f);  // conversion to have the resistor value expressed in  ohms units
							SET_BITS(gpTempSensors->HkHighOrLowGain, (K_BMASK_TEMP_SENS_HK_STATUS << (i*2)), (K_BMASK_TEMP_SENS_HK_STATUS_LOW_GAIN << (i*2)));
						}
						else
						{
							gpTempSensors->TempMeasures[i] = (int)(resistorValue*3162.0f);	// conversion to have the resistor value expressed in ohms units
							SET_BITS(gpTempSensors->HkHighOrLowGain, (K_BMASK_TEMP_SENS_HK_STATUS << (i*2)), (K_BMASK_TEMP_SENS_HK_STATUS_HIGH_GAIN << (i*2)));
						}

						SetResistorValue(i, gpTempSensors->TempMeasures[i]);
					}
				}

				// this measure is completed, test if all the measures have been done
				for (i=0; i < 7; ++i)
				{
					if (gpTempSensors->MeasuresStatus[i] == TEMP_SENS_MEASURED)
					{
						measuresCompleted++;
					}
				}

				// if all measures are done, start a new sequence of measure
				if (measuresCompleted == 7)
				{
#ifndef SIMULATOR
					gpTempSensors->CurrentPWM = 10;
					//write PWM to registers
					*((int*)HK_PWM) = gpTempSensors->CurrentPWM;
#endif //SIMULATOR
					taskStatus = WAIT_BEFORE_NEW_CYCLE_2;
				}
				else //continue to increase PWM for the other sensors
				{
					SET_BIT(gpFPGA->ControlRegister, K_BMASK_TIMING_HK_POSNEG);
#ifndef SIMULATOR
					*((int*)TIMING_CTRL_REG) = gpFPGA->ControlRegister;	
#endif //SIMULATOR
					taskStatus = INCREASING_PWM;
				}
			}
		}
	}
}

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

AUTHOR : Amazy

USE :
Switch on the Temperature Sensors in FPU
*/
BOOL CommandSwitchOnTempSensors(CommandParameter paramNotUsed)
{
	SET_BIT(gpTempSensors->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON);
	SET_BIT(gpFPGA->ControlRegister, K_BMASK_TIMING_HK_ENB);
#ifndef SIMULATOR
	*((int*)TIMING_CTRL_REG) = gpFPGA->ControlRegister;		//tell hardware to start sending current in sensors
#endif //SIMULATOR

	//mark all measures as invalid (they will be updated soon)
	gpTempSensors->HkHighOrLowGain = 0;

	//give them an 'invalid' value until they are updated by the TempSensors task
	gpTempSensors->ChopperTemperatureSensorResistorValue = -1;
	gpTempSensors->BB1TemperatureSensorResistorValue = -1;
	gpTempSensors->FpuTemperatureSensor1ResistorValue = -1;
	gpTempSensors->FpuTemperatureSensor2ResistorValue = -1;
	gpTempSensors->GratingTemperatureSensorResistorValue = -1;
	gpTempSensors->FWSpecTemperatureSensorResistorValue = -1;
	gpTempSensors->FWPhotoTemperatureSensorResistorValue = -1;
	
	//validate them (they have an invalid value)
	VALIDATE_HK(DMC_FW_SPEC_TEMP);
	VALIDATE_HK(DMC_FW_PHOT_TEMP);
	VALIDATE_HK(DMC_CHOPPER_TEMP);
	VALIDATE_HK(DMC_GRATING_TEMP);
	VALIDATE_HK(DMC_FPU_T1_T);
	VALIDATE_HK(DMC_FPU_T2_T);
	VALIDATE_HK(DMC_CAL_SRC_TEMP);

	//acknowledge the command
	SequencerSendAck(DMC_SWON_TEMP_SENSORS);

	return FALSE;
}

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

AUTHOR : Amazy

USE :
Switch off the Temperature Sensors in FPU
*/
BOOL CommandSwitchOffTempSensors(CommandParameter paramNotUsed)
{
	CLEAR_BIT(gpTempSensors->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON);
	CLEAR_BIT(gpFPGA->ControlRegister, K_BMASK_TIMING_HK_ENB);
#ifndef SIMULATOR
	*((int*)TIMING_CTRL_REG) = gpFPGA->ControlRegister;	
#endif //SIMULATOR

	SequencerSendAck(DMC_SWON_TEMP_SENSORS);

	//mark all measures as invalid (they won't be updated anymore)
	gpTempSensors->HkHighOrLowGain = 0;

	INVALIDATE_HK(DMC_FW_SPEC_TEMP);
	INVALIDATE_HK(DMC_FW_PHOT_TEMP);
	INVALIDATE_HK(DMC_CHOPPER_TEMP);
	INVALIDATE_HK(DMC_GRATING_TEMP);
	INVALIDATE_HK(DMC_FPU_T1_T);
	INVALIDATE_HK(DMC_FPU_T2_T);
	INVALIDATE_HK(DMC_CAL_SRC_TEMP);

	return FALSE;
}