Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -rd9b5f588d81e15ed3849222bed3362e15dbf4b0a -r8f913069e28f0d1913ede0a346ee1525986650d1 --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision d9b5f588d81e15ed3849222bed3362e15dbf4b0a) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 8f913069e28f0d1913ede0a346ee1525986650d1) @@ -16,6 +16,7 @@ ***************************************************************************/ #include "AlarmMgmtTD.h" +#include "CpldInterface.h" #include "Messaging.h" #include "OperationModes.h" #include "PersistentAlarm.h" @@ -32,10 +33,12 @@ #define VALVES_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) ///< Valves data publish interval. #define DATA_PUBLISH_COUNTER_START_COUNT 13 ///< Valves data publish start counter. +#define VALVE_TRANSITION_MIN_TGT_DELTA 10 ///< Minimum encoder position delta from target position to end transition state. #define VALVE_HOME_MIN_POS_CHG 10 ///< Minimum encoder position change to indicate a home operation is still moving toward edge. #define VALVE_HOME_BACK_OFF_EDGE 10 ///< 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 100U ///< 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 /// Valve controller states typedef enum valve_Control_States @@ -143,7 +146,8 @@ if ( ( valve < NUM_OF_VALVES ) && ( position < NUM_OF_VALVE_POSITIONS ) ) { - currentValveStates[ valve ].commandedPosition = position; + currentValveStates[ valve ].pendingCommandedPosition = position; + currentValveStates[ valve ].hasTransitionBeenRequested = TRUE; result = TRUE; } else @@ -192,11 +196,16 @@ { VALVE_T valve; - // update valve readings + // get latest valve readings from FPGA readValves(); for ( valve = FIRST_VALVE; valve < NUM_OF_VALVES; valve++ ) { + // update valve encoder positions + currentValveStates[ valve ].priorEncPosition = currentValveStates[ valve ].currentEncPosition; + currentValveStates[ valve ].currentEncPosition = getValveEncoderPosition( valve ); + + // execute valve state machine switch( currentValveStates[ valve ].controlState ) { case VALVE_STATE_WAIT_FOR_POST: @@ -408,8 +417,8 @@ * 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: - * @details \b Outputs: + * @details \b Inputs: currentValveStates[] + * @details \b Outputs: currentValveStates[], cmd to FPGA * @param valve ID of valve for which to handle the Idle state * @return next state of the state machine for the given valve *************************************************************************/ @@ -427,7 +436,22 @@ // handle a position change request else if ( TRUE == currentValveStates[ valve ].hasTransitionBeenRequested ) { + 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; + } } } else @@ -443,8 +467,8 @@ * 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: - * @details \b Outputs: + * @details \b Inputs: currentValveStates[] + * @details \b Outputs: currentValveStates[] * @param valve ID of valve for which to handle the Transition state * @return next state of the state machine for the given valve *************************************************************************/ @@ -454,7 +478,31 @@ if ( valve < NUM_OF_VALVES ) { + S16 curPos = currentValveStates[ valve ].currentEncPosition; + S16 tgtPos = currentValveStates[ valve ].positionsABC[ currentValveStates[ valve ].commandedPosition ]; + S16 delta = tgtPos - curPos; + // have we completed the transition? + if ( abs( delta ) < VALVE_TRANSITION_MIN_TGT_DELTA ) + { + currentValveStates[ valve ].currentPosition = currentValveStates[ valve ].commandedPosition; + nextState = VALVE_STATE_IDLE; + } + // 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; + // TODO 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 {