/*************************************************************************
$Archive: /pacs/OnBoard/pid_ctrl.c $
$Revision: 8 $  $Date: 11/10/08 11:23a $  
$Author: Pacs1 $ 
$Log: /pacs/OnBoard/pid_ctrl.c $
 * 
 * 8     11/10/08 11:23a Pacs1
 * 
 * 7     9/10/08 12:05 Amazy
 * 
 * 6     30/09/08 15:19 Amazy
 * - increased grating power limit to 150mA and made it commandable
 * - added a synchro counter in HK
 * - new parameters value for DMC_SYNCHRONIZE_ON_DET to avoid using the
 * synchro to trigger the mechanisms move
 * - In photo, the RedSpuTransmission mode was not copied into red science
 * headers.  It is now corrected
 * 
 * 5     3/20/07 3:14p Amazy
 * 
 * 37    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 "u_bits.h"
#include "hk.h"
#include "params.h"


/*
FUNCTION : void InitializePidControllerParams(PidControllerParams* p_controller)
********************************************************************************

AUTHOR : Amazy

USE :
This function initializes the parameters that are common to every PID controller

PARAMS :
p_controller : a pointer to a PID controller
*/
void InitializePidControllerParams(PidControllerParams* p_controller)
{
	//common initialization
	p_controller->CurrentPosition = 0; 
	p_controller->Output = 0;		
	p_controller->Error = 0;		 
	p_controller->Accumulator = 0;	 
	p_controller->CurrentSetPoint = 0; 
	p_controller->PidStatus = 0;		 
	p_controller->ControlWord = 0;	 
	p_controller->Target = 0;			 
	p_controller->TaskStatus = 0;
	p_controller->PosOffset = 0;
}


/*
FUNCTION : BOOL EnablePidController(PidControllerParams* p_controller)
**********************************************************************

AUTHOR : Amazy

USE :
Enable a PID controller

PARAMS :
p_controller : a pointer to a PID controller

RETURN VALUE :
TRUE		: if the PID controller could be enabled
FALSE		: if the PID controller could not be enabled
*/
BOOL EnablePidController(PidControllerParams* p_controller)
{
	// if the controller is not switched on, reject the command
	if (!TEST_ONE_BIT(p_controller->TaskStatus, K_BMASK_PID_CTRL_STATUS_POWER_ON))
	{
		return FALSE;
	}

	// reset the integral accumulator
	p_controller->Accumulator = 0;

	if (p_controller == gpGratingPidController)
	{
		// set the current setpoint to current position
		p_controller->CurrentSetPoint = p_controller->CurrentPosition;
		
		// set the current target to current position
		p_controller->Target = p_controller->CurrentPosition;
	}
	else if (p_controller == gpChopperPidController)

	{
		//if we are in open loop, the setpoint must be zero because the setpoint will be copied in the DAC
		if (TEST_ONE_BIT(p_controller->PidStatus, K_BMASK_CHOP_CTRL_STATUS_OPEN_LOOP))
		{
			// set the current chopper setpoint to zero
			p_controller->CurrentSetPoint = 0;
			
			// set the current chopper target to zero
			p_controller->Target = 0;
		}
		else
		{
			// set the current setpoint to current position
			p_controller->CurrentSetPoint = p_controller->CurrentPosition;
			
			// set the current target to current position
			p_controller->Target = p_controller->CurrentPosition;
		}
	}
	else
	{
		// set the current chopper setpoint to zero
		p_controller->CurrentSetPoint = 0;
		
		// set the current chopper target to zero
		p_controller->Target = 0;
	}

	// reset the move bits
	CLEAR_BIT(p_controller->ControlWord, K_BMASK_PID_CTRL_COMMAND_MOVE_COMMAND);

	// enable controller and commutation
	p_controller->ControlWord  = K_BMASK_PID_CTRL_COMMAND_ENABLE_PID | K_BMASK_PID_CTRL_COMMAND_ENABLE_COMMUTATION;
	
#ifdef SIMULATOR
	SET_BIT(p_controller->PidStatus, K_BMASK_PID_CTRL_STATUS_PID_ENABLED);
#endif
	return TRUE;
}


/*
FUNCTION : BOOL StartMovingToTarget(PidControllerParams* p_controller, int target)
**********************************************************************************

AUTHOR : Amazy

USE :
Start a movement for a PID controller

PARAMS :
p_controller	: a pointer to a PID controller
int				: the target

RETURN VALUE :
TRUE		: if the movement could be started
FALSE		: if not
*/
BOOL StartMovingToTarget(PidControllerParams* p_controller, int target)
{
	int controlWord;

	// if the controller is not enabled, return FALSE
	if (!TEST_ONE_BIT(p_controller->PidStatus, K_BMASK_PID_CTRL_STATUS_PID_ENABLED))
	{
		return FALSE;
	}

	// test if previous move is finished
	if ((p_controller->PidStatus & K_BMASK_PID_CTRL_STATUS_MOVING_STATUS) != 0)
	{
		return FALSE;
	}

	// set the target
	p_controller->Target = target;

	if (target > p_controller->CurrentSetPoint)
	{
		 controlWord = K_BMASK_PID_CTRL_COMMAND_MOVE_UP;
	}
	else 
	{
		controlWord = K_BMASK_PID_CTRL_COMMAND_MOVE_DOWN;
	}

	if (gpMim->MechanismsUseSynchro)
	{
		controlWord += K_BMASK_PID_CTRL_COMMAND_USE_SYNCHRO;
	}

	p_controller->ControlWord = controlWord;

	return TRUE;
}


/*
COMMAND : BOOL CommandSelectMechControlMode(CommandParameter param)
*******************************************************************

AUTHOR : Amazy

USE :
Select in which mode each of the controller is operated

PARAMS :
bit field:
bit0:
0 = grating use nominal controller
1 = grating is simulated (pos = setpoint)
bit1:
0 = chopper use nominal controller
1 = chopper is simulated (pos = setpoint)
bit2:
0 = fw_spec use nominal position
1 = fw_spec use simulated position
bit3:
0 = fw_photo use nominal position
1 = fw_photo use simulated position
bit4:
0 = CS1 use nominal resistor reading
1 = CS1 is simulated (resistor = setpoint)
bit5:
0 = CS2 use nominal resistor reading
1 = CS2 is simulated (resistor = setpoint)
bit6:
0 = no synchro diagnostic
1 = OBS enters a diagnostic mode for synchro
*/
BOOL CommandSelectMechControlMode(CommandParameter param)
{
	gSelectControllersMode = param.Int;

	SequencerSendAck(DMC_SELECT_MECH_CTRL_MODE);

	return TRUE;
}