Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -rf31c92f93880dbe20596a727dd72f48813ffa88a -r9d11c51da60da3f8d2917433938a583f8a105318 --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision f31c92f93880dbe20596a727dd72f48813ffa88a) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 9d11c51da60da3f8d2917433938a583f8a105318) @@ -17,6 +17,7 @@ #include "AlarmMgmtTD.h" #include "CpldInterface.h" +#include "FpgaTD.h" #include "Messaging.h" #include "OperationModes.h" #include "PersistentAlarm.h" @@ -36,45 +37,59 @@ #define VALVE_TRANSITION_MIN_TGT_DELTA 10 ///< Minimum encoder position delta from target position to end transition state. #define VALVE_HOME_MIN_POS_CHG 3 ///< Minimum encoder position change to indicate a home operation is still moving toward edge. #define VALVE_HOME_BACK_OFF_EDGE 3 ///< Encoder counts to back off of detected edge position. -#define MAX_ALLOWED_FAILED_VALVE_HOMINGS 3U ///< Maximum allowed failed valve home attempts #define MAX_HOME_FULL_TRAVEL_DIFF 10U ///< Maximum allowed difference in full travel encoder counts between expected and measured during home operation. -#define VALVE_TRANSITION_TIMEOUT_MS 3000U ///< Valves transition time out in ms +#define VALVE_TRANSITION_TIMEOUT_MS ( 3 * MS_PER_SECOND ) ///< Valves transition time out in ms. +#define HOMING_EDGE_DETECTION_TIMEOUT_MS ( 0.5 * MS_PER_SECOND ) ///< Valves homing edge detection timeout in milliseconds. +#define VALVE_FORCE_HOME TRUE ///< Force valve to home even if already homed. +#define ZERO_ENC_DEBOUNCE_THRESHOLD_CNT 5 ///< Valves zero encoder debounce threshold count. +#define ZERO_ENC_DEBOUNCE_TIMEOUT_MS ( 0.25 * MS_PER_SECOND ) ///< Valves zero encoder debounce timeout in milliseconds. +#define POS_D_PARTIAL_CLOSE_FROM_ZERO_CNT 32 ///< Position D partial close from zero position in counts. -#define VALVE_FORCE_HOME TRUE ///< Force valve to home even if already homed - /// Valve controller states typedef enum Valve_Control_States { - VALVE_STATE_WAIT_FOR_POST = 0, ///< Valve state wait for POST - VALVE_STATE_HOMING_NOT_STARTED, ///< Valve state homing not started - VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE, ///< Valve state homing find energized edge - VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE, ///< Valve state homing find de-energized edge - VALVE_STATE_IDLE, ///< Valve state idle - VALVE_STATE_IN_TRANSITION, ///< Valve state in transition - NUM_OF_VALVE_STATES, ///< Number of valve exec states + VALVE_STATE_WAIT_FOR_POST = 0, ///< Valve state wait for POST. + VALVE_STATE_RESET_VALVE, ///< Valve state reset valve. + VALVE_STATE_RESET_ENCODER, ///< Valve state reset encoder. + VALVE_STATE_ENABLE_VALVE, ///< Valve state enable valve. + VALVE_STATE_HOMING_NOT_STARTED, ///< Valve state homing not started. + VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE, ///< Valve state homing find energized edge. + VALVE_STATE_IDLE, ///< Valve state idle. + VALVE_STATE_IN_TRANSITION, ///< Valve state in transition. + NUM_OF_VALVE_STATES, ///< Number of valve exec states. } VALVE_STATE_T; +/// Valve homing status +typedef enum Valve_Homing_Status +{ + VALVE_HOMING_INIT = 0, ///< Valve homing init. + VALVE_HOMING_FORWARD, ///< Valve homing forward. + VALVE_HOMING_REVERSE, ///< Valve homing reverse. + VALVE_HOMING_COMPLETE, ///< Valve homing complete. + NUM_OF_VALVE_HOMING_STATE, ///< Number of valve homing. +} VALVE_HOMING_STATUS_T; + /// Valve status structure typedef struct { - VALVE_POSITION_T commandedPosition; ///< Valve commanded position enum - VALVE_POSITION_T currentPosition; ///< Valve current position enum - VALVE_POSITION_T pendingCommandedPosition; ///< Valve pending position enum - S16 targetEncPosition; ///< Valve target position in encoder counts - S16 currentEncPosition; ///< Valve current position in encoder counts - S16 priorEncPosition; ///< Valve prior position in encoder counts - BOOL hasTransitionBeenRequested; ///< Valve transition request flag - VALVE_STATE_T controlState; ///< Valve control state - U32 transitionStartTime; ///< Valve transition start time - S16 positionsABC[ NUM_OF_VALVE_POSITIONS ]; ///< Valve positions (in encoder counts) for positions A, B, and C - U32 overCurrentCounter; ///< Valve over current counter - U32 positionOutOfRangeCounter; ///< Valve position out of range counter + VALVE_POSITION_T commandedPosition; ///< Valve commanded position enum. + VALVE_POSITION_T currentPosition; ///< Valve current position enum. + VALVE_POSITION_T pendingCommandedPosition; ///< Valve pending position enum. + S16 currentEncPosition; ///< Valve current position in encoder counts. + S16 priorEncPosition; ///< Valve prior position in encoder counts. + BOOL hasTransitionBeenRequested; ///< Valve transition request flag. + VALVE_STATE_T controlState; ///< Valve control state. + U32 valveOpsStartTime; ///< Valve transition start time. + S16 positionsABC[ NUM_OF_VALVE_POSITIONS ]; ///< Valve positions (in encoder counts) for positions A, B, and C. + U32 overCurrentCounter; ///< Valve over current counter. + U32 positionOutOfRangeCounter; ///< Valve position out of range counter. // Homing variables - U32 numberOfFailedHomings; ///< Valve number of failed homing - U32 homingEdgeDetectionCounter; ///< Valve homing counter when there is not much travel - BOOL hasHomingBeenRequested; ///< Valve homing request flag - BOOL hasValveBeenHomed; ///< Valve homing completed flag + BOOL hasHomingBeenRequested; ///< Valve homing request flag. + BOOL hasValveBeenHomed; ///< Valve homing completed flag. BOOL hasHomingFailed; ///< Valve homing failed flag + S16 proposedEnergizedPos; ///< Valve homing proposed energized position. + BOOL hasValveBeenReset; ///< Valve homing has valve been reset. + VALVE_HOMING_STATUS_T valveHomingStatus; ///< Valve homing status. } VALVE_STATUS_T; /// Payload record structure for pinch valve set position request @@ -103,12 +118,14 @@ // ********** private function prototypes ********** static VALVE_STATE_T handleValvesWait4PostState( VALVE_T valve ); +static VALVE_STATE_T handleValvesResetValve( VALVE_T valve ); +static VALVE_STATE_T handleValvesResetEncoder( VALVE_T valve ); +static VALVE_STATE_T handleValvesEnableValve( VALVE_T valve ); static VALVE_STATE_T handleValvesNotHomedState( VALVE_T valve ); static VALVE_STATE_T handleValvesFindEnergizedEdgeState( VALVE_T valve ); -static VALVE_STATE_T handleValvesFindDeenergizedEdgeState( VALVE_T valve ); static VALVE_STATE_T handleValvesIdleState( VALVE_T valve ); static VALVE_STATE_T handleValvesTransitionState( VALVE_T valve ); - +static void checkValveInRange( VALVE_T valve, SW_FAULT_ID_T SWFault ); static void publishValvesData( void ); /*********************************************************************//** @@ -118,18 +135,28 @@ * @details \b Outputs: Valves unit initialized * @return none *************************************************************************/ -void initValves(void) +void initValves( void ) { // Initialize driver initRotaryValvesDriver(); // Initialize controller variables memset( ¤tValveStates[0], 0, sizeof( currentValveStates ) ); - valvesDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; - valvesDataPublishInterval.data = VALVES_DATA_PUB_INTERVAL; - valvesDataPublishInterval.ovData = VALVES_DATA_PUB_INTERVAL; + valvesDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; + valvesDataPublishInterval.data = VALVES_DATA_PUB_INTERVAL; + valvesDataPublishInterval.ovData = VALVES_DATA_PUB_INTERVAL; valvesDataPublishInterval.ovInitData = 0; - valvesDataPublishInterval.override = OVERRIDE_RESET; + valvesDataPublishInterval.override = OVERRIDE_RESET; + +#ifdef ASN_DEMO + currentValveStates[ H1_VALV ].positionsABC[ VALVE_POSITION_C_CLOSE ] = ROTARY_VALVE_MICROSTEP_FRACTION; + currentValveStates[ H1_VALV ].positionsABC[ VALVE_POSITION_B_OPEN ] = 1728; + currentValveStates[ H1_VALV ].positionsABC[ VALVE_POSITION_A_INSERT_EJECT ] = 880; + + currentValveStates[ H19_VALV ].positionsABC[ VALVE_POSITION_C_CLOSE ] = ROTARY_VALVE_MICROSTEP_FRACTION; + currentValveStates[ H19_VALV ].positionsABC[ VALVE_POSITION_B_OPEN ] = 1728; + currentValveStates[ H19_VALV ].positionsABC[ VALVE_POSITION_A_INSERT_EJECT ] = 880; +#endif } /*********************************************************************//** @@ -147,7 +174,6 @@ { BOOL result = FALSE; -#ifndef TEST_NO_PINCH_VALVES if ( valve < NUM_OF_VALVES ) { VALVE_STATE_T currState = currentValveStates[ valve ].controlState; @@ -171,7 +197,6 @@ { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE8, (U32)valve ) } -#endif return result; } @@ -190,7 +215,6 @@ { BOOL result = FALSE; -#ifndef TEST_NO_PINCH_VALVES if ( ( valve < NUM_OF_VALVES ) && ( position < NUM_OF_VALVE_POSITIONS ) ) { currentValveStates[ valve ].pendingCommandedPosition = position; @@ -202,7 +226,6 @@ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_PARAM, ( (U32)valve << SHIFT_16_BITS_FOR_WORD_SHIFT ) | (U32)position ) } -#endif return result; } @@ -250,7 +273,7 @@ for ( valve = FIRST_VALVE; valve < NUM_OF_VALVES; valve++ ) { // update valve encoder positions - currentValveStates[ valve ].priorEncPosition = currentValveStates[ valve ].currentEncPosition; + currentValveStates[ valve ].priorEncPosition = currentValveStates[ valve ].currentEncPosition; currentValveStates[ valve ].currentEncPosition = getValveEncoderPosition( valve ); // execute valve state machine @@ -260,6 +283,18 @@ currentValveStates[ valve ].controlState = handleValvesWait4PostState( valve ); break; + case VALVE_STATE_RESET_VALVE: + currentValveStates[ valve ].controlState = handleValvesResetValve( valve ); + break; + + case VALVE_STATE_RESET_ENCODER: + currentValveStates[ valve ].controlState = handleValvesResetEncoder( valve ); + break; + + case VALVE_STATE_ENABLE_VALVE: + currentValveStates[ valve ].controlState = handleValvesEnableValve( valve ); + break; + case VALVE_STATE_HOMING_NOT_STARTED: currentValveStates[ valve ].controlState = handleValvesNotHomedState( valve ); break; @@ -268,10 +303,6 @@ currentValveStates[ valve ].controlState = handleValvesFindEnergizedEdgeState( valve ); break; - case VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE: - currentValveStates[ valve ].controlState = handleValvesFindDeenergizedEdgeState( valve ); - break; - case VALVE_STATE_IDLE: currentValveStates[ valve ].controlState = handleValvesIdleState( valve ); break; @@ -293,172 +324,242 @@ * @brief * The handleValvesWait4PostState function handles the Wait for POST state * of the state machine for a given valve. - * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given valve is invalid. * @details \b Inputs: current operating mode * @details \b Outputs: none * @param valve ID of valve for which to handle the Wait for POST state * @return next state of the state machine for the given valve *************************************************************************/ static VALVE_STATE_T handleValvesWait4PostState( VALVE_T valve ) { -#ifndef TEST_PINCH_VALVES VALVE_STATE_T nextState = VALVE_STATE_WAIT_FOR_POST; -#else - VALVE_STATE_T nextState = VALVE_STATE_HOMING_NOT_STARTED; -#endif - if ( valve < NUM_OF_VALVES ) + checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_WAIT_FOR_POST_STATE ); + + if ( getCurrentOperationMode() != MODE_INIT ) { - if ( getCurrentOperationMode() != MODE_INIT ) - { - nextState = VALVE_STATE_HOMING_NOT_STARTED; - } + nextState = VALVE_STATE_RESET_VALVE; } - else - { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE2, (U32)valve ) - } return nextState; } /*********************************************************************//** * @brief - * The handleValvesNotHomedState function handles the Not Homed state - * of the state machine for a given valve. - * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given valve is invalid. + * The handleValvesResetValve function handles the reset valve of the state + * machine for a given valve. + * @details \b Inputs: current operating mode + * @details \b Outputs: none + * @param valve ID of valve for which to handle the Wait for POST state + * @return next state of the state machine for the given valve + *************************************************************************/ +static VALVE_STATE_T handleValvesResetValve( VALVE_T valve ) +{ + VALVE_STATE_T nextState = VALVE_STATE_RESET_ENCODER; + + checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_RESET ); + resetValve( valve ); + + return nextState; +} + +/*********************************************************************//** + * @brief + * The handleValvesResetEncoder function handles the reset encoder of the state + * machine for a given valve. + * @details \b Inputs: current operating mode + * @details \b Outputs: none + * @param valve ID of valve for which to handle the Wait for POST state + * @return next state of the state machine for the given valve + *************************************************************************/ +static VALVE_STATE_T handleValvesResetEncoder( VALVE_T valve ) +{ + VALVE_STATE_T nextState = VALVE_STATE_ENABLE_VALVE; + + checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_RESET_ENCODER ); + resetValveEncoder( valve ); + // Set the timer for the first debounce read + currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); + + return nextState; +} + +/*********************************************************************//** + * @brief + * The handleValvesEnableValve function handles the enable valve of the state + * machine for a given valve. * @details \b Inputs: currentValveStates[] * @details \b Outputs: currentValveStates[] - * @param valve ID of valve for which to handle the Not Homed state + * @param valve ID of valve for which to handle the Wait for POST state * @return next state of the state machine for the given valve *************************************************************************/ -static VALVE_STATE_T handleValvesNotHomedState( VALVE_T valve ) +static VALVE_STATE_T handleValvesEnableValve( VALVE_T valve ) { - VALVE_STATE_T nextState = VALVE_STATE_HOMING_NOT_STARTED; + VALVE_STATE_T nextState = VALVE_STATE_ENABLE_VALVE; + S16 valveEncPosistion = getValveEncoderPosition( valve ); - if ( valve < NUM_OF_VALVES ) - { - if ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) - { - U16 mag = ROTARY_VALVE_FULL_SWING_TRAVEL_COUNTS; + checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_ENABLE ); - // command valve to move to energized edge (end of travel) - currentValveStates[ valve ].homingEdgeDetectionCounter = 0; - currentValveStates[ valve ].targetEncPosition = mag; - currentValveStates[ valve ].hasValveBeenHomed = FALSE; - currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; - setValveCmdChangePosition( valve, (U16)mag, MOTOR_DIR_FORWARD ); - nextState = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; - } + if ( FALSE == currentValveStates[ valve ].hasValveBeenReset ) + { + enableValve( valve ); + currentValveStates[ valve ].hasValveBeenReset = TRUE; } else { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE3, (U32)valve ) + setValveZeroEncoderPosition( valve ); } + if ( valveEncPosistion >= ZERO_ENC_DEBOUNCE_THRESHOLD_CNT ) + { + // Keep reseting the debouce timer until the encoder count is less than the encoder count is less than the number + currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); + } + else if ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, ZERO_ENC_DEBOUNCE_TIMEOUT_MS ) ) + { + // Position C is hard coded as count 8. + // Position D is hard coded as an offset form 0. + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ] = ROTARY_VALVE_MICROSTEP_FRACTION; + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_D_PARTIAL_CLOSE ] = POS_D_PARTIAL_CLOSE_FROM_ZERO_CNT; + nextState = VALVE_STATE_HOMING_NOT_STARTED; + } + return nextState; } /*********************************************************************//** * @brief - * The handleValvesFindEnergizedEdgeState function handles the Find Energized - * Edge state of the state machine for a given valve. - * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given valve is invalid. + * The handleValvesNotHomedState function handles the Not Homed state + * of the state machine for a given valve. * @details \b Inputs: currentValveStates[] * @details \b Outputs: currentValveStates[] - * @param valve ID of valve for which to handle the Find Energized Edge state + * @param valve ID of valve for which to handle the Not Homed state * @return next state of the state machine for the given valve *************************************************************************/ -static VALVE_STATE_T handleValvesFindEnergizedEdgeState( VALVE_T valve ) +static VALVE_STATE_T handleValvesNotHomedState( VALVE_T valve ) { - VALVE_STATE_T nextState = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; + VALVE_STATE_T nextState = VALVE_STATE_HOMING_NOT_STARTED; - if ( valve < NUM_OF_VALVES ) + checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_NOT_HOMED ); + + if ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) { - S16 curPos = currentValveStates[ valve ].currentEncPosition; + currentValveStates[ valve ].proposedEnergizedPos = ROTARY_VALVE_INIT_FULL_SWING_COUNTS; - // have we found foward edge of travel? - if ( fabs( curPos - currentValveStates[ valve ].priorEncPosition ) < VALVE_HOME_MIN_POS_CHG ) - { - U16 mag = ROTARY_VALVE_FULL_SWING_TRAVEL_COUNTS + VALVE_HOME_MIN_POS_CHG; + //switch ( currentValveStates[ valve ].valveHomingStatus ) + //{ + // case VALVE_HOMING_INIT: + // currentValveStates[ valve ].proposedEnergizedPos = 1880; //ROTARY_VALVE_INIT_FULL_SWING_COUNTS; + // break; + // + // case VALVE_HOMING_FORWARD: + // currentValveStates[ valve ].proposedEnergizedPos += ROTARY_VALVE_MICROSTEP_FRACTION; + // break; + // + // case VALVE_HOMING_REVERSE: + // currentValveStates[ valve ].proposedEnergizedPos -= ROTARY_VALVE_MICROSTEP_FRACTION; + // break; + // + // default: + // // Do nothing as complete has nothing to do + // break; + //} - // Record edge position - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ] = curPos - VALVE_HOME_BACK_OFF_EDGE; - // command valve to move to de-energized edge (end of travel) - currentValveStates[ valve ].homingEdgeDetectionCounter = 0; - currentValveStates[ valve ].targetEncPosition = 0; - setValveCmdChangePosition( valve, mag, MOTOR_DIR_REVERSE ); - nextState = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; + if ( ( currentValveStates[ valve ].proposedEnergizedPos <= ROTARY_VALVE_ENERGIZED_EDGE_MIN_COUNTS ) || + ( currentValveStates[ valve ].proposedEnergizedPos >= ROTARY_VALVE_ENERGIZED_EDGE_MAX_COUNTS ) ) + { + currentValveStates[ valve ].hasHomingFailed = TRUE; + currentValveStates[ valve ].hasHomingBeenRequested = FALSE; + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_VALVE_HOMING_FAILED, (U32)valve, (U32)currentValveStates[ valve ].proposedEnergizedPos ) } + else + { + // Command valve to move to energized edge (end of travel) + currentValveStates[ valve ].valveOpsStartTime = 0; + currentValveStates[ valve ].hasValveBeenHomed = FALSE; + currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; + nextState = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; + setValveCmdChangePosition( valve, (U16)currentValveStates[ valve ].proposedEnergizedPos, MOTOR_DIR_FORWARD ); + } } - else - { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE4, (U32)valve ) - } return nextState; } /*********************************************************************//** * @brief - * The handleValvesFindDeenergizedEdgeState function handles the Find - * De-energized Edge state of the state machine for a given valve. - * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given valve is invalid. - * @details \b Inputs: - * @details \b Outputs: - * @param valve ID of valve for which to handle the Find De-energized Edge state + * The handleValvesFindEnergizedEdgeState function handles the Find Energized + * Edge state of the state machine for a given valve. + * @details \b Inputs: currentValveStates[] + * @details \b Outputs: currentValveStates[] + * @param valve ID of valve for which to handle the Find Energized Edge state * @return next state of the state machine for the given valve *************************************************************************/ -static VALVE_STATE_T handleValvesFindDeenergizedEdgeState( VALVE_T valve ) +static VALVE_STATE_T handleValvesFindEnergizedEdgeState( VALVE_T valve ) { - VALVE_STATE_T nextState = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; + VALVE_STATE_T nextState = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; + S16 curPos = currentValveStates[ valve ].currentEncPosition; - if ( valve < NUM_OF_VALVES ) - { - S16 curPos = currentValveStates[ valve ].currentEncPosition; + checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_FIND_ENERGIZED_EDGE ); - // have we found forward edge of travel? - if ( fabs( currentValveStates[ valve ].priorEncPosition - curPos ) < VALVE_HOME_MIN_POS_CHG ) + // have we found forward edge of travel? + if ( abs( curPos - currentValveStates[ valve ].priorEncPosition ) < VALVE_HOME_MIN_POS_CHG ) + { + if ( 0 == currentValveStates[ valve ].valveOpsStartTime ) { - S16 posC = curPos + VALVE_HOME_BACK_OFF_EDGE; - S16 edgeDelta = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ] - posC; - U32 expDelta = abs( (S16)ROTARY_VALVE_FULL_SWING_TRAVEL_COUNTS - edgeDelta ); - - // verify edge to edge travel is reasonable - if ( expDelta <= MAX_HOME_FULL_TRAVEL_DIFF ) + currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); + } + else if ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, HOMING_EDGE_DETECTION_TIMEOUT_MS ) ) + { + //if ( VALVE_HOMING_INIT == currentValveStates[ valve ].valveHomingStatus ) + //{ + // S16 tgtPos = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; + // S16 mag = curPos - tgtPos; // calculate magnitude of position change + // + // // Set commanded position and transition start time stamp + // // Transition back to position C to try the next energized position. It has to start from position C + // currentValveStates[ valve ].commandedPosition = VALVE_POSITION_C_CLOSE; + // currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); + // currentValveStates[ valve ].valveHomingStatus = VALVE_HOMING_FORWARD; + // nextState = VALVE_STATE_IN_TRANSITION; + // // Command FPGA to move valve to commanded position + // setValveCmdChangePosition( valve, (U16)(mag), MOTOR_DIR_REVERSE ); + //} + //else { - // Record edge position - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ] = posC; - // calculate and record middle position - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_A_INSERT_EJECT ] = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ] + \ - ( ( currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ] - currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ] ) / 2 ); - // Set the current position to Position C and the commanded position to Position A - currentValveStates[ valve ].hasValveBeenHomed = TRUE; - currentValveStates[ valve ].currentPosition = VALVE_POSITION_C_CLOSE; - currentValveStates[ valve ].pendingCommandedPosition = VALVE_POSITION_A_INSERT_EJECT; - currentValveStates[ valve ].hasTransitionBeenRequested = TRUE; - currentValveStates[ valve ].hasHomingBeenRequested = FALSE; - currentValveStates[ valve ].numberOfFailedHomings = 0; - nextState = VALVE_STATE_IDLE; + S16 posC = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; + S16 posA = posC + ( ( curPos - posC ) / 2 ); + + // If position A was calculated to not be a division of 8 it is rounded down to be a division of 8 + posA = ( posA % ROTARY_VALVE_MICROSTEP_FRACTION ) < 4 ? ( posA >> 3 ) << 3 : posA; + + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ] = curPos; + currentValveStates[ valve ].positionsABC[ VALVE_POSITION_A_INSERT_EJECT ] = posA; + currentValveStates[ valve ].hasValveBeenHomed = TRUE; + currentValveStates[ valve ].currentPosition = VALVE_POSITION_B_OPEN; + currentValveStates[ valve ].pendingCommandedPosition = VALVE_POSITION_A_INSERT_EJECT; + currentValveStates[ valve ].valveHomingStatus = VALVE_HOMING_COMPLETE; + currentValveStates[ valve ].hasTransitionBeenRequested = TRUE; + currentValveStates[ valve ].hasHomingBeenRequested = FALSE; + nextState = VALVE_STATE_IDLE; } - // if home failed, attempt retry or fault if out of retries - else - { - currentValveStates[ valve ].numberOfFailedHomings++; - if ( currentValveStates[ valve ].numberOfFailedHomings > MAX_ALLOWED_FAILED_VALVE_HOMINGS ) - { - currentValveStates[ valve ].hasValveBeenHomed = FALSE; - currentValveStates[ valve ].hasHomingFailed = TRUE; - currentValveStates[ valve ].hasHomingBeenRequested = FALSE; - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_VALVE_HOMING_FAILED, (U32)valve, (U32)edgeDelta ); - } - nextState = VALVE_STATE_HOMING_NOT_STARTED; - } } } - else + else if ( curPos <= 0 ) { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE5, (U32)valve ) + switch ( currentValveStates[ valve ].valveHomingStatus ) + { + case VALVE_HOMING_INIT: + case VALVE_HOMING_FORWARD: + currentValveStates[ valve ].valveHomingStatus = VALVE_HOMING_REVERSE; + break; + + default: + // If init or forward modes, then reverse again. + break; + } + + currentValveStates[ valve ].hasValveBeenReset = FALSE; + nextState = VALVE_STATE_RESET_VALVE; } return nextState; @@ -468,7 +569,6 @@ * @brief * The handleValvesIdleState function handles the Idle state of the state * machine for a given valve. - * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given valve is invalid. * @details \b Inputs: currentValveStates[] * @details \b Outputs: currentValveStates[], cmd to FPGA * @param valve ID of valve for which to handle the Idle state @@ -478,37 +578,51 @@ { VALVE_STATE_T nextState = VALVE_STATE_IDLE; - if ( valve < NUM_OF_VALVES ) + checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_IDLE ); + + // handle a home request + if ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) { - // handle a home request - if ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) + if ( VALVE_HOMING_COMPLETE == currentValveStates[ valve ].valveHomingStatus ) { - nextState = VALVE_STATE_HOMING_NOT_STARTED; + S16 tgtPos = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; + S16 mag = currentValveStates[ valve ].currentEncPosition - tgtPos; // calculate magnitude of position change + + // Set commanded position and transition start time stamp + // Transition back to position C to try the next energized position. It has to start from position C + currentValveStates[ valve ].commandedPosition = VALVE_POSITION_C_CLOSE; + currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); + currentValveStates[ valve ].valveHomingStatus = VALVE_HOMING_INIT; + currentValveStates[ valve ].hasValveBeenReset = FALSE; + nextState = VALVE_STATE_IN_TRANSITION; + // Command FPGA to move valve to commanded position + setValveCmdChangePosition( valve, (U16)(mag), MOTOR_DIR_REVERSE ); } - // handle a position change request - else if ( TRUE == currentValveStates[ valve ].hasTransitionBeenRequested ) + else if ( VALVE_HOMING_INIT == currentValveStates[ valve ].valveHomingStatus ) { - currentValveStates[ valve ].hasTransitionBeenRequested = FALSE; - if ( currentValveStates[ valve ].currentPosition != currentValveStates[ valve ].pendingCommandedPosition ) - { - S16 curPos = currentValveStates[ valve ].currentEncPosition; - S16 tgtPos = currentValveStates[ valve ].positionsABC[ currentValveStates[ valve ].pendingCommandedPosition ]; - S16 mag = tgtPos - curPos; // calculate magnitude of position change - MOTOR_DIR_T dir = ( mag < 0 ? MOTOR_DIR_REVERSE : MOTOR_DIR_FORWARD ); // determine direction of position change - - // set commanded position and transition start time stamp - currentValveStates[ valve ].commandedPosition = currentValveStates[ valve ].pendingCommandedPosition; - currentValveStates[ valve ].transitionStartTime = getMSTimerCount(); - // command FPGA to move valve to commanded position - setValveCmdChangePosition( valve, (U16)( abs(mag) ), dir ); - // go on to transition state - nextState = VALVE_STATE_IN_TRANSITION; - } + currentValveStates[ valve ].hasValveBeenReset = FALSE; + nextState = VALVE_STATE_HOMING_NOT_STARTED; } } - else + // handle a position change request + else if ( TRUE == currentValveStates[ valve ].hasTransitionBeenRequested ) { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE6, (U32)valve ) + currentValveStates[ valve ].hasTransitionBeenRequested = FALSE; + if ( currentValveStates[ valve ].currentPosition != currentValveStates[ valve ].pendingCommandedPosition ) + { + S16 curPos = currentValveStates[ valve ].currentEncPosition; + S16 tgtPos = currentValveStates[ valve ].positionsABC[ currentValveStates[ valve ].pendingCommandedPosition ]; + S16 mag = tgtPos - curPos; // calculate magnitude of position change + MOTOR_DIR_T dir = ( mag < 0 ? MOTOR_DIR_REVERSE : MOTOR_DIR_FORWARD ); // determine direction of position change + + // Set commanded position and transition start time stamp + currentValveStates[ valve ].commandedPosition = currentValveStates[ valve ].pendingCommandedPosition; + currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); + nextState = VALVE_STATE_IN_TRANSITION; + + // Command FPGA to move valve to commanded position + setValveCmdChangePosition( valve, (U16)( abs(mag) ), dir ); + } } return nextState; @@ -518,7 +632,6 @@ * @brief * The handleValvesTransitionState function handles the Transition state * of the state machine for a given valve. - * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given valve is invalid. * @details \b Inputs: currentValveStates[] * @details \b Outputs: currentValveStates[] * @param valve ID of valve for which to handle the Transition state @@ -527,39 +640,33 @@ static VALVE_STATE_T handleValvesTransitionState( VALVE_T valve ) { VALVE_STATE_T nextState = VALVE_STATE_IN_TRANSITION; + S16 curPos = currentValveStates[ valve ].currentEncPosition; + S16 tgtPos = currentValveStates[ valve ].positionsABC[ currentValveStates[ valve ].commandedPosition ]; + S16 delta = tgtPos - curPos; - if ( valve < NUM_OF_VALVES ) + checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_IN_TRANSITION ); + + // have we completed the transition? + if ( abs( delta ) < VALVE_TRANSITION_MIN_TGT_DELTA ) { - S16 curPos = currentValveStates[ valve ].currentEncPosition; - S16 tgtPos = currentValveStates[ valve ].positionsABC[ currentValveStates[ valve ].commandedPosition ]; - S16 delta = tgtPos - curPos; + currentValveStates[ valve ].currentPosition = currentValveStates[ valve ].commandedPosition; + nextState = ( VALVE_HOMING_COMPLETE != currentValveStates[ valve ].valveHomingStatus ? VALVE_STATE_HOMING_NOT_STARTED : VALVE_STATE_IDLE ); + } + // is transition taking too long? + else if ( ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, VALVE_TRANSITION_TIMEOUT_MS ) ) && + ( getCPLDACPowerLossDetected() != TRUE ) ) + { + // Go back to Idle state and set the valve position to not in position because it was not able to get to the target position + currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; + nextState = VALVE_STATE_IDLE; + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_VALVE_TRANSITION_TIMEOUT, (U32)valve, (U32)abs( delta ) ); - // have we completed the transition? - if ( abs( delta ) < VALVE_TRANSITION_MIN_TGT_DELTA ) + // If the valve's commanded position is C, activate safety shutdown + if ( VALVE_POSITION_C_CLOSE == currentValveStates[ valve ].commandedPosition ) { - currentValveStates[ valve ].currentPosition = currentValveStates[ valve ].commandedPosition; - nextState = VALVE_STATE_IDLE; + activateSafetyShutdown(); } - // is transition taking too long? - else if ( ( TRUE == didTimeout( currentValveStates[ valve ].transitionStartTime, VALVE_TRANSITION_TIMEOUT_MS ) ) && - ( getCPLDACPowerLossDetected() != TRUE ) ) - { - // Go back to Idle state and set the valve position to not in position because it was not able to get to the target position - currentValveStates[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; - nextState = VALVE_STATE_IDLE; - SET_ALARM_WITH_1_U32_DATA( ALARM_ID_TD_VALVE_TRANSITION_TIMEOUT, (U32)valve ); - - // If the valve's commanded position is C, activate safety shutdown - if ( VALVE_POSITION_C_CLOSE == currentValveStates[ valve ].commandedPosition ) - { - activateSafetyShutdown(); - } - } } - else - { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE7, (U32)valve ) - } return nextState; } @@ -591,6 +698,25 @@ /*********************************************************************//** * @brief + * The checkValveInRange function checks whether the selected valve is in range + * and if not it will trigger the provided software fault. + * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given valve is invalid. + * @details \b Inputs: none + * @details \b Outputs: none + * @param valve the valve check whether it is in range + * @param SWFault the provided software fault to trigger + * @return none + *************************************************************************/ +static void checkValveInRange( VALVE_T valve, SW_FAULT_ID_T SWFault ) +{ + if ( valve >= NUM_OF_VALVES ) + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, (U32)SWFault, (U32)valve ) + } +} + +/*********************************************************************//** + * @brief * The publishValvesData function constructs and sends the valves data * broadcast message. * @details \b Message \b Sent: MSG_ID_TD_VALVES_DATA @@ -612,7 +738,11 @@ data.state = (U32)currentValveStates[ valve ].controlState; data.currentPosID = (U32)currentValveStates[ valve ].currentPosition; data.currentPos = getValveEncoderPosition( valve ); - data.cmdPos = (U32)currentValveStates[ valve ].commandedPosition; + data.cmdPos = currentValveStates[ valve ].commandedPosition; + data.posA = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_A_INSERT_EJECT ]; + data.posB = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_B_OPEN ]; + data.posC = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; + data.posD = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_D_PARTIAL_CLOSE ]; broadcastData( MSG_ID_TD_VALVES_DATA, COMM_BUFFER_OUT_CAN_TD_BROADCAST, (U08*)&data, sizeof( TD_VALVE_DATA_T ) ); }