Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -r4f4050a451d88043f170814d9929ff497dc43030 -rdbcbd145954ef52510ac929a412e0dba42f0941b --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision 4f4050a451d88043f170814d9929ff497dc43030) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision dbcbd145954ef52510ac929a412e0dba42f0941b) @@ -65,7 +65,7 @@ #define INITIAL_ENERGIZED_EDGE_UPPER_RANGE 13000U ///< Energized edge maximum count #define INITIAL_ENERGIZED_EDGE_LOWER_RANGE 9000U ///< Energized edge minimum count -#define AIR_TRAP_VALVE_GPIO_PIN 0x12 ///< Air trap valve GPIO pin number +#define AIR_TRAP_VALVE_GPIO_PIN 0x12 ///< Air trap valve GPIO Pin 18 of HET Port 1 #define HOMING_STEP_CHANGE_IN_COUNTS 1000 ///< Step change in counts during homing #define TRANSITION_STEP_CHANGE_IN_COUNTS 15000 ///< Step change in counts during normal operations @@ -86,8 +86,6 @@ #define VALVE_CCW_PWM_TO_CNT_CONVERSION( pwm ) ( ( -20 * pwm ) + 2500 ) ///< Valve counter clockwise PWM to count conversion #define VALVE_CCW_CNT_TO_PWM_CONVERSION( cnt ) ( ( -0.05 * cnt ) + 125 ) ///< Valve counter clockwise count to PWM conversion -// ********** private data ********** - /// Exec valve self test states typedef enum valves_Self_Test_States { @@ -132,8 +130,8 @@ /// Valves bypass mode direction typedef enum valve_bypass_mode_direction { - VAVLE_DIR_CLOCKWISE = 0, ///< Clockwise direction in bypass mode - VALVE_DIR_COUNTERCLOCKWISE, ///< Counter clockwise direction in bypass mode + VAVLE_DIR_CLOCKWISE = 0, ///< Clockwise direction in bypass mode to moves the valves towards energized edge + VALVE_DIR_COUNTERCLOCKWISE, ///< Counter clockwise direction in bypass mode to move the valves towards de-energized edge NUM_OF_VALVE_DIRECTION ///< Number of valve directions } VALVE_DIRECTION_T; @@ -164,7 +162,6 @@ U32 overCurrentCounter; ///< Valve over current counter U32 positionOutOfRangeCounter; ///< Valve position out of range counter U32 dataPublishCounter; ///< Valve data publish counter - U32 restCounter; //TODO REMOVE? // Homing variables U32 numberOfFailedHomings; ///< Valve number of failed homing U32 homingEdgeDetectionCounter; ///< Valve homing edge detection counter @@ -176,11 +173,14 @@ } VALVE_STATUS_T; #pragma pack(pop) -static VALVE_SELF_TEST_STATE_T valveSelfTestState = VALVE_SELF_TEST_ENABLE_VALVES; ///< Valves set test state -static SELF_TEST_STATUS_T valvesSelfTestResult = SELF_TEST_STATUS_IN_PROGRESS; ///< Valves set test result +// ********** private data ********** -static VALVE_STATUS_T valvesStatus[ NUM_OF_VALVES ]; ///< Valves status +static VALVE_SELF_TEST_STATE_T valveSelfTestState = VALVE_SELF_TEST_ENABLE_VALVES; ///< Valves self test state +static SELF_TEST_STATUS_T valvesSelfTestResult = SELF_TEST_STATUS_IN_PROGRESS; ///< Valves self test result +/// Array of type VALVE_STATUS_T structure that holds the status of each valve +static VALVE_STATUS_T valvesStatus[ NUM_OF_VALVES ]; ///< Valves status array + static OVERRIDE_U32_T valvesDataPublishInterval = { VALVES_DATA_PUB_INTERVAL, VALVES_DATA_PUB_INTERVAL, 0, 0 }; ///< Valves data publish interval static OVERRIDE_U32_T valvesPositionOverride[ NUM_OF_VALVES ] = { VALVE_POSITION_C_CLOSE, VALVE_POSITION_C_CLOSE, 0, 0 }; ///< Valves position override @@ -201,6 +201,7 @@ { VBV_INIT_STATUS_BIT_MASK, VBV_ENABLE_PID_STATUS_BIT_MASK, VBV_ENABLE_BYPASS_STATUS_BIT_MASK, VALVES_DISABLED_BIT_MASK } }; ///< Valves control status bits static U16 valvesControlSetBits = 0x0000; ///< Valves control set bit +static OPN_CLS_STATE_T valveAirTrapStatus; ///< Air trap valve status (open/close) // Self test function prototypes static VALVE_SELF_TEST_STATE_T handleValveSelfTestEnableValves( void ); @@ -246,10 +247,12 @@ VALVE_T valve; valveSelfTestState = VALVE_SELF_TEST_ENABLE_VALVES; valvesSelfTestResult = SELF_TEST_STATUS_IN_PROGRESS; + valveAirTrapStatus = STATE_CLOSED; // Set the valves bit control to 0 valvesControlSetBits = 0x0000; setFPGAValvesControlMode( valvesControlSetBits ); + setValveAirTrap( STATE_CLOSED ); // Initialize some of the variables for ( valve = VDI; valve < NUM_OF_VALVES; valve++ ) @@ -318,33 +321,64 @@ *************************************************************************/ VALVE_POSITION_T getValvePosition( VALVE_T valve ) { - return valvesStatus[ valve ].currentPosition; + VALVE_POSITION_T position = VALVE_POSITION_NOT_IN_POSITION; + + if ( valve < NUM_OF_VALVES ) + { + position = valvesStatus[ valve ].currentPosition; + } + + return position; } /*********************************************************************//** * @brief * The setValveBloodTrap function set the blood trap valve to open or close * position. * @details - * Inputs: none - * Outputs: none + * Inputs: valveAirTrapStatus + * Outputs: valveAirTrapStatus * @param: state enumeration of open or close for valve * @return: none *************************************************************************/ void setValveAirTrap( OPN_CLS_STATE_T state ) { BOOL energized = FALSE; - if ( STATE_OPEN == state ) + // Check if the proper air trap state was requested + if ( state < NUM_OF_OPN_CLS_STATES ) { - energized = TRUE; + if ( STATE_OPEN == state ) + { + energized = TRUE; + } } + // Of course if invalid air trap state was requested, keep the valve de-energized (close) + else + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, ALARM_ID_HD_VALVE_INVALID_AIR_TRAP_REQUEST, state ); + } + valveAirTrapStatus = state; gioSetBit( hetPORT1, AIR_TRAP_VALVE_GPIO_PIN, energized ); } /*********************************************************************//** * @brief + * The getValveAirTrapStatus function returns the status of air trap valve + * which is either open or close + * @details + * Inputs: valveAirTrapStatus + * Outputs: none + * @return: valveAirTrapStatus the status of air trap valve (open or close) + *************************************************************************/ +OPN_CLS_STATE_T getValveAirTrapStatus( void ) +{ + return valveAirTrapStatus; +} + +/*********************************************************************//** + * @brief * The execValvesSelfTest function executes the valves self test. * @details * Inputs: valveSelfTestState @@ -387,57 +421,56 @@ void execValves( void ) { VALVE_T valve; - VALVE_STATE_T state; - execMonitorValves(); + // Monitoring should be done once POST is complete and the valves are + // set correctly + if ( valveSelfTestState == VALVE_SELF_TEST_COMPLETE ) + { + execMonitorValves(); + } + // Loop through all of the valves for ( valve = VDI; valve < NUM_OF_VALVES; valve++ ) { - //TODO FOR TESTING ONLY REMOVE - //valve = VBA; - //TODO FOR TESTING ONLY REMOVE - switch ( valvesStatus[ valve ].execState ) { case VALVE_STATE_WAIT_FOR_POST: - state = handleValveStateWaitForPost( valve ); + valvesStatus[ valve ].execState = handleValveStateWaitForPost( valve ); break; case VALVE_STATE_HOMING_NOT_STARTED: - state = handleValveStateHomingNotStarted( valve ); + valvesStatus[ valve ].execState = handleValveStateHomingNotStarted( valve ); break; case VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE: - state = handleValveStateHomingFindEnergizedEdge( valve ); + valvesStatus[ valve ].execState = handleValveStateHomingFindEnergizedEdge( valve ); break; case VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE: - state = handleValveStateHomingFindDeenergizedEdge( valve ); + valvesStatus[ valve ].execState = handleValveStateHomingFindDeenergizedEdge( valve ); break; case VALVE_STATE_IDLE: - state = handleValveStateIdle( valve ); + valvesStatus[ valve ].execState = handleValveStateIdle( valve ); break; case VALVE_STATE_IN_TRANSITION: - state = handleValveStateInTransition( valve ); + valvesStatus[ valve ].execState = handleValveStateInTransition( valve ); break; // PWM mode only used in debug mode #ifdef DEBUG_ENABLED case VALVE_STATE_IN_BYPASS_MODE: - state = handleValveStateInBypassMode( valve ); + valvesStatus[ valve ].execState = handleValveStateInBypassMode( valve ); break; #endif default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_HD_VALVES_INVALID_EXEC_STATE, valvesStatus[ valve ].execState ); - state = VALVE_STATE_IDLE; + valvesStatus[ valve ].execState = VALVE_STATE_IDLE; break; } - valvesStatus[ valve ].execState = state; - publishValvesData( valve ); } } @@ -482,7 +515,6 @@ { VALVE_SELF_TEST_STATE_T state = VALVE_SELF_TEST_COMPLETE; - // TODO do we need to wait for 50ms? if ( areValvesFunctional() ) { valvesSelfTestResult = SELF_TEST_STATUS_PASSED; @@ -536,7 +568,7 @@ // If homing has been requested or POST is completed and the door has been close for the specified // period of time, start the homing - if ( ( valvesStatus[ valve ].hasHomingBeenRequested || valveSelfTestState == VALVE_SELF_TEST_COMPLETE ) && + if ( valveSelfTestState == VALVE_SELF_TEST_COMPLETE && valvesStatus[ valve ].hasHomingBeenRequested || // This is disabled for VectoCAST testing. Once the door // driver was implemented, this build switch must be removed #ifdef DEBUG_ENABLED @@ -574,8 +606,6 @@ S16 targetPosition = valvesStatus[ valve ].targetPositionInCounts; S16 deltaPosition = targetPosition - currentPosition; - valvesStatus[ valve ].homingEdgeDetectionCounter++; - // If there has not been any major travel for a certain period of time if ( deltaPosition > MAX_DEVIATION_FROM_TARGET_IN_COUNTS && valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TIME_INTERVAL ) @@ -588,16 +618,18 @@ state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; } + // If there has not been a major travel but the time has not elapsed yet + else if ( deltaPosition > MAX_DEVIATION_FROM_TARGET_IN_COUNTS && + valvesStatus[ valve ].homingEdgeDetectionCounter < HOMING_EDGE_DETECTION_TIME_INTERVAL ) + { + valvesStatus[ valve ].homingEdgeDetectionCounter++; + } + // Reached to target, go for the next target else if ( deltaPosition < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) { -#ifdef DEBUG_ENABLED // To remove this if statement from VectorCAST - if ( valvesStatus[ valve ].restCounter >= 0 )//TODO add a #define for the rest time. Do we need the rest? -#endif - { - valvesStatus[ valve ].homingEdgeDetectionCounter = 0; - valvesStatus[ valve ].targetPositionInCounts = currentPosition + HOMING_STEP_CHANGE_IN_COUNTS; - setFPGAValveSetPoint( valve, valvesStatus[ valve ].targetPositionInCounts, FALSE ); - } + valvesStatus[ valve ].homingEdgeDetectionCounter = 0; + valvesStatus[ valve ].targetPositionInCounts = currentPosition + HOMING_STEP_CHANGE_IN_COUNTS; + setFPGAValveSetPoint( valve, valvesStatus[ valve ].targetPositionInCounts, FALSE ); } return state; @@ -619,11 +651,10 @@ S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts; S16 targetPosition = valvesStatus[ valve ].targetPositionInCounts; - S16 deltaPosition = targetPosition - currentPosition; + S16 deltaPosition = currentPosition - targetPosition; - valvesStatus[ valve ].homingEdgeDetectionCounter++; - - if ( abs(deltaPosition) > MAX_DEVIATION_FROM_TARGET_IN_COUNTS && + // If there has not been any major travel for a certain period of time + if ( deltaPosition > MAX_DEVIATION_FROM_TARGET_IN_COUNTS && valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TIME_INTERVAL ) { S16 energizedEdge = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; @@ -632,6 +663,7 @@ if ( deltaEdges <= INITIAL_ENERGIZED_EDGE_UPPER_RANGE && deltaEdges >= INITIAL_ENERGIZED_EDGE_LOWER_RANGE ) { S16 positionB = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; + // Positions B and C will have an offset from the edge to make sure each time the valve will not hit the edge valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] = positionB - INITIAL_EDGE_OFFSET_READ_COUNT; valvesStatus[ valve ].positions[ VALVE_POSITION_C_CLOSE ] = currentPosition + INITIAL_EDGE_OFFSET_READ_COUNT; @@ -663,19 +695,20 @@ valvesStatus[ valve ].numberOfFailedHomings++; state = VALVE_STATE_HOMING_NOT_STARTED; } - } - else if ( abs(deltaPosition) < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) + // If there has not been any major travel but the time has not elapsed yet. + else if ( deltaPosition > MAX_DEVIATION_FROM_TARGET_IN_COUNTS && + valvesStatus[ valve ].homingEdgeDetectionCounter < HOMING_EDGE_DETECTION_TIME_INTERVAL ) { -#ifdef DEBUG_ENABLED //To remove this if statement from VecotrCAST - if ( valvesStatus[ valve ].restCounter >= 0 ) //TODO add a #define for the rest time. Do we need the rest? -#endif - { - valvesStatus[ valve ].homingEdgeDetectionCounter = 0; - valvesStatus[ valve ].targetPositionInCounts = currentPosition - HOMING_STEP_CHANGE_IN_COUNTS; - setFPGAValveSetPoint( valve, valvesStatus[ valve ].targetPositionInCounts, FALSE ); - } + valvesStatus[ valve ].homingEdgeDetectionCounter++; } + // Reached to target, go for the next target + else if ( deltaPosition < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) + { + valvesStatus[ valve ].homingEdgeDetectionCounter = 0; + valvesStatus[ valve ].targetPositionInCounts = currentPosition - HOMING_STEP_CHANGE_IN_COUNTS; + setFPGAValveSetPoint( valve, valvesStatus[ valve ].targetPositionInCounts, FALSE ); + } return state; } @@ -698,8 +731,7 @@ { state = VALVE_STATE_HOMING_NOT_STARTED; } - else if ( valvesStatus[ valve ].hasValveBeenHomed && - valvesStatus[ valve ].hasTransitionBeenRequested ) + else if ( valvesStatus[ valve ].hasValveBeenHomed && valvesStatus[ valve ].hasTransitionBeenRequested ) { if ( valvesStatus[ valve ].currentPosition != valvesStatus[ valve ].commandedPosition ) { @@ -754,7 +786,8 @@ S16 currentPosition = valvesStatus[ valve ].currentPositionInCounts; S16 targetPosition = valvesStatus[ valve ].targetPositionInCounts; - // Check if the valve is within range of the commanded position + // Check if the valve is within range of the final commanded position. + // NOTE: This is checked to the final position and not the temporary target en route to the final destination if ( abs( currentPosition - commandedPosition ) < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) { // The valve is in range. Set the current position to the commanded position @@ -779,6 +812,7 @@ state = VALVE_STATE_IDLE; SET_ALARM_WITH_1_U32_DATA( ALARM_ID_HD_VALVE_TRANSITION_TIMEOUT, (U32)valve ); } + // Check if the valve is close to the temporary target position and if it is, assign the next target position else if ( abs( currentPosition - targetPosition ) < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) { setValveNextStep( valve, TRANSITION_STEP_CHANGE_IN_COUNTS ); @@ -1137,6 +1171,7 @@ valveData.posA = valvesStatus[ valve ].positions[ VALVE_POSITION_A_INSERT_EJECT ]; valveData.posB = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; valveData.pwm = valvesStatus[ valve ].bypassModeStatus.currentPWMInPercent; + valveData.airTrapValve = (U32)getValveAirTrapStatus(); broadcastHDValves( &valveData ); @@ -1256,7 +1291,7 @@ break; default: - // TODO alarm or ignore? + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_HD_VALVES_INVALID_VALVE_SELECTED, (U32)valve ); break; } }