Index: firmware/App/Drivers/PinchValve.c =================================================================== diff -u -r71e6f494ca47fd6f4a6791bf6aa8b90e4d2eeb3c -r04bcfefa2d7a3878f75a3bff329e1c4dc4e03de0 --- firmware/App/Drivers/PinchValve.c (.../PinchValve.c) (revision 71e6f494ca47fd6f4a6791bf6aa8b90e4d2eeb3c) +++ firmware/App/Drivers/PinchValve.c (.../PinchValve.c) (revision 04bcfefa2d7a3878f75a3bff329e1c4dc4e03de0) @@ -33,6 +33,8 @@ #define PINCH_VALVE_HOME_OFFSET 10 ///< Offset (in encoder counts) from end of travel edge to set position B and C to. +#define PINCH_VALVE_MOTION_TIMEOUT_MS 10000 + #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. @@ -74,6 +76,10 @@ static const S32 H19_HOMING_VELOCITY_SECOND = -3342; static const S32 POSITION_VELOCITY = 7000; +#define PINCH_VALVE_EVENT_MOTION_COMPLETE_MASK 0x0001 ///< Event Status bit 0: motion complete. +#define PINCH_VALVE_EVENT_MOTION_ERROR_MASK 0x0010 ///< Event Status bit 4: motion error. + + /// Pinch valve function state machine. typedef enum pinchValveFunctionStates { @@ -154,7 +160,7 @@ { 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, TRUE }, { PINCH_VALVE_PMD_CMD_SET_VELOCITY, 0, 0, 0, 2, 0, TRUE }, - { PINCH_VALVE_PMD_CMD_UPDATE, 0, 0, 0, 0, 0, TRUE } + { PINCH_VALVE_PMD_CMD_UPDATE, 0, 0, 0, 0, 0, TRUE }, { 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_GET_ACTUAL_POSITION, 0, 0, 0, 0, 2, FALSE }, @@ -170,12 +176,12 @@ { PINCH_VALVE_PMD_CMD_UPDATE, 0, 0, 0, 0, 0, TRUE }, { 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_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, TRUE }, { PINCH_VALVE_PMD_CMD_SET_VELOCITY, 0, 0, 0, 2, 0, TRUE }, - { PINCH_VALVE_PMD_CMD_UPDATE, 0, 0, 0, 0, 0, TRUE } + { PINCH_VALVE_PMD_CMD_UPDATE, 0, 0, 0, 0, 0, TRUE }, { 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, TRUE }, { 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, TRUE } }; @@ -224,7 +230,7 @@ static U32 executeStartTime[ NUM_OF_VALVES ]; -static U32 tempStart[ NUM_OF_VALVES ]; +static U32 motionStartTime[ NUM_OF_VALVES ]; // ********** private function prototypes ********** @@ -253,6 +259,12 @@ static void clearPinchValveCommandRegisters( VALVE_T valve ); static void resetPinchValveFunctionSequence( VALVE_T valve ); +static BOOL isPinchValveMotionComplete( U16 status ); +static BOOL isPinchValveMotionError( U16 status ); +static BOOL isValidPinchValvePosition( VALVE_POSITION_T position ); + +static BOOL isPinchValveCommandResponseReady( VALVE_T valve ); + /*********************************************************************//** * @brief * The initPinchValveDriver function initializes the pinch valve @@ -316,6 +328,12 @@ // activeCommand[ valve ].outputWordCount = 0; // activeCommand[ valve ].writeOnly = FALSE; + valvePositions[ valve ][ VALVE_POSITION_NOT_IN_POSITION ] = 0; + valvePositions[ valve ][ VALVE_POSITION_A_INSERT_EJECT ] = 0; + valvePositions[ valve ][ VALVE_POSITION_B_OPEN ] = 0; + valvePositions[ valve ][ VALVE_POSITION_C_CLOSE ] = 0; + motionStartTime[ valve ] = 0; + clearActivePinchValveCommand( valve ); clearPinchValveCommandRegisters( valve ); setPinchValveEnableReset( valve, TRUE, FALSE ); @@ -347,7 +365,6 @@ { errorCountAtCommandStart[ valve ] = readPinchValveErrorCount( valve ); commandState[ valve ] = PINCH_VALVE_CMD_XMIT_CMD_STATE; - tempStart[ valve ] = getMSTimerCount(); } else { @@ -357,18 +374,15 @@ case PINCH_VALVE_CMD_XMIT_CMD_STATE: - if ( TRUE == didTimeout(tempStart[ valve ],PINCH_VALVE_COMMAND_RESPONSE_DELAY_MS ) ) + if ( TRUE == transmitPinchValveCommand( valve ) ) { - if ( TRUE == transmitPinchValveCommand( valve ) ) - { - executeStartTime[ valve ] = getMSTimerCount(); - commandState[ valve ] = PINCH_VALVE_CMD_WAIT_DONE_STATE; - } - else - { - commandState[ valve ] = PINCH_VALVE_CMD_ERROR_STATE; - } + executeStartTime[ valve ] = getMSTimerCount(); + commandState[ valve ] = PINCH_VALVE_CMD_WAIT_DONE_STATE; } + else + { + commandState[ valve ] = PINCH_VALVE_CMD_ERROR_STATE; + } break; @@ -385,7 +399,7 @@ { commandState[ valve ] = PINCH_VALVE_CMD_ERROR_STATE; } - else if ( TRUE == isPinchValveCommandDone( valve ) ) + else if ( TRUE == isPinchValveCommandResponseReady( valve ) ) { commandState[ valve ] = PINCH_VALVE_CMD_READ_WORDS_STATE; } @@ -402,10 +416,30 @@ } else { - commandState[ valve ] = processPinchValveCommandOutput( valve ); + PINCH_VALVE_CMD_STATE_T nextState; + + nextState = processPinchValveCommandOutput( valve ); clearPinchValveFpgaCommand( valve ); - clearActivePinchValveCommand( valve ); - commandFailed[ valve ] = FALSE; + + if ( PINCH_VALVE_CMD_IDLE_STATE == nextState ) + { + clearActivePinchValveCommand( valve ); + commandFailed[ valve ] = FALSE; + commandState[ valve ] = PINCH_VALVE_CMD_IDLE_STATE; + } + else if ( PINCH_VALVE_CMD_SET_CMD_STATE == nextState ) + { + commandOutputWord1[ valve ] = 0; + commandOutputWord2[ valve ] = 0; + commandOutputWord3[ valve ] = 0; + commandSpiStatus[ valve ] = 0; + commandFailed[ valve ] = FALSE; + commandState[ valve ] = PINCH_VALVE_CMD_SET_CMD_STATE; + } + else + { + commandState[ valve ] = PINCH_VALVE_CMD_ERROR_STATE; + } } break; @@ -687,103 +721,12 @@ default: commandFailed[ valve ] = TRUE; resetPinchValveFunctionSequence( valve ); - // TODO: Raise the project-defined software fault for an invalid pinch-valve function state. + // TODO: software fault for an invalid pinch-valve function state. break; } } } -//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; -// activeCommandCount[ valve ] = 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; -// setSigned32CommandWords( &positionCommands[ PINCH_VALVE_POSITION_CMD_SET_POSITION ], pendingValvePosition[ valve ] ); -// activeCommandArray[ valve ] = positionCommands; -// activeCommandCount[ valve ] = 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 ( activeCommandIndex[ valve ] < activeCommandCount[ valve ] ) -// { -// if ( PINCH_VALVE_CMD_IDLE_STATE == commandState[ valve ] ) -// { -// 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; -// } -// } -// } -// else -// { -// functionState[ valve ] = PINCH_VALVE_FUNCTION_IDLE_STATE; -// } -// break; -// -// case PINCH_VALVE_FUNCTION_WAIT_STATE: -// if ( PINCH_VALVE_CMD_IDLE_STATE == commandState[ valve ] ) -// { -// if ( TRUE == commandFailed[ valve ] ) -// { -// functionState[ valve ] = PINCH_VALVE_FUNCTION_IDLE_STATE; -// // TODO: Valve fault -// } -// else -// { -// activeCommandIndex[ valve ]++; -// if ( activeCommandIndex[ valve ] >= activeCommandCount[ valve ] ) -// { -// activeCommandArray[ valve ] = 0; -// activeCommandCount[ valve ] = 0; -// activeCommandIndex[ valve ] = 0; -// functionState[ valve ] = PINCH_VALVE_FUNCTION_IDLE_STATE; -// } -// else -// { -// functionState[ valve ] = PINCH_VALVE_FUNCTION_SEND_STATE; -// } -// } -// } -// break; -// -// default: -// activeCommandArray[ valve ] = 0; -// activeCommandCount[ valve ] = 0; -// activeCommandIndex[ valve ] = 0; -// commandFailed[ valve ] = TRUE; -// functionState[ valve ] = PINCH_VALVE_FUNCTION_IDLE_STATE; -// // TODO: s/w fault -// break; -// } -// } -//} - /*********************************************************************//** * @brief * The setPinchValveCommand function requests execution @@ -1028,12 +971,28 @@ *************************************************************************/ static BOOL arePinchValveWordCountsValid( VALVE_T valve ) { - U08 expectedTransmitCount = (U08)( activeCommand[ valve ].inputWordCount + 1 ); - 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 ); + BOOL result = FALSE; + U08 expectedTransmitCount; + U08 transmittedCount; + U08 receivedCount; - //return ( ( transmittedCount == expectedTransmitCount ) && ( receivedCount == activeCommand[ valve ].outputWordCount ) ); - return TRUE; + if ( TRUE == isValidPinchValve( valve ) ) + { + // Transmit count for command header. + expectedTransmitCount = (U08)( activeCommand[ valve ].inputWordCount + 1 ); + transmittedCount = (U08)( ( commandSpiStatus[ valve ] & PINCH_VALVE_SPI_TX_COUNT_MASK ) >> PINCH_VALVE_SPI_TX_COUNT_SHIFT ); + // Receive count does not required for command header. + receivedCount = (U08)( ( commandSpiStatus[ valve ] & PINCH_VALVE_SPI_RX_COUNT_MASK ) >> PINCH_VALVE_SPI_RX_COUNT_SHIFT ); + + result = ( transmittedCount == expectedTransmitCount ); + + if ( 0 != activeCommand[ valve ].outputWordCount ) + { + result = ( result && ( receivedCount == activeCommand[ valve ].outputWordCount ) ); + } + } + + return result; } /*********************************************************************//** @@ -1149,59 +1108,90 @@ *************************************************************************/ static PINCH_VALVE_CMD_STATE_T processPinchValveCommandOutput( VALVE_T valve ) { - PINCH_VALVE_CMD_STATE_T result = ; // V - IDLE + PINCH_VALVE_CMD_STATE_T result = PINCH_VALVE_CMD_ERROR_STATE; + S32 positionB; + S32 positionC; if ( TRUE == isValidPinchValve( valve ) ) { + result = PINCH_VALVE_CMD_IDLE_STATE; + switch ( activeCommand[ valve ].cmdHeader ) { + case PINCH_VALVE_PMD_CMD_UPDATE: + motionStartTime[ valve ] = getMSTimerCount(); + break; + case PINCH_VALVE_PMD_CMD_GET_ACTIVITY_STATUS: activityStatus[ valve ] = commandOutputWord1[ valve ]; break; case PINCH_VALVE_PMD_CMD_GET_EVENT_STATUS: eventStatus[ valve ] = commandOutputWord1[ valve ]; // TODO - if not motion complete or motion error, stay in this state, otherwise idle - break; + // During homing, Motion Complete bit 0 and Motion Error bit 4, both be set when the valve reaches the mechanical stop. + // Therefore, check Motion Complete first. - default: - // Check with Sean TODO later - break; - } - - switch ( activeCommandIndex[ valve ] ) - { - case PINCH_VALVE_HOME_CMD_GET_ACTUAL_POSITION_B: - currentValvePosition[ valve ] = getSigned32OutputValue( valve ) - if ( H1_VALV == valve ) + if ( TRUE == isPinchValveMotionComplete( eventStatus[ valve ] ) ) { - valvePositions[ valve ][ VALVE_POSITION_B_OPEN ] = currentValvePosition[ valve ] + PINCH_VALVE_HOME_OFFSET; + result = PINCH_VALVE_CMD_IDLE_STATE; } - else + else if ( TRUE == isPinchValveMotionError( eventStatus[ valve ] ) ) { - valvePositions[ valve ][ VALVE_POSITION_B_OPEN ] = currentValvePosition[ valve ] - PINCH_VALVE_HOME_OFFSET; + result = PINCH_VALVE_CMD_ERROR_STATE; } - break; - - case PINCH_VALVE_HOME_CMD_GET_ACTUAL_POSITION_C: - currentValvePosition[ valve ] = getSigned32OutputValue( valve ); - if ( H1_VALV == valve ) + else if ( TRUE == didTimeout( motionStartTime[ valve ], PINCH_VALVE_MOTION_TIMEOUT_MS ) ) { - valvePositions[ valve ][ VALVE_POSITION_C_CLOSE ] = currentValvePosition[ valve ] - PINCH_VALVE_HOME_OFFSET; - valvePositions[ valve ][ VALVE_POSITION_A_INSERT_EJECT ] = ; + result = PINCH_VALVE_CMD_ERROR_STATE; } else { - valvePositions[ valve ][ VALVE_POSITION_C_CLOSE ] = currentValvePosition[ valve ] + PINCH_VALVE_HOME_OFFSET; - valvePositions[ valve ][ VALVE_POSITION_A_INSERT_EJECT ] = ; + // This is to check whether the motion is still running and repeating the same to Get Event Status + result = PINCH_VALVE_CMD_SET_CMD_STATE; } break; default: // Check with Sean TODO later break; } + + if ( PINCH_VALVE_PMD_CMD_GET_ACTUAL_POSITION == activeCommand[ valve ].cmdHeader ) + { + switch ( activeCommandIndex[ valve ] ) + { + case PINCH_VALVE_HOME_CMD_GET_ACTUAL_POSITION_B: + if ( H1_VALV == valve ) + { + valvePositions[ valve ][ VALVE_POSITION_B_OPEN ] = currentValvePosition[ valve ] + PINCH_VALVE_HOME_OFFSET; + } + else + { + valvePositions[ valve ][ VALVE_POSITION_B_OPEN ] = currentValvePosition[ valve ] - PINCH_VALVE_HOME_OFFSET; + } + break; + + case PINCH_VALVE_HOME_CMD_GET_ACTUAL_POSITION_C: + if ( H1_VALV == valve ) + { + valvePositions[ valve ][ VALVE_POSITION_C_CLOSE ] = currentValvePosition[ valve ] - PINCH_VALVE_HOME_OFFSET; + } + else + { + valvePositions[ valve ][ VALVE_POSITION_C_CLOSE ] = currentValvePosition[ valve ] + PINCH_VALVE_HOME_OFFSET; + } + positionB = valvePositions[ valve ][ VALVE_POSITION_B_OPEN ]; + positionC = valvePositions[ valve ][ VALVE_POSITION_C_CLOSE ]; + valvePositions[ valve ][ VALVE_POSITION_A_INSERT_EJECT ] = positionB + ( ( positionC - positionB ) / 2 ); + break; + + default: + // Check with Sean TODO later + break; + } + } } + return result; } //TODO Temporary code from here @@ -1290,5 +1280,141 @@ } } +/*********************************************************************//** +* @brief +* The isPinchValveMotionComplete function stores output data returned by +* Get Activity Status, Get Event Status, and Get Actual Position commands. +* @details \b Inputs: activeCommand, commandOutputWord1, +* commandOutputWord2, commandOutputWord3 +* @details \b Outputs: activityStatus, eventStatus, +* currentValvePosition +* @param valve H1_VALV or H19_VALV. +* @return none +*************************************************************************/ +static BOOL isPinchValveMotionComplete( U16 status ) +{ + return ( 0 != ( status & PINCH_VALVE_EVENT_MOTION_COMPLETE_MASK ) ); +} +/*********************************************************************//** +* @brief +* The isPinchValveMotionComplete function stores output data returned by +* Get Activity Status, Get Event Status, and Get Actual Position commands. +* @details \b Inputs: activeCommand, commandOutputWord1, +* commandOutputWord2, commandOutputWord3 +* @details \b Outputs: activityStatus, eventStatus, +* currentValvePosition +* @param valve H1_VALV or H19_VALV. +* @return none +*************************************************************************/ +static BOOL isPinchValveMotionError( U16 status ) +{ + return ( 0 != ( status & PINCH_VALVE_EVENT_MOTION_ERROR_MASK ) ); +} + + +//Temporary code this is adding to store position functions + +/*********************************************************************//** +* @brief +* The isPinchValveMotionComplete function stores output data returned by +* Get Activity Status, Get Event Status, and Get Actual Position commands. +* @details \b Inputs: activeCommand, commandOutputWord1, +* commandOutputWord2, commandOutputWord3 +* @details \b Outputs: activityStatus, eventStatus, +* currentValvePosition +* @param valve H1_VALV or H19_VALV. +* @return none +*************************************************************************/ +static BOOL isValidPinchValvePosition( VALVE_POSITION_T position ) +{ + return ( ( position > VALVE_POSITION_NOT_IN_POSITION ) && ( position < NUM_OF_VALVE_POSITIONS ) ); +} + +/*********************************************************************//** +* @brief +* The isPinchValveMotionComplete function stores output data returned by +* Get Activity Status, Get Event Status, and Get Actual Position commands. +* @details \b Inputs: activeCommand, commandOutputWord1, +* commandOutputWord2, commandOutputWord3 +* @details \b Outputs: activityStatus, eventStatus, +* currentValvePosition +* @param valve H1_VALV or H19_VALV. +* @return none +*************************************************************************/ +S32 getPinchValveStoredPosition( VALVE_T valve, VALVE_POSITION_T position ) +{ + S32 result = 0; + + if ( ( TRUE == isValidPinchValve( valve ) ) && ( TRUE == isValidPinchValvePosition( position ) ) ) + { + result = valvePositions[ valve ][ position ]; + } + + return result; +} + +/*********************************************************************//** +* @brief +* The isPinchValveMotionComplete function stores output data returned by +* Get Activity Status, Get Event Status, and Get Actual Position commands. +* @details \b Inputs: activeCommand, commandOutputWord1, +* commandOutputWord2, commandOutputWord3 +* @details \b Outputs: activityStatus, eventStatus, +* currentValvePosition +* @param valve H1_VALV or H19_VALV. +* @return none +*************************************************************************/ +BOOL movePinchValveToStoredPosition( VALVE_T valve, VALVE_POSITION_T position ) +{ + BOOL result = FALSE; + + if ( ( TRUE == isValidPinchValve( valve ) ) && ( TRUE == isValidPinchValvePosition( position ) ) ) + { + result = setPinchValvePosition( valve, valvePositions[ valve ][ position ] ); + } + + return result; +} + +/*********************************************************************//** +* @brief +* The isPinchValveMotionComplete function stores output data returned by +* Get Activity Status, Get Event Status, and Get Actual Position commands. +* @details \b Inputs: activeCommand, commandOutputWord1, +* commandOutputWord2, commandOutputWord3 +* @details \b Outputs: activityStatus, eventStatus, +* currentValvePosition +* @param valve H1_VALV or H19_VALV. +* @return none +*************************************************************************/ +static BOOL isPinchValveCommandResponseReady( VALVE_T valve ) +{ + BOOL result = FALSE; + U08 receivedCount; + + if ( TRUE == isValidPinchValve( valve ) ) + { + if ( TRUE == isPinchValveCommandDone( valve ) ) + { + // This is for the commands expecting no returned words, the done bit is sufficient. + if ( 0 == activeCommand[ valve ].outputWordCount ) + { + result = TRUE; + } + else + { + receivedCount = (U08)( ( commandSpiStatus[ valve ] & PINCH_VALVE_SPI_RX_COUNT_MASK ) >> PINCH_VALVE_SPI_RX_COUNT_SHIFT ); + + if ( receivedCount == activeCommand[ valve ].outputWordCount ) + { + result = TRUE; + } + } + } + } + + return result; +} + /**@}*/