/************************************************************************** * * Copyright (c) 2026-2026 Diality Inc. - All Rights Reserved. * * THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN * WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER. * * @file PinchValve.c * * @author (last) Varshini Nagabooshanam * @date (last) 08-Jul-2026 * * @author (original) Varshini Nagabooshanam * @date (original) 08-Jul-2026 * ***************************************************************************/ #include "Common.h" #include "FpgaTD.h" #include "PinchValve.h" #include "RotaryValve.h" #include "Timers.h" #include "Utilities.h" /** * @addtogroup PinchValve * @{ */ // ********** private definitions ********** #define PINCH_VALVE_STATUS_POLL_INTERVAL_MS 10 ///< FPGA status polling interval in (ms). #define PINCH_VALVE_COMMAND_TIMEOUT_MS 1000 ///< Maximum FPGA command execution time. #define PINCH_VALVE_DEFAULT_VELOCITY 3342 #define PINCH_VALVE_FPGA_CMD_START_BIT_MASK 0x01 ///< Bit 0 starts a command transaction. (TODO: To change the comments later) #define PINCH_VALVE_FPGA_CMD_WRITE_ONLY_BIT_MASK 0x02 ///< Bit 1 selects a write-only command. #define PINCH_VALVE_FPGA_CMD_TX_COUNT_SHIFT 2 ///< Bits 4-2 contain transmit word count. #define PINCH_VALVE_FPGA_CMD_RX_COUNT_SHIFT 5 ///< Bits 7-5 contain receive word count. #define PINCH_VALVE_FPGA_CMD_WORD_COUNT_MASK 0x07 ///< Three-bit FPGA word-count mask. #define PINCH_VALVE_SPI_DONE_BIT_MASK 0x01 ///< SPI command completed. #define PINCH_VALVE_SPI_TX_COUNT_MASK 0x0E ///< SPI status bits 3-1: transmitted words. #define PINCH_VALVE_SPI_TX_COUNT_SHIFT 1 #define PINCH_VALVE_SPI_RX_COUNT_MASK 0x30 ///< SPI status bits 5-4: received words. #define PINCH_VALVE_SPI_RX_COUNT_SHIFT 4 #define PINCH_VALVE_SPI_CONTROLLER_FAULT_BIT_MASK 0x40 ///< SPI status bit 6: controller fault. #define PINCH_VALVE_ENABLE_BIT_MASK 0x01 ///< Enable controller. #define PINCH_VALVE_RESET_BIT_MASK 0x02 ///< Reset controller. #define PINCH_VALVE_MAX_INPUT_WORDS 3 ///< Maximum number of command input words. #define PINCH_VALVE_MAX_OUTPUT_WORDS 3 ///< Maximum number of command output words. #define PINCH_VALVE_MAX_TRANSMIT_WORDS 4 #define PINCH_VALVE_PMD_CMD_SET_POSITION 0x0010 #define PINCH_VALVE_PMD_CMD_SET_VELOCITY 0x0011 #define PINCH_VALVE_PMD_CMD_UPDATE 0x001A #define PINCH_VALVE_PMD_CMD_GET_EVENT_STATUS 0x0031 #define PINCH_VALVE_PMD_CMD_RESET_EVENT_STATUS 0x0034 #define PINCH_VALVE_PMD_CMD_GET_ACTUAL_POSITION 0x0037 #define PINCH_VALVE_PMD_CMD_SET_ACTUAL_POSITION 0x004D #define PINCH_VALVE_PMD_CMD_SET_PROFILE_MODE 0x00A0 #define PINCH_VALVE_PMD_CMD_GET_ACTIVITY_STATUS 0x00A6 #define PINCH_VALVE_PROFILE_MODE_VELOCITY 1 #define PINCH_VALVE_PROFILE_MODE_S_CURVE 2 #define PINCH_VALVE_RESET_EVENT_STATUS_VALUE 0xFFEE #define PINCH_VALVE_ACTIVITY_PHASE_INITIALIZED_MASK 0x0001 #define PINCH_VALVE_ACTIVITY_AXIS_SETTLED_MASK 0x0080 #define PINCH_VALVE_EVENT_MOTION_COMPLETE_MASK 0x0001 #define PINCH_VALVE_EVENT_MOTION_ERROR_MASK 0x0010 static const S32 HOMING_VELOCITY_FWD = 3342; ///< Forward velocity used while moving toward the first homing hard stop. static const S32 HOMING_VELOCITY_REV = -3342; ///< Reverse velocity used while moving toward the opposite homing hard stop. static const S32 POSITION_VELOCITY = 3342; ///< Velocity used when moving the pinch valve to a requested position. /// Pinch valve function state machine. typedef enum pinchValveFunctionStates { PINCH_VALVE_FUNCTION_IDLE_STATE = 0, PINCH_VALVE_FUNCTION_SEND_STATE, PINCH_VALVE_FUNCTION_WAIT_STATE } PINCH_VALVE_FUNCTION_STATE_T; /// Low-level FPGA command state machine. typedef enum pinchValveCommandStates { PINCH_VALVE_CMD_IDLE_STATE = 0, PINCH_VALVE_CMD_SET_CMD_STATE, PINCH_VALVE_CMD_XMIT_CMD_STATE, PINCH_VALVE_CMD_WAIT_DONE_STATE, PINCH_VALVE_CMD_READ_WORDS_STATE, PINCH_VALVE_CMD_ERROR_STATE } PINCH_VALVE_CMD_STATE_T; /// Pinch valve command request. typedef struct { U16 cmdHeader; ///< Command header. U16 inputWord1; ///< Input word 1. U16 inputWord2; ///< Input word 2. U16 inputWord3; ///< Input word 3. U08 inputWordCount; ///< Number of valid input words, excluding the command header. U08 outputWordCount; ///< Number of output words expected. BOOL writeOnly; ///< TRUE for a write-only command. } PINCH_VALVE_COMMAND_T; /// Pinch Valve Command for Homing. typedef enum pinchValveHomingCommandIndexes { PINCH_VALVE_HOME_CMD_SET_PROFILE_MODE = 0, PINCH_VALVE_HOME_CMD_RESET_EVENTS_B, PINCH_VALVE_HOME_CMD_SET_VELOCITY_B, PINCH_VALVE_HOME_CMD_UPDATE_B, PINCH_VALVE_HOME_CMD_GET_ACTIVITY_STATUS, PINCH_VALVE_HOME_CMD_GET_EVENT_STATUS_B, PINCH_VALVE_HOME_CMD_SET_ACTUAL_POSITION_ZERO, PINCH_VALVE_HOME_CMD_RESET_EVENTS_C, PINCH_VALVE_HOME_CMD_SET_VELOCITY_C, PINCH_VALVE_HOME_CMD_UPDATE_C, PINCH_VALVE_HOME_CMD_GET_EVENT_STATUS_C, PINCH_VALVE_HOME_CMD_GET_ACTUAL_POSITION_C, PINCH_VALVE_HOME_CMD_RESET_EVENTS_COMPLETE, NUM_OF_PINCH_VALVE_HOMING_COMMANDS } PINCH_VALVE_HOMING_COMMAND_INDEX_T; /// Pinch Valve Command for Set-Position. typedef enum pinchValvePositionCommandIndexes { PINCH_VALVE_POSITION_CMD_SET_PROFILE_MODE = 0, PINCH_VALVE_POSITION_CMD_SET_VELOCITY, PINCH_VALVE_POSITION_CMD_GET_ACTIVITY_STATUS, PINCH_VALVE_POSITION_CMD_GET_EVENT_STATUS_BEFORE, PINCH_VALVE_POSITION_CMD_RESET_EVENT_STATUS, PINCH_VALVE_POSITION_CMD_SET_POSITION, PINCH_VALVE_POSITION_CMD_UPDATE, PINCH_VALVE_POSITION_CMD_GET_EVENT_STATUS_AFTER, PINCH_VALVE_POSITION_CMD_GET_ACTUAL_POSITION, PINCH_VALVE_POSITION_CMD_RESET_EVENT_STATUS_COMPLETE, NUM_OF_PINCH_VALVE_POSITION_COMMANDS } PINCH_VALVE_POSITION_COMMAND_INDEX_T; // ********** private data ********** // This array is used for both H1_VALV and H19_VALV. The active valve only determines which FPGA register access functions run. static PINCH_VALVE_COMMAND_T homingCommands[ NUM_OF_PINCH_VALVE_HOMING_COMMANDS ] = { { PINCH_VALVE_PMD_CMD_SET_PROFILE_MODE, PINCH_VALVE_PROFILE_MODE_VELOCITY, 0, 0, 1, 0, FALSE }, { PINCH_VALVE_PMD_CMD_RESET_EVENT_STATUS, PINCH_VALVE_RESET_EVENT_STATUS_VALUE, 0, 0, 1, 0, FALSE }, { PINCH_VALVE_PMD_CMD_SET_VELOCITY, (U16)( ( (U32)HOMING_VELOCITY_FWD >> 16 ) & 0xFFFF ), (U16)( (U32)HOMING_VELOCITY_FWD & 0xFFFF ), 0, 2, 0, FALSE }, { PINCH_VALVE_PMD_CMD_UPDATE, 0, 0, 0, 0, 0, FALSE }, { PINCH_VALVE_PMD_CMD_GET_ACTIVITY_STATUS, 0, 0, 0, 0, 1, FALSE }, { PINCH_VALVE_PMD_CMD_GET_EVENT_STATUS, 0, 0, 0, 0, 1, FALSE }, { PINCH_VALVE_PMD_CMD_SET_ACTUAL_POSITION, 0, 0, 0, 2, 0, FALSE }, { PINCH_VALVE_PMD_CMD_RESET_EVENT_STATUS, PINCH_VALVE_RESET_EVENT_STATUS_VALUE, 0, 0, 1, 0, FALSE }, { PINCH_VALVE_PMD_CMD_SET_VELOCITY, (U16)( ( (U32)HOMING_VELOCITY_REV >> 16 ) & 0xFFFF ),(U16)( (U32)HOMING_VELOCITY_REV & 0xFFFF ), 0, 2, 0, FALSE }, { PINCH_VALVE_PMD_CMD_UPDATE, 0, 0, 0, 0, 0, FALSE }, { PINCH_VALVE_PMD_CMD_GET_ACTUAL_POSITION, 0, 0, 0, 0, 2, FALSE }, { PINCH_VALVE_PMD_CMD_RESET_EVENT_STATUS, PINCH_VALVE_RESET_EVENT_STATUS_VALUE, 0, 0, 1, 0, FALSE } }; static PINCH_VALVE_COMMAND_T positionCommands[ NUM_OF_PINCH_VALVE_POSITION_COMMANDS ] = { { PINCH_VALVE_PMD_CMD_SET_PROFILE_MODE, PINCH_VALVE_PROFILE_MODE_S_CURVE, 0, 0, 1, 0, FALSE }, { PINCH_VALVE_PMD_CMD_SET_VELOCITY, (U16)( ( (U32)POSITION_VELOCITY >> 16 ) & 0xFFFF ), (U16)( (U32)POSITION_VELOCITY & 0xFFFF ), 0, 2, 0, FALSE }, { PINCH_VALVE_PMD_CMD_GET_ACTIVITY_STATUS, 0, 0, 0, 0, 1, FALSE }, { PINCH_VALVE_PMD_CMD_GET_EVENT_STATUS, 0, 0, 0, 0, 1, FALSE }, { PINCH_VALVE_PMD_CMD_RESET_EVENT_STATUS, PINCH_VALVE_RESET_EVENT_STATUS_VALUE, 0, 0, 1, 0, FALSE }, { PINCH_VALVE_PMD_CMD_SET_POSITION, 0, 0, 0, 2, 0, FALSE }, { PINCH_VALVE_PMD_CMD_UPDATE, 0, 0, 0, 0, 0, FALSE }, { PINCH_VALVE_PMD_CMD_GET_EVENT_STATUS, 0, 0, 0, 0, 1, FALSE }, { PINCH_VALVE_PMD_CMD_GET_ACTUAL_POSITION, 0, 0, 0, 0, 2, FALSE }, { PINCH_VALVE_PMD_CMD_RESET_EVENT_STATUS, PINCH_VALVE_RESET_EVENT_STATUS_VALUE, 0, 0, 1, 0, FALSE } }; static PINCH_VALVE_FUNCTION_STATE_T functionState[ NUM_OF_VALVES ]; static PINCH_VALVE_CMD_STATE_T commandState[ NUM_OF_VALVES ]; static PINCH_VALVE_COMMAND_T activeCommand[ NUM_OF_VALVES ]; static const PINCH_VALVE_COMMAND_T *activeCommandArray[ NUM_OF_VALVES ]; static U08 activeCommandCount[ NUM_OF_VALVES ]; static U08 activeCommandIndex[ NUM_OF_VALVES ]; static BOOL pendingValveHomeRequest[ NUM_OF_VALVES ]; static BOOL pendingValvePositionRequest[ NUM_OF_VALVES ]; static BOOL commandFailed[ NUM_OF_VALVES ]; static BOOL commandCompleted[ NUM_OF_VALVES ]; static S32 pendingValvePosition[ NUM_OF_VALVES ]; static S32 currentValvePosition[ NUM_OF_VALVES ]; static U16 commandOutputWord1[ NUM_OF_VALVES ]; static U16 commandOutputWord2[ NUM_OF_VALVES ]; static U08 commandSpiStatus[ NUM_OF_VALVES ]; static U08 commandErrorCount[ NUM_OF_VALVES ]; static U08 errorCountAtCommandStart[ NUM_OF_VALVES ]; static U08 enableResetValue[ NUM_OF_VALVES ]; static U32 executeStartTime[ NUM_OF_VALVES ]; static U32 statusPollStartMSCount[ NUM_OF_VALVES ]; // ********** private function prototypes ********** static BOOL setPinchValveCommand( VALVE_T valve, const PINCH_VALVE_COMMAND_T *command ); static BOOL isPinchValveCommandCompleted( VALVE_T valve, BOOL *repeatCommand ); static void writePinchValveCommandRegisters( VALVE_T valve ); static BOOL transmitPinchValveCommand( VALVE_T valve ); static void readPinchValveCommandResponse( VALVE_T valve ); static void clearPinchValveFpgaCommand( VALVE_T valve ); static BOOL setPinchValveFpgaCommandByte( const PINCH_VALVE_COMMAND_T *command, U08 *fpgaCommand ); static BOOL isPinchValveCommandDone( VALVE_T valve ); static BOOL isPinchValveControllerFaultActive( VALVE_T valve ); static BOOL didPinchValveErrorCountChange( VALVE_T valve ); static BOOL arePinchValveWordCountsValid( VALVE_T valve ); static U08 readPinchValveErrorCount( VALVE_T valve ); static BOOL isValidPinchValve( VALVE_T valve ); static BOOL isValidPinchValveCommand( const PINCH_VALVE_COMMAND_T *command ); static void setSigned32CommandWords( PINCH_VALVE_COMMAND_T *command, S32 value ); static S32 getSigned32OutputValue( VALVE_T valve ); /*********************************************************************//** * @brief * The initPinchValveDriver function initializes the pinch valve * communication driver. * @details \b Inputs: none * @details \b Outputs: PinchValve driver unit is initialized * @return none *************************************************************************/ void initPinchValveDriver( void ) { VALVE_T valve; setSigned32CommandWords( &homingCommands[ PINCH_VALVE_HOME_CMD_SET_VELOCITY_B ], HOMING_VELOCITY_FWD ); setSigned32CommandWords( &homingCommands[ PINCH_VALVE_HOME_CMD_SET_VELOCITY_C ], HOMING_VELOCITY_REV ); setSigned32CommandWords( &positionCommands[ PINCH_VALVE_POSITION_CMD_SET_VELOCITY ], POSITION_VELOCITY ); for ( valve = FIRST_VALVE; valve < NUM_OF_VALVES; valve++ ) { functionState[ valve ] = PINCH_VALVE_FUNCTION_IDLE_STATE; commandState[ valve ] = PINCH_VALVE_CMD_IDLE_STATE; activeCommandArray[ valve ] = 0; activeCommandCount[ valve ] = 0; activeCommandIndex[ valve ] = 0; pendingValveHomeRequest[ valve ] = FALSE; pendingValvePositionRequest[ valve ] = FALSE; commandFailed[ valve ] = FALSE; commandCompleted[ valve ] = FALSE; pendingValvePosition[ valve ] = 0; currentValvePosition[ valve ] = 0; commandOutputWord1[ valve ] = 0; commandOutputWord2[ valve ] = 0; commandSpiStatus[ valve ] = 0; commandErrorCount[ valve ] = 0; errorCountAtCommandStart[ valve ] = 0; enableResetValue[ valve ] = 0; executeStartTime[ valve ] = 0; statusPollStartMSCount[ valve ] = 0; activeCommand[ valve ].cmdHeader = 0; activeCommand[ valve ].inputWord1 = 0; activeCommand[ valve ].inputWord2 = 0; activeCommand[ valve ].inputWord3 = 0; activeCommand[ valve ].inputWordCount = 0; activeCommand[ valve ].outputWordCount = 0; activeCommand[ valve ].writeOnly = FALSE; clearPinchValveFpgaCommand( valve ); } } /*********************************************************************//** * @brief * The execPinchValveCommand function executes the low-level FPGA command * state machine. * @details \b Inputs: activeCommand, activeValve, * @details \b Outputs: currentResponse, commandState, * and commandResult * @return none *************************************************************************/ void execPinchValveCommand( void ) { VALVE_T valve; for ( valve = FIRST_VALVE; valve < NUM_OF_VALVES; valve++ ) { switch ( commandState[ valve ] ) { case PINCH_VALVE_CMD_IDLE_STATE: break; case PINCH_VALVE_CMD_SET_CMD_STATE: writePinchValveCommandRegisters( valve ); if ( PINCH_VALVE_CMD_ERROR_STATE != commandState[ valve ] ) { errorCountAtCommandStart[ valve ] = readPinchValveErrorCount( valve ); commandState[ valve ] = PINCH_VALVE_CMD_XMIT_CMD_STATE; } break; case PINCH_VALVE_CMD_XMIT_CMD_STATE: if ( TRUE == transmitPinchValveCommand( valve ) ) { executeStartTime[ valve ] = getMSTimerCount(); statusPollStartMSCount[ valve ] = executeStartTime[ valve ]; commandState[ valve ] = PINCH_VALVE_CMD_WAIT_DONE_STATE; } else { commandState[ valve ] = PINCH_VALVE_CMD_ERROR_STATE; } break; case PINCH_VALVE_CMD_WAIT_DONE_STATE: if ( TRUE == didTimeout( executeStartTime[ valve ], PINCH_VALVE_COMMAND_TIMEOUT_MS ) ) { readPinchValveCommandResponse( valve ); commandState[ valve ] = PINCH_VALVE_CMD_ERROR_STATE; } else if ( TRUE == didTimeout( statusPollStartMSCount[ valve ], PINCH_VALVE_STATUS_POLL_INTERVAL_MS ) ) { readPinchValveCommandResponse( valve ); statusPollStartMSCount[ valve ] = getMSTimerCount(); if ( ( TRUE == didPinchValveErrorCountChange( valve ) ) || ( TRUE == isPinchValveControllerFaultActive( valve ) ) ) { commandState[ valve ] = PINCH_VALVE_CMD_ERROR_STATE; } else if ( TRUE == isPinchValveCommandDone( valve ) ) { commandState[ valve ] = PINCH_VALVE_CMD_READ_WORDS_STATE; } } break; case PINCH_VALVE_CMD_READ_WORDS_STATE: readPinchValveCommandResponse( valve ); if ( ( TRUE == didPinchValveErrorCountChange( valve ) ) || ( TRUE == isPinchValveControllerFaultActive( valve ) ) || ( FALSE == arePinchValveWordCountsValid( valve ) ) ) { commandState[ valve ] = PINCH_VALVE_CMD_ERROR_STATE; } else { clearPinchValveFpgaCommand( valve ); commandFailed[ valve ] = FALSE; commandCompleted[ valve ] = TRUE; commandState[ valve ] = PINCH_VALVE_CMD_IDLE_STATE; } break; case PINCH_VALVE_CMD_ERROR_STATE: readPinchValveCommandResponse( valve ); clearPinchValveFpgaCommand( valve ); commandFailed[ valve ] = TRUE; commandCompleted[ valve ] = TRUE; commandState[ valve ] = PINCH_VALVE_CMD_IDLE_STATE; break; default: clearPinchValveFpgaCommand( valve ); commandFailed[ valve ] = TRUE; commandCompleted[ valve ] = TRUE; commandState[ valve ] = PINCH_VALVE_CMD_IDLE_STATE; break; } } } /*********************************************************************//** * @brief * The homePinchValve function starts the common homing command sequence. * @note homingCommands[] is shared by H1 and H19. Only the FPGA register set * changes according to the selected valve. * @details \b Inputs: * @details \b Outputs: * @param valve H1_VALV or H19_VALV. * @return TRUE if accepted, FALSE otherwise. *************************************************************************/ BOOL homePinchValve( VALVE_T valve ) { BOOL result = FALSE; if ( ( TRUE == isValidPinchValve( valve ) ) && ( FALSE == isPinchValveBusy( valve ) ) ) { pendingValveHomeRequest[ valve ] = TRUE; result = TRUE; } return result; } /*********************************************************************//** * @brief * The setPinchValvePosition function starts a raw position command sequence. * //TODO: Need too fix the code f * @details \b Inputs: * @details \b Outputs: * @param valve H1_VALV or H19_VALV. * @param position Signed destination position. * @return TRUE if accepted, FALSE otherwise. *************************************************************************/ BOOL setPinchValvePosition( VALVE_T valve, S32 position ) { BOOL result = FALSE; if ( ( TRUE == isValidPinchValve( valve ) ) && ( FALSE == isPinchValveBusy( valve ) ) ) { pendingValvePosition[ valve ] = position; pendingValvePositionRequest[ valve ] = TRUE; result = TRUE; } return result; } /*********************************************************************//** * @brief * The getPinchValvePosition function returns the latest actual position. * @details \b Inputs: currentValvePosition * @details \b Outputs: none * @param valve H1_VALV or H19_VALV. * @return Latest signed position, or 0 for an invalid valve. *************************************************************************/ S32 getPinchValvePosition( VALVE_T valve ) { S32 result = 0; if ( TRUE == isValidPinchValve( valve ) ) { result = currentValvePosition[ valve ]; } return result; } /*********************************************************************//** * @brief * The setPinchValveEnableReset function sets the enable and reset * register for the selected valve. * @details \b Inputs: valve, enable, reset * @details \b Outputs: FPGA enable/reset register, enableResetValue[] * @param valve H1_VALV or H19_VALV. * @param enable TRUE to enable the controller. * @param reset TRUE to assert the reset bit. * @return none *************************************************************************/ void setPinchValveEnableReset( VALVE_T valve, BOOL enable, BOOL reset ) { U08 value = 0; if ( TRUE == enable ) { value |= PINCH_VALVE_ENABLE_BIT_MASK; } if ( TRUE == reset ) { value |= PINCH_VALVE_RESET_BIT_MASK; } if ( H1_VALV == valve ) { enableResetValue[ valve ] = value; setH1EnableReset( value ); } else if ( H19_VALV == valve ) { enableResetValue[ valve ] = value; setH19EnableReset( value ); } } /*********************************************************************//** * @brief * The getPinchValveEnableReset function returns the last enable/reset value * written for the selected valve. * @details \b Inputs: enableResetValue[] * @details \b Outputs: none * @param valve H1_VALV or H19_VALV. * @return Last enable/reset value, or 0 for an invalid valve. *************************************************************************/ U08 getPinchValveEnableReset( VALVE_T valve ) { U08 result = 0; if ( TRUE == isValidPinchValve( valve ) ) { result = enableResetValue[ valve ]; } return result; } /*********************************************************************//** * @brief * The isPinchValveBusy function indicates whether a command is active. * @details \b Inputs: functionState, commandState * @details \b Outputs: none * @return TRUE if the driver is busy, FALSE otherwise. *************************************************************************/ BOOL isPinchValveBusy( VALVE_T valve ) { BOOL result = TRUE; if ( TRUE == isValidPinchValve( valve ) ) { result = ( ( PINCH_VALVE_FUNCTION_IDLE_STATE != functionState[ valve ] ) || ( PINCH_VALVE_CMD_IDLE_STATE != commandState[ valve ] ) || ( TRUE == pendingValveHomeRequest[ valve ] ) || ( TRUE == pendingValvePositionRequest[ valve ] ) ); } return result; } /*********************************************************************//** * @brief * The execPinchValveFunction function executes the IDLE, SEND, and * WAIT state machine. * @details \b Inputs: functionState, commandState * @details \b Outputs: functionState, commandState, commandResult * @return none *************************************************************************/ void execPinchValveFunction( void ) { VALVE_T valve; for ( valve = FIRST_VALVE; valve < NUM_OF_VALVES; valve++ ) { switch ( functionState[ valve ] ) { case PINCH_VALVE_FUNCTION_IDLE_STATE: if ( TRUE == pendingValveHomeRequest[ valve ] ) { pendingValveHomeRequest[ valve ] = FALSE; activeCommandArray[ valve ] = &homingCommands[ 0 ]; activeCommandCount[ valve ] = (U08)NUM_OF_PINCH_VALVE_HOMING_COMMANDS; activeCommandIndex[ valve ] = 0; commandFailed[ valve ] = FALSE; functionState[ valve ] = PINCH_VALVE_FUNCTION_SEND_STATE; } else if ( TRUE == pendingValvePositionRequest[ valve ] ) { pendingValvePositionRequest[ valve ] = FALSE; activeCommandArray[ valve ] = &positionCommands[ 0 ]; activeCommandCount[ valve ] = (U08)NUM_OF_PINCH_VALVE_POSITION_COMMANDS; activeCommandIndex[ valve ] = 0; commandFailed[ valve ] = FALSE; functionState[ valve ] = PINCH_VALVE_FUNCTION_SEND_STATE; } break; case PINCH_VALVE_FUNCTION_SEND_STATE: if ( activeCommandArray[ valve ] != 0 ) { if ( TRUE == setPinchValveCommand( valve, &activeCommandArray[ valve ][ activeCommandIndex[ valve ] ] ) ) { functionState[ valve ] = PINCH_VALVE_FUNCTION_WAIT_STATE; } } else { //commandFailed[ valve ] = TRUE; functionState[ valve ] = PINCH_VALVE_FUNCTION_IDLE_STATE; // TODO - s/w fault } break; case PINCH_VALVE_FUNCTION_WAIT_STATE: if ( PINCH_VALVE_CMD_IDLE_STATE == commandState[ valve ] ) // && ( TRUE == commandCompleted[ valve ] ) ) { // BOOL repeatCommand = FALSE; // commandCompleted[ valve ] = FALSE; if ( TRUE == commandFailed[ valve ] ) { // TODO - valve fault functionState[ valve ] = PINCH_VALVE_FUNCTION_IDLE_STATE; } // else if ( FALSE == isPinchValveCommandCompleted( valve, &repeatCommand ) ) // { // commandFailed[ valve ] = TRUE; // functionState[ valve ] = PINCH_VALVE_FUNCTION_IDLE_STATE; // } // else if ( TRUE == repeatCommand ) // { // functionState[ valve ] = PINCH_VALVE_FUNCTION_SEND_STATE; // } else { activeCommandIndex[ valve ]++; if ( activeCommandIndex[ valve ] >= activeCommandCount[ valve ] ) { functionState[ valve ] = PINCH_VALVE_FUNCTION_IDLE_STATE; } else { functionState[ valve ] = PINCH_VALVE_FUNCTION_SEND_STATE; } } } // TODO - need to handle else if IDLE and not completed (error?) break; default: commandFailed[ valve ] = TRUE; functionState[ valve ] = PINCH_VALVE_FUNCTION_IDLE_STATE; break; } } } /*********************************************************************//** * @brief * The setPinchValveCommand function requests execution * @details \b Inputs: valve, command * @details \b Outputs: pendingCommand, functionState, commandResult * @param valve H1_VALV or H19_VALV. * @return TRUE if the request was accepted, FALSE otherwise. *************************************************************************/ static BOOL setPinchValveCommand( VALVE_T valve, const PINCH_VALVE_COMMAND_T *command ) { BOOL result = FALSE; if ( ( TRUE == isValidPinchValve( valve ) ) && ( TRUE == isValidPinchValveCommand( command ) ) && ( PINCH_VALVE_CMD_IDLE_STATE == commandState[ valve ] ) && ( FALSE == commandCompleted[ valve ] ) ) { activeCommand[ valve ] = *command; if ( ( activeCommandArray[ valve ] == &positionCommands[ 0 ] ) && ( PINCH_VALVE_POSITION_CMD_SET_POSITION == activeCommandIndex[ valve ] ) ) { setSigned32CommandWords( &activeCommand[ valve ], pendingValvePosition[ valve ] ); } commandOutputWord1[ valve ] = 0; commandOutputWord2[ valve ] = 0; commandSpiStatus[ valve ] = 0; commandErrorCount[ valve ] = 0; commandFailed[ valve ] = FALSE; commandState[ valve ] = PINCH_VALVE_CMD_SET_CMD_STATE; result = TRUE; } return result; } /*********************************************************************//** * @brief * The isPinchValveCommandCompleted function handles returned PMD data. * @details \b Inputs: activeFunction, activeCommandIndex, lastResponse * @details \b Outputs: currentValvePosition, repeatCommand * @param repeatCommand Set TRUE when the current status command shall repeat. * @return TRUE if the response is valid, FALSE if an error occurred. *************************************************************************/ static BOOL isPinchValveCommandCompleted( VALVE_T valve, BOOL *repeatCommand ) { BOOL result = FALSE; if ( ( TRUE == isValidPinchValve( valve ) ) && ( repeatCommand != 0 ) ) { *repeatCommand = FALSE; result = TRUE; if ( activeCommandArray[ valve ] == &homingCommands[ 0 ] ) { if ( PINCH_VALVE_HOME_CMD_GET_ACTIVITY_STATUS == activeCommandIndex[ valve ] ) { U16 required = PINCH_VALVE_ACTIVITY_PHASE_INITIALIZED_MASK | PINCH_VALVE_ACTIVITY_AXIS_SETTLED_MASK; if ( required != ( commandOutputWord1[ valve ] & required ) ) { *repeatCommand = TRUE; } } else if ( PINCH_VALVE_HOME_CMD_GET_EVENT_STATUS_B == activeCommandIndex[ valve ] ) { if ( 0 == ( commandOutputWord1[ valve ] & ( PINCH_VALVE_EVENT_MOTION_COMPLETE_MASK | PINCH_VALVE_EVENT_MOTION_ERROR_MASK ) ) ) { *repeatCommand = TRUE; } } else if ( PINCH_VALVE_HOME_CMD_GET_ACTUAL_POSITION_C == activeCommandIndex[ valve ] ) { currentValvePosition[ valve ] = getSigned32OutputValue( valve ); } } else if ( activeCommandArray[ valve ] == &positionCommands[ 0 ] ) { if ( PINCH_VALVE_POSITION_CMD_GET_ACTIVITY_STATUS == activeCommandIndex[ valve ] ) { U16 required = PINCH_VALVE_ACTIVITY_PHASE_INITIALIZED_MASK | PINCH_VALVE_ACTIVITY_AXIS_SETTLED_MASK; if ( required != ( commandOutputWord1[ valve ] & required ) ) { *repeatCommand = TRUE; } } else if ( PINCH_VALVE_POSITION_CMD_GET_EVENT_STATUS_BEFORE == activeCommandIndex[ valve ] ) { if ( 0 != ( commandOutputWord1[ valve ] & PINCH_VALVE_EVENT_MOTION_ERROR_MASK ) ) { result = FALSE; } } else if ( PINCH_VALVE_POSITION_CMD_GET_EVENT_STATUS_AFTER == activeCommandIndex[ valve ] ) { if ( 0 != ( commandOutputWord1[ valve ] & PINCH_VALVE_EVENT_MOTION_ERROR_MASK ) ) { result = FALSE; } else if ( 0 == ( commandOutputWord1[ valve ] & PINCH_VALVE_EVENT_MOTION_COMPLETE_MASK ) ) { *repeatCommand = TRUE; } } else if ( PINCH_VALVE_POSITION_CMD_GET_ACTUAL_POSITION == activeCommandIndex[ valve ] ) { currentValvePosition[ valve ] = getSigned32OutputValue( valve ); } } else { result = FALSE; } } return result; } /*********************************************************************//** * @brief * The writePinchValveCommandRegisters function writes the command header and * input words through the FpgaTD interface. * @details \b Inputs: activeValve, activeCommand * @details \b Outputs: FPGA command header and input registers * @return none *************************************************************************/ static void writePinchValveCommandRegisters( VALVE_T valve ) { if ( H1_VALV == valve ) { setH1CmdHeader( activeCommand[ valve ].cmdHeader ); setH1InputWord1( activeCommand[ valve ].inputWord1 ); setH1InputWord2( activeCommand[ valve ].inputWord2 ); setH1InputWord3( activeCommand[ valve ].inputWord3 ); } else if ( H19_VALV == valve ) { setH19CmdHeader( activeCommand[ valve ].cmdHeader ); setH19InputWord1( activeCommand[ valve ].inputWord1 ); setH19InputWord2( activeCommand[ valve ].inputWord2 ); setH19InputWord3( activeCommand[ valve ].inputWord3 ); } else { commandState[ valve ] = PINCH_VALVE_CMD_ERROR_STATE; } } /*********************************************************************//** * @brief * The transmitPinchValveCommand function builds and writes the FPGA command * byte. * @details \b Inputs: activeValve, activeCommand * @details \b Outputs: FPGA command register * @return TRUE if the command byte was valid and written. *************************************************************************/ static BOOL transmitPinchValveCommand( VALVE_T valve ) { BOOL result = FALSE; U08 fpgaCommand; if ( TRUE == setPinchValveFpgaCommandByte( &activeCommand[ valve ], &fpgaCommand ) ) { if ( H1_VALV == valve ) { setH1FPGACmd( fpgaCommand ); result = TRUE; } else if ( H19_VALV == valve ) { setH19FPGACmd( fpgaCommand ); result = TRUE; } } return result; } /*********************************************************************//** * @brief * The readPinchValveCommandResponse function reads the active valve response * through the FpgaTD interface. * @details \b Inputs: activeValve, FPGA response registers * @details \b Outputs: currentResponse * @return none *************************************************************************/ static void readPinchValveCommandResponse( VALVE_T valve ) { if ( H1_VALV == valve ) { commandSpiStatus[ valve ] = getH1SPICmdStatus(); commandErrorCount[ valve ] = getH1ErrorCount(); commandOutputWord1[ valve ] = getH1OutputWord1(); commandOutputWord2[ valve ] = getH1OutputWord2(); } else if ( H19_VALV == valve ) { commandSpiStatus[ valve ] = getH19SPICmdStatus(); commandErrorCount[ valve ] = getH19ErrorCount(); commandOutputWord1[ valve ] = getH19OutputWord1(); commandOutputWord2[ valve ] = getH19OutputWord2(); } } /*********************************************************************//** * @brief * The clearPinchValveFpgaCommand function clears the active valve FPGA command * register. * @details \b Inputs: activeValve * @details \b Outputs: FPGA command register * @return none *************************************************************************/ static void clearPinchValveFpgaCommand( VALVE_T valve ) { if ( H1_VALV == valve ) { setH1FPGACmd( 0 ); } else if ( H19_VALV == valve ) { setH19FPGACmd( 0 ); } } /*********************************************************************//** * @brief * The setPinchValveFpgaCommandBytePinchValveFpgaCommandByte function builds the FPGA command byte. * @details The transmit count includes the command header, so one is added to * the number of input words. * @details \b Inputs: command * @details \b Outputs: fpgaCommand * @param command Pinch valve command request. * @param fpgaCommand Destination for the built FPGA command byte. * @return TRUE if the word counts are valid. *************************************************************************/ static BOOL setPinchValveFpgaCommandByte( const PINCH_VALVE_COMMAND_T *command, U08 *fpgaCommand ) { BOOL result = FALSE; if ( ( command != 0 ) && ( fpgaCommand != 0 ) ) { U08 transmitWordCount = (U08)( command->inputWordCount + 1 ); if ( ( command->inputWordCount <= PINCH_VALVE_MAX_INPUT_WORDS ) && ( command->outputWordCount <= PINCH_VALVE_MAX_OUTPUT_WORDS ) && ( transmitWordCount <= PINCH_VALVE_MAX_TRANSMIT_WORDS ) ) { *fpgaCommand = PINCH_VALVE_FPGA_CMD_START_BIT_MASK; if ( TRUE == command->writeOnly ) { *fpgaCommand |= PINCH_VALVE_FPGA_CMD_WRITE_ONLY_BIT_MASK; } *fpgaCommand |= (U08)( ( transmitWordCount & PINCH_VALVE_FPGA_CMD_WORD_COUNT_MASK ) << PINCH_VALVE_FPGA_CMD_TX_COUNT_SHIFT ); *fpgaCommand |= (U08)( ( command->outputWordCount & PINCH_VALVE_FPGA_CMD_WORD_COUNT_MASK ) << PINCH_VALVE_FPGA_CMD_RX_COUNT_SHIFT ); result = TRUE; } } return result; } /*********************************************************************//** * @brief * The isPinchValveCommandDone function checks the SPI command-done bit. * @details \b Inputs: * @details \b Outputs: * @return TRUE if the FPGA transaction is complete. *************************************************************************/ static BOOL isPinchValveCommandDone( VALVE_T valve ) { return ( ( commandSpiStatus[ valve ] & PINCH_VALVE_SPI_DONE_BIT_MASK ) != 0 ); } /*********************************************************************//** * @brief * The isPinchValveControllerFaultActive function checks the controller fault * status bit. * @details \b Inputs: currentResponse * @details \b Outputs: none * @return TRUE if the controller fault bit is active. *************************************************************************/ static BOOL isPinchValveControllerFaultActive( VALVE_T valve ) { return ( ( commandSpiStatus[ valve ] & PINCH_VALVE_SPI_CONTROLLER_FAULT_BIT_MASK ) != 0 ); } /*********************************************************************//** * @brief * The didPinchValveErrorCountChange function checks whether the active * transaction incremented the invalid-command counter. * @details \b Inputs: errorCountAtCommandStart, currentResponse.errorCount * @details \b Outputs: none * @return TRUE if the error count changed. *************************************************************************/ static BOOL didPinchValveErrorCountChange( VALVE_T valve ) { return ( commandErrorCount[ valve ] != errorCountAtCommandStart[ valve ] ); } /*********************************************************************//** * @brief * The arePinchValveWordCountsValid function verifies the FPGA-reported * transmitted and received word counts. * @details \b Inputs: currentResponse.spiCmdStatus, activeCommand * @details \b Outputs: none * @return TRUE if reported counts match the command. *************************************************************************/ static BOOL arePinchValveWordCountsValid( VALVE_T valve ) { U08 expectedTransmitCount = (U08)( activeCommand[ valve ].inputWordCount + 1U ); U08 transmittedCount = (U08)( ( commandSpiStatus[ valve ] & PINCH_VALVE_SPI_TX_COUNT_MASK ) >> PINCH_VALVE_SPI_TX_COUNT_SHIFT ); U08 receivedCount = (U08)( ( commandSpiStatus[ valve ] & PINCH_VALVE_SPI_RX_COUNT_MASK ) >> PINCH_VALVE_SPI_RX_COUNT_SHIFT ); return ( ( transmittedCount == expectedTransmitCount ) && ( receivedCount == activeCommand[ valve ].outputWordCount ) ); } /*********************************************************************//** * @brief * The readPinchValveErrorCount function returns the current error count for * the selected valve. * @details \b Inputs: * @details \b Outputs: none * @param valve H1_VALV or H19_VALV. * @return Current error count, or 0 for an invalid valve. *************************************************************************/ static U08 readPinchValveErrorCount( VALVE_T valve ) { U08 result = 0; if ( H1_VALV == valve ) { result = getH1ErrorCount(); } else if ( H19_VALV == valve ) { result = getH19ErrorCount(); } return result; } /*********************************************************************//** * @brief * The isValidPinchValve function validates a valve ID. * @details \b Inputs: valve * @details \b Outputs: none * @param valve Valve to validate. * @return TRUE if the valve is H1 or H19. *************************************************************************/ static BOOL isValidPinchValve( VALVE_T valve ) { return ( ( H1_VALV == valve ) || ( H19_VALV == valve ) ); } /*********************************************************************//** * @brief * The isValidPinchValveCommand function validates a generic command request. * @details \b Inputs: command * @details \b Outputs: none * @param command Command to validate. * @return TRUE if the request is valid. *************************************************************************/ static BOOL isValidPinchValveCommand( const PINCH_VALVE_COMMAND_T *command ) { BOOL result = FALSE; if ( command != 0 ) { if ( ( command->inputWordCount <= PINCH_VALVE_MAX_INPUT_WORDS ) && ( command->outputWordCount <= PINCH_VALVE_MAX_OUTPUT_WORDS ) ) { result = TRUE; } } return result; } /*********************************************************************//** * @brief * The setSigned32CommandWords function stores a signed 32-bit value. * @details \b Inputs: value * @details \b Outputs: * @param command Command to update. * @return none *************************************************************************/ static void setSigned32CommandWords( PINCH_VALVE_COMMAND_T *command, S32 value ) { if ( command != 0 ) { U32 rawValue = (U32)value; command->inputWord1 = (U16)( ( rawValue >> 16 ) & 0xFFFF ); command->inputWord2 = (U16)( rawValue & 0xFFFF ); } } /*********************************************************************//** * @brief * The getSigned32ResponseValue function combines two PMD output words. * @details \b Inputs: * @details \b Outputs: none * @return Signed 32-bit value. *************************************************************************/ static S32 getSigned32OutputValue( VALVE_T valve ) { U32 rawValue = ( (U32)commandOutputWord1[ valve ] << 16 ) | (U32)commandOutputWord2[ valve ]; return (S32)rawValue; } /**@}*/