Index: firmware/App/Drivers/PinchValve.c =================================================================== diff -u -r078ab958e0cac18c5b8a5cc2f9d22710ac52aa1e -r1f3e57c1351a085c6a05105a50ef73393cc974c6 --- firmware/App/Drivers/PinchValve.c (.../PinchValve.c) (revision 078ab958e0cac18c5b8a5cc2f9d22710ac52aa1e) +++ firmware/App/Drivers/PinchValve.c (.../PinchValve.c) (revision 1f3e57c1351a085c6a05105a50ef73393cc974c6) @@ -15,6 +15,7 @@ * ***************************************************************************/ +#include "Common.h" #include "FpgaTD.h" #include "PinchValve.h" #include "RotaryValve.h" @@ -32,6 +33,8 @@ #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. @@ -48,56 +51,196 @@ #define PINCH_VALVE_ENABLE_BIT_MASK 0x01 ///< Enable controller. #define PINCH_VALVE_RESET_BIT_MASK 0x02 ///< Reset controller. -/// Pinch Valve driver function state machine. + +#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_DEFAULT_VELOCITY 3342 + +#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 + + +/// Pinch valve function state machine. typedef enum pinchValveFunctionStates { - PINCH_VALVE_FUNCTION_IDLE_STATE = 0, ///< Waiting for a new command request. - PINCH_VALVE_FUNCTION_SEND_STATE, ///< Starts the low level command state machine. - PINCH_VALVE_FUNCTION_WAIT_STATE ///< Waits for command completion or error. + 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. +/// Low-level FPGA command state machine. typedef enum pinchValveCommandStates { - PINCH_VALVE_CMD_IDLE_STATE = 0, ///< No FPGA command is active. - PINCH_VALVE_CMD_SET_CMD_STATE, ///< Writes header and input words. - PINCH_VALVE_CMD_XMIT_CMD_STATE, ///< Writes FPGA command byte. - PINCH_VALVE_CMD_WAIT_DONE_STATE, ///< Polls command-done status. - PINCH_VALVE_CMD_READ_WORDS_STATE, ///< Reads response words and counters. - PINCH_VALVE_CMD_ERROR_STATE, ///< Command failed. - PINCH_VALVE_CMD_READY_STATE ///< Command completed successfully. + 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; +/// Result of the active low level command. +typedef enum pinchValveCommandResults +{ + PINCH_VALVE_CMD_RESULT_IDLE = 0, + PINCH_VALVE_CMD_RESULT_BUSY, + PINCH_VALVE_CMD_RESULT_COMPLETE, + PINCH_VALVE_CMD_RESULT_ERROR +} PINCH_VALVE_CMD_RESULT_T; + +/// Pinch Valve Function. +typedef enum pinchValveFunctions +{ + PINCH_VALVE_FUNCTION_NONE = 0, + PINCH_VALVE_FUNCTION_HOME, + PINCH_VALVE_FUNCTION_SET_POSITION +} PINCH_VALVE_FUNCTION_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; + +/// Response from the FPGA command interface. +typedef struct +{ + U16 outputWord1; ///< Output word 1. + U16 outputWord2; ///< Output word 2. + U16 outputWord3; ///< Output word 3. + U08 spiCmdStatus; ///< FPGA SPI command status. + U08 readCount; ///< FPGA read count. + U08 errorCount; ///< FPGA invalid command error count. +} PINCH_VALVE_RESPONSE_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, 0, 0, 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, 0, 0, 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, 0, 0, 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; ///< Current function state. static PINCH_VALVE_CMD_STATE_T commandState; ///< Current command state. +static PINCH_VALVE_FUNCTION_T activeFunction; static VALVE_T activeValve; ///< Valve executing the current command. static PINCH_VALVE_COMMAND_T pendingCommand; ///< Command waiting to be started. static PINCH_VALVE_COMMAND_T activeCommand; ///< Command currently being executed. +static PINCH_VALVE_COMMAND_T *activeCommandArray; +static U08 activeCommandCount; +static U08 activeCommandIndex; + static PINCH_VALVE_RESPONSE_T currentResponse; ///< Response for the active command. static PINCH_VALVE_RESPONSE_T lastResponse[ NUM_OF_VALVES ]; ///< Last response for each valve. static PINCH_VALVE_CMD_RESULT_T commandResult; ///< Latest driver result. static U08 enableResetValue[ NUM_OF_VALVES ]; ///< Last enable/reset value written per valve. static U08 errorCountAtCommandStart; ///< Error counter captured before command start. +static S32 currentValvePosition[ NUM_OF_VALVES ]; static U32 executeStartTime; ///< Execute transaction start time. static U32 statusPollStartMSCount; ///< Status polling interval start time. // ********** private function prototypes ********** -static void handlePinchValveFunction( void ); -static void handlePinchValveCommand( void ); +static PINCH_VALVE_CMD_RESULT_T getPinchValveCommandResult( void ); +static BOOL getPinchValveResponse( VALVE_T valve, PINCH_VALVE_RESPONSE_T *response ); +static BOOL setPinchValveCommand( VALVE_T valve, const PINCH_VALVE_COMMAND_T *command ); + static void writePinchValveCommandRegisters( void ); static BOOL transmitPinchValveCommand( void ); static void readPinchValveCommandResponse( void ); static void clearPinchValveFpgaCommand( void ); -static BOOL buildPinchValveFpgaCommandByte( const PINCH_VALVE_COMMAND_T *command, U08 *fpgaCommand ); +static BOOL setPinchValveFpgaCommandByte( const PINCH_VALVE_COMMAND_T *command, U08 *fpgaCommand ); static BOOL isPinchValveCommandDone( void ); static BOOL isPinchValveControllerFaultActive( void ); static BOOL didPinchValveErrorCountChange( void ); @@ -108,6 +251,10 @@ static BOOL isValidPinchValveCommand( const PINCH_VALVE_COMMAND_T *command ); static void clearPinchValveResponse( PINCH_VALVE_RESPONSE_T *response ); +static void setSigned32CommandWords( PINCH_VALVE_COMMAND_T *command, S32 value ); +static S32 getSigned32ResponseValue( const PINCH_VALVE_RESPONSE_T *response ); +static BOOL isPinchValveCommandCompleted( BOOL *repeatCommand ); + /*********************************************************************//** * @brief * The initPinchValveDriver function initializes the pinch valve @@ -121,70 +268,182 @@ U32 valve; functionState = PINCH_VALVE_FUNCTION_IDLE_STATE; - commandState = PINCH_VALVE_CMD_IDLE_STATE; - activeValve = H1_VALV; + commandState = PINCH_VALVE_CMD_IDLE_STATE; + activeFunction = PINCH_VALVE_FUNCTION_NONE; + activeValve = H1_VALV; commandResult = PINCH_VALVE_CMD_RESULT_IDLE; - pendingCommand.cmdHeader = 0; - pendingCommand.inputWord1 = 0; - pendingCommand.inputWord2 = 0; - pendingCommand.inputWord3 = 0; - pendingCommand.inputWordCount = 0; - pendingCommand.outputWordCount = 0; - pendingCommand.writeOnly = FALSE; + activeCommandArray = 0; + activeCommandCount = 0; + activeCommandIndex = 0; - activeCommand = pendingCommand; - clearPinchValveResponse( ¤tResponse ); for ( valve = FIRST_VALVE; valve < NUM_OF_VALVES; valve++ ) { clearPinchValveResponse( &lastResponse[ valve ] ); enableResetValue[ valve ] = 0; + currentValvePosition[ valve ] = 0; } + setSigned32CommandWords( &homingCommands[ PINCH_VALVE_HOME_CMD_SET_VELOCITY_B ], PINCH_VALVE_DEFAULT_VELOCITY ); + setSigned32CommandWords( &homingCommands[ PINCH_VALVE_HOME_CMD_SET_VELOCITY_C ], -PINCH_VALVE_DEFAULT_VELOCITY ); + setSigned32CommandWords( &homingCommands[ PINCH_VALVE_HOME_CMD_SET_ACTUAL_POSITION_ZERO ], 0 ); + setSigned32CommandWords( &positionCommands[ PINCH_VALVE_POSITION_CMD_SET_VELOCITY ], PINCH_VALVE_DEFAULT_VELOCITY ); + setSigned32CommandWords( &positionCommands[ PINCH_VALVE_POSITION_CMD_SET_POSITION ], 0 ); + errorCountAtCommandStart = 0; executeStartTime = 0; statusPollStartMSCount = 0; - clearPinchValveFpgaCommand(); + setH1FPGACmd( 0 ); + setH19FPGACmd( 0 ); } /*********************************************************************//** * @brief -* The execPinchValveDriver function executes the function and command state -* machines. -* TODO: All Inputs and Outputs will be changed later -* @details \b Inputs: PinchValve private data and FPGA response data -* @details \b Outputs: PinchValve private data and FPGA command data +* The execPinchValveDriver function executes the command state machine. +* //TODO: +* @details \b Inputs: PinchValve private data and FPGA response registers +* @details \b Outputs: PinchValve private data and FPGA command registers * @return none *************************************************************************/ void execPinchValveDriver( void ) { - //TODO: Need to fix this - handlePinchValveCommand(); - handlePinchValveFunction(); + execPinchValveFunction(); + execPinchValveCommand(); } /*********************************************************************//** * @brief -* The setPinchValveCommand function requests execution of one generic -* Magellan command. -* @details \b Inputs: valve, command -* @details \b Outputs: pendingCommand, functionState, commandResult +* 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 ) +{ + switch ( commandState ) + { + case PINCH_VALVE_CMD_IDLE_STATE: + { + execPinchValveFunction(); + break; + } + + case PINCH_VALVE_CMD_SET_CMD_STATE: + { + writePinchValveCommandRegisters(); + if ( PINCH_VALVE_CMD_ERROR_STATE != commandState ) + { + errorCountAtCommandStart = readPinchValveErrorCount( activeValve ); + commandState = PINCH_VALVE_CMD_XMIT_CMD_STATE; + } + break; + } + + case PINCH_VALVE_CMD_XMIT_CMD_STATE: + { + if ( TRUE == transmitPinchValveCommand() ) + { + executeStartTime = getMSTimerCount(); + statusPollStartMSCount = executeStartTime; + commandState = PINCH_VALVE_CMD_WAIT_DONE_STATE; + } + else + { + commandState = PINCH_VALVE_CMD_ERROR_STATE; + } + break; + } + + case PINCH_VALVE_CMD_WAIT_DONE_STATE: + { + if ( TRUE == didTimeout( executeStartTime, PINCH_VALVE_COMMAND_TIMEOUT_MS ) ) + { + readPinchValveCommandResponse(); + commandState = PINCH_VALVE_CMD_ERROR_STATE; + } + else if ( TRUE == didTimeout( statusPollStartMSCount, PINCH_VALVE_STATUS_POLL_INTERVAL_MS ) ) + { + readPinchValveCommandResponse(); + statusPollStartMSCount = getMSTimerCount(); + + if ( ( TRUE == didPinchValveErrorCountChange() ) || ( TRUE == isPinchValveControllerFaultActive() ) ) + { + commandState = PINCH_VALVE_CMD_ERROR_STATE; + } + else if ( TRUE == isPinchValveCommandDone() ) + { + commandState = PINCH_VALVE_CMD_READ_WORDS_STATE; + } + } + break; + } + + case PINCH_VALVE_CMD_READ_WORDS_STATE: + { + readPinchValveCommandResponse(); + + if ( ( TRUE == didPinchValveErrorCountChange() ) || ( TRUE == isPinchValveControllerFaultActive() ) || ( FALSE == arePinchValveWordCountsValid() ) ) + { + commandState = PINCH_VALVE_CMD_ERROR_STATE; + } + else + { + lastResponse[ activeValve ] = currentResponse; + clearPinchValveFpgaCommand(); + commandResult = PINCH_VALVE_CMD_RESULT_COMPLETE; + commandState = PINCH_VALVE_CMD_IDLE_STATE; + } + break; + } + + case PINCH_VALVE_CMD_ERROR_STATE: + { + readPinchValveCommandResponse(); + lastResponse[ activeValve ] = currentResponse; + clearPinchValveFpgaCommand(); + commandResult = PINCH_VALVE_CMD_RESULT_ERROR; + commandState = PINCH_VALVE_CMD_IDLE_STATE; + break; + } + + default: + { + clearPinchValveFpgaCommand(); + commandResult = PINCH_VALVE_CMD_RESULT_ERROR; + commandState = PINCH_VALVE_CMD_IDLE_STATE; + functionState = PINCH_VALVE_FUNCTION_IDLE_STATE; + activeFunction = PINCH_VALVE_FUNCTION_NONE; + 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. -* @param command Command header, input words, word counts, and transaction type. -* @return TRUE if the request was accepted, FALSE otherwise. +* @return TRUE if accepted, FALSE otherwise. *************************************************************************/ -BOOL setPinchValveCommand( VALVE_T valve, const PINCH_VALVE_COMMAND_T *command ) +BOOL homePinchValve( VALVE_T valve ) { BOOL result = FALSE; - if ( ( TRUE == isValidPinchValve( valve ) ) && ( TRUE == isValidPinchValveCommand( command ) ) && - ( PINCH_VALVE_FUNCTION_IDLE_STATE == functionState ) && ( PINCH_VALVE_CMD_IDLE_STATE == commandState ) ) + if ( ( TRUE == isValidPinchValve( valve ) ) && ( FALSE == isPinchValveBusy() ) ) { activeValve = valve; - pendingCommand = *command; + activeFunction = PINCH_VALVE_FUNCTION_HOME; + activeCommandArray = homingCommands; + activeCommandCount = (U08)NUM_OF_PINCH_VALVE_HOMING_COMMANDS; + activeCommandIndex = 0; commandResult = PINCH_VALVE_CMD_RESULT_BUSY; functionState = PINCH_VALVE_FUNCTION_SEND_STATE; result = TRUE; @@ -195,6 +454,57 @@ /*********************************************************************//** * @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() ) ) + { + setSigned32CommandWords( &positionCommands[ PINCH_VALVE_POSITION_CMD_SET_POSITION ], position ); + + activeValve = valve; + activeFunction = PINCH_VALVE_FUNCTION_SET_POSITION; + activeCommandArray = positionCommands; + activeCommandCount = (U08)NUM_OF_PINCH_VALVE_POSITION_COMMANDS; + activeCommandIndex = 0; + commandResult = PINCH_VALVE_CMD_RESULT_BUSY; + functionState = PINCH_VALVE_FUNCTION_SEND_STATE; + 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 @@ -268,217 +578,247 @@ ( PINCH_VALVE_CMD_IDLE_STATE != commandState ) ); } -/*********************************************************************//** -* @brief -* The getPinchValveCommandResult function returns the latest command result. -* @details \b Inputs: commandResult -* @details \b Outputs: none -* @return Latest driver command result. -*************************************************************************/ -PINCH_VALVE_CMD_RESULT_T getPinchValveCommandResult( void ) -{ - return commandResult; -} /*********************************************************************//** * @brief -* The getPinchValveResponse function returns the last response for a valve. -* @details \b Inputs: lastResponse[] -* @details \b Outputs: response -* @param valve H1_VALV or H19_VALV. -* @param response Destination for the response data. -* @return TRUE if the response was returned, FALSE otherwise. -*************************************************************************/ -BOOL getPinchValveResponse( VALVE_T valve, PINCH_VALVE_RESPONSE_T *response ) -{ - BOOL result = FALSE; - - if ( ( TRUE == isValidPinchValve( valve ) ) && ( response != 0 ) ) - { - *response = lastResponse[ valve ]; - result = TRUE; - } - - return result; -} - -/*********************************************************************//** -* @brief -* The handlePinchValveFunction function executes the high level -* IDLE, SEND, and WAIT state machine. +* The execPinchValveFunction function executes the IDLE, SEND, and +* WAIT state machine. * @details \b Inputs: functionState, commandState * @details \b Outputs: functionState, commandState, commandResult * @return none *************************************************************************/ -static void handlePinchValveFunction( void ) +void execPinchValveFunction( void ) { switch ( functionState ) { case PINCH_VALVE_FUNCTION_IDLE_STATE: { - // TODO: Check with Sean and update later break; } case PINCH_VALVE_FUNCTION_SEND_STATE: { - activeCommand = pendingCommand; - clearPinchValveResponse( ¤tResponse ); - commandState = PINCH_VALVE_CMD_SET_CMD_STATE; - functionState = PINCH_VALVE_FUNCTION_WAIT_STATE; + if ( ( activeCommandArray != 0 ) && ( activeCommandIndex < activeCommandCount ) && + ( TRUE == setPinchValveCommand( activeValve, &activeCommandArray[ activeCommandIndex ] ) ) ) + { + functionState = PINCH_VALVE_FUNCTION_WAIT_STATE; + } + else + { + commandResult = PINCH_VALVE_CMD_RESULT_ERROR; + functionState = PINCH_VALVE_FUNCTION_IDLE_STATE; + activeFunction = PINCH_VALVE_FUNCTION_NONE; + } break; } case PINCH_VALVE_FUNCTION_WAIT_STATE: { - if ( PINCH_VALVE_CMD_READY_STATE == commandState ) + PINCH_VALVE_CMD_RESULT_T cmdResult = getPinchValveCommandResult(); + + if ( PINCH_VALVE_CMD_RESULT_COMPLETE == cmdResult ) { - clearPinchValveFpgaCommand(); - lastResponse[ activeValve ] = currentResponse; - commandResult = PINCH_VALVE_CMD_RESULT_COMPLETE; - commandState = PINCH_VALVE_CMD_IDLE_STATE; - functionState = PINCH_VALVE_FUNCTION_IDLE_STATE; + BOOL repeatCommand = FALSE; + + if ( TRUE == isPinchValveCommandCompleted( &repeatCommand ) ) + { + if ( TRUE == repeatCommand ) + { + commandResult = PINCH_VALVE_CMD_RESULT_BUSY; + functionState = PINCH_VALVE_FUNCTION_SEND_STATE; + } + else + { + activeCommandIndex++; + + if ( activeCommandIndex >= activeCommandCount ) + { + commandResult = PINCH_VALVE_CMD_RESULT_COMPLETE; + functionState = PINCH_VALVE_FUNCTION_IDLE_STATE; + activeFunction = PINCH_VALVE_FUNCTION_NONE; + } + else + { + commandResult = PINCH_VALVE_CMD_RESULT_BUSY; + functionState = PINCH_VALVE_FUNCTION_SEND_STATE; + } + } + } + else + { + commandResult = PINCH_VALVE_CMD_RESULT_ERROR; + functionState = PINCH_VALVE_FUNCTION_IDLE_STATE; + activeFunction = PINCH_VALVE_FUNCTION_NONE; + } } - else if ( PINCH_VALVE_CMD_ERROR_STATE == commandState ) + else if ( PINCH_VALVE_CMD_RESULT_ERROR == cmdResult ) { - clearPinchValveFpgaCommand(); - lastResponse[ activeValve ] = currentResponse; - commandResult = PINCH_VALVE_CMD_RESULT_ERROR; - commandState = PINCH_VALVE_CMD_IDLE_STATE; functionState = PINCH_VALVE_FUNCTION_IDLE_STATE; + activeFunction = PINCH_VALVE_FUNCTION_NONE; } - else - { - //TODO: Check with Sean and update later (The command state machine is still executing) - } - break; } default: { - clearPinchValveFpgaCommand(); commandResult = PINCH_VALVE_CMD_RESULT_ERROR; - commandState = PINCH_VALVE_CMD_IDLE_STATE; functionState = PINCH_VALVE_FUNCTION_IDLE_STATE; + activeFunction = PINCH_VALVE_FUNCTION_NONE; break; } } } /*********************************************************************//** * @brief -* The handlePinchValveCommand function executes one FPGA command -* transaction. -* @details \b Inputs: activeCommand, activeValve, FPGA response data -* @details \b Outputs: FPGA command data, currentResponse, commandState -* @return none +* The getPinchValveCommandResult function returns the low-level result. +* @details \b Inputs: commandResult +* @details \b Outputs: none +* @return Latest command result. *************************************************************************/ -static void handlePinchValveCommand( void ) +static PINCH_VALVE_CMD_RESULT_T getPinchValveCommandResult( void ) { - switch ( commandState ) + return commandResult; +} + +/*********************************************************************//** +* @brief +* The getPinchValveResponse function returns the last response for a valve. +* @details \b Inputs: lastResponse[] +* @details \b Outputs: response +* @param valve H1_VALV or H19_VALV. +* @param response Destination for the response data. +* @return TRUE if the response was returned, FALSE otherwise. +*************************************************************************/ +BOOL getPinchValveResponse( VALVE_T valve, PINCH_VALVE_RESPONSE_T *response ) +{ + BOOL result = FALSE; + + if ( ( TRUE == isValidPinchValve( valve ) ) && ( response != 0 ) ) { - case PINCH_VALVE_CMD_IDLE_STATE: - { - break; - } + *response = lastResponse[ valve ]; + result = TRUE; + } - case PINCH_VALVE_CMD_SET_CMD_STATE: - { - writePinchValveCommandRegisters(); + return result; +} - if ( PINCH_VALVE_CMD_ERROR_STATE != commandState ) - { - errorCountAtCommandStart = readPinchValveErrorCount( activeValve ); - commandState = PINCH_VALVE_CMD_XMIT_CMD_STATE; - } +/*********************************************************************//** +* @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. +*************************************************************************/ +BOOL setPinchValveCommand( VALVE_T valve, const PINCH_VALVE_COMMAND_T *command ) +{ + BOOL result = FALSE; - break; - } + if ( ( TRUE == isValidPinchValve( valve ) ) && ( TRUE == isValidPinchValveCommand( command ) ) && + ( PINCH_VALVE_FUNCTION_IDLE_STATE == functionState ) && ( PINCH_VALVE_CMD_IDLE_STATE == commandState ) ) + { + activeValve = valve; + pendingCommand = *command; + commandResult = PINCH_VALVE_CMD_RESULT_BUSY; + functionState = PINCH_VALVE_FUNCTION_SEND_STATE; + result = TRUE; + } - case PINCH_VALVE_CMD_XMIT_CMD_STATE: + 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( BOOL *repeatCommand ) +{ + BOOL result = TRUE; + PINCH_VALVE_RESPONSE_T response; + + if ( repeatCommand == 0 ) + { + result = FALSE; + } + else + { + *repeatCommand = FALSE; + + if ( FALSE == getPinchValveResponse( activeValve, &response ) ) { - if ( TRUE == transmitPinchValveCommand() ) + result = FALSE; + } + else if ( PINCH_VALVE_FUNCTION_HOME == activeFunction ) + { + if ( PINCH_VALVE_HOME_CMD_GET_ACTIVITY_STATUS == activeCommandIndex ) { - executeStartTime = getMSTimerCount(); - statusPollStartMSCount = executeStartTime; - commandState = PINCH_VALVE_CMD_WAIT_DONE_STATE; + U16 required = PINCH_VALVE_ACTIVITY_PHASE_INITIALIZED_MASK | PINCH_VALVE_ACTIVITY_AXIS_SETTLED_MASK; + + if ( required != ( response.outputWord1 & required ) ) + { + *repeatCommand = TRUE; + } } - else + else if ( ( PINCH_VALVE_HOME_CMD_GET_EVENT_STATUS_B == activeCommandIndex ) || + ( PINCH_VALVE_HOME_CMD_GET_EVENT_STATUS_C == activeCommandIndex ) ) { - commandState = PINCH_VALVE_CMD_ERROR_STATE; + if ( 0 == ( response.outputWord1 & ( PINCH_VALVE_EVENT_MOTION_COMPLETE_MASK | PINCH_VALVE_EVENT_MOTION_ERROR_MASK ) ) ) + { + *repeatCommand = TRUE; + } } - - break; + else if ( PINCH_VALVE_HOME_CMD_GET_ACTUAL_POSITION_C == activeCommandIndex ) + { + currentValvePosition[ activeValve ] = getSigned32ResponseValue( &response ); + } } - - case PINCH_VALVE_CMD_WAIT_DONE_STATE: + else if ( PINCH_VALVE_FUNCTION_SET_POSITION == activeFunction ) { - if ( TRUE == didTimeout( executeStartTime, PINCH_VALVE_COMMAND_TIMEOUT_MS ) ) + if ( PINCH_VALVE_POSITION_CMD_GET_ACTIVITY_STATUS == activeCommandIndex ) { - readPinchValveCommandResponse(); - commandState = PINCH_VALVE_CMD_ERROR_STATE; + U16 required = PINCH_VALVE_ACTIVITY_PHASE_INITIALIZED_MASK | PINCH_VALVE_ACTIVITY_AXIS_SETTLED_MASK; + + if ( required != ( response.outputWord1 & required ) ) + { + *repeatCommand = TRUE; + } } - else if ( TRUE == didTimeout( statusPollStartMSCount, PINCH_VALVE_STATUS_POLL_INTERVAL_MS ) ) + else if ( PINCH_VALVE_POSITION_CMD_GET_EVENT_STATUS_BEFORE == activeCommandIndex ) { - readPinchValveCommandResponse(); - statusPollStartMSCount = getMSTimerCount(); - - if ( ( TRUE == didPinchValveErrorCountChange() ) || ( TRUE == isPinchValveControllerFaultActive() ) ) + if ( 0 != ( response.outputWord1 & PINCH_VALVE_EVENT_MOTION_ERROR_MASK ) ) { - commandState = PINCH_VALVE_CMD_ERROR_STATE; + result = FALSE; } - else if ( TRUE == isPinchValveCommandDone() ) + } + else if ( PINCH_VALVE_POSITION_CMD_GET_EVENT_STATUS_AFTER = activeCommandIndex ) + { + if ( 0 != ( response.outputWord1 & PINCH_VALVE_EVENT_MOTION_ERROR_MASK ) ) { - commandState = PINCH_VALVE_CMD_READ_WORDS_STATE; + result = FALSE; } - else + else if ( 0 == ( response.outputWord1 & PINCH_VALVE_EVENT_MOTION_COMPLETE_MASK ) ) { - // TODO: Remove later (FPGA command is still executing) + *repeatCommand = TRUE; } } - else + else if ( PINCH_VALVE_POSITION_CMD_GET_ACTUAL_POSITION == activeCommandIndex ) { - // TODO: Remove later (Wait for the next polling interval) + currentValvePosition[ activeValve ] = getSigned32ResponseValue( &response ); } - - break; } - - case PINCH_VALVE_CMD_READ_WORDS_STATE: + else { - readPinchValveCommandResponse(); - - if ( ( TRUE == didPinchValveErrorCountChange() ) || ( TRUE == isPinchValveControllerFaultActive() ) || ( FALSE == arePinchValveWordCountsValid() ) ) - { - commandState = PINCH_VALVE_CMD_ERROR_STATE; - } - else - { - commandState = PINCH_VALVE_CMD_READY_STATE; - } - - break; + result = FALSE; } - - case PINCH_VALVE_CMD_ERROR_STATE: - { - // TODO: Check with Sean and update later - break; - } - - case PINCH_VALVE_CMD_READY_STATE: - { - // TODO: Check with Sean and update later - break; - } - - default: - { - commandState = PINCH_VALVE_CMD_ERROR_STATE; - break; - } } + + return result; } /*********************************************************************//** @@ -524,7 +864,7 @@ BOOL result = FALSE; U08 fpgaCommand; - if ( TRUE == buildPinchValveFpgaCommandByte( &activeCommand, &fpgaCommand ) ) + if ( TRUE == setPinchValveFpgaCommandByte( &activeCommand, &fpgaCommand ) ) { if ( H1_VALV == activeValve ) { @@ -597,15 +937,14 @@ { setH19FPGACmd( 0 ); } - else - { - //TODO: Check with Sean and Update - } } + + + /*********************************************************************//** * @brief -* The buildPinchValveFpgaCommandByte function builds the FPGA command byte. +* 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 @@ -614,7 +953,7 @@ * @param fpgaCommand Destination for the built FPGA command byte. * @return TRUE if the word counts are valid. *************************************************************************/ -static BOOL buildPinchValveFpgaCommandByte( const PINCH_VALVE_COMMAND_T *command, U08 *fpgaCommand ) +static BOOL setPinchValveFpgaCommandByte( const PINCH_VALVE_COMMAND_T *command, U08 *fpgaCommand ) { BOOL result = FALSE; @@ -623,7 +962,7 @@ U08 transmitWordCount = (U08)( command->inputWordCount + 1 ); if ( ( command->inputWordCount <= PINCH_VALVE_MAX_INPUT_WORDS ) && ( command->outputWordCount <= PINCH_VALVE_MAX_OUTPUT_WORDS ) && - ( transmitWordCount <= 4 ) ) + ( transmitWordCount <= PINCH_VALVE_MAX_TRANSMIT_WORDS ) ) { *fpgaCommand = PINCH_VALVE_FPGA_CMD_START_BIT_MASK; @@ -785,4 +1124,45 @@ } } +/*********************************************************************//** +* @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 & 0xFFFFU ); + } +} + +/*********************************************************************//** +* @brief +* The getSigned32ResponseValue function combines two PMD output words. +* @details \b Inputs: response +* @details \b Outputs: none +* @param response PMD response. +* @return Signed 32-bit value. +*************************************************************************/ +static S32 getSigned32ResponseValue( const PINCH_VALVE_RESPONSE_T *response ) +{ + S32 result = 0; + + if ( response != 0 ) + { + U32 rawValue = ( (U32)response->outputWord1 << 16 ) | (U32)response->outputWord2; + + result = (S32)rawValue; + } + + return result; +} + /**@}*/ Index: firmware/App/Drivers/PinchValve.h =================================================================== diff -u -ra02481d0934a1a13209d652c24837e4f56869c43 -r1f3e57c1351a085c6a05105a50ef73393cc974c6 --- firmware/App/Drivers/PinchValve.h (.../PinchValve.h) (revision a02481d0934a1a13209d652c24837e4f56869c43) +++ firmware/App/Drivers/PinchValve.h (.../PinchValve.h) (revision 1f3e57c1351a085c6a05105a50ef73393cc974c6) @@ -32,56 +32,22 @@ * @{ */ -// ********** public definitions ********** -#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. - -/// 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 response. -typedef struct -{ - U16 outputWord1; ///< Output word 1. - U16 outputWord2; ///< Output word 2. - U16 outputWord3; ///< Output word 3. - U08 spiCmdStatus; ///< FPGA SPI command status. - U08 readCount; ///< FPGA read count. - U08 errorCount; ///< FPGA invalid command error count. -} PINCH_VALVE_RESPONSE_T; - -/// Pinch valve driver command result. -typedef enum pinchValveCommandResults -{ - PINCH_VALVE_CMD_RESULT_IDLE = 0, ///< No command has been requested. - PINCH_VALVE_CMD_RESULT_BUSY, ///< Command is executing. - PINCH_VALVE_CMD_RESULT_COMPLETE, ///< Command completed successfully. - PINCH_VALVE_CMD_RESULT_ERROR ///< Command failed. -} PINCH_VALVE_CMD_RESULT_T; - // ********** public function prototypes ********** void initPinchValveDriver( void ); void execPinchValveDriver( void ); +void execPinchValveFunction( void ); +void execPinchValveCommand( void ); -BOOL setPinchValveCommand( VALVE_T valve, const PINCH_VALVE_COMMAND_T *command ); +BOOL homePinchValve( VALVE_T valve ); +BOOL setPinchValvePosition( VALVE_T valve, S32 position ); +S32 getPinchValvePosition( VALVE_T valve ); void setPinchValveEnableReset( VALVE_T valve, BOOL enable, BOOL reset ); -U08 getPinchValveEnableReset( VALVE_T valve ); +U08 getPinchValveEnableReset( VALVE_T valve ); BOOL isPinchValveBusy( void ); -PINCH_VALVE_CMD_RESULT_T getPinchValveCommandResult( void ); -BOOL getPinchValveResponse( VALVE_T valve, PINCH_VALVE_RESPONSE_T *response ); /**@}*/