Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -rdbcbd145954ef52510ac929a412e0dba42f0941b -ra42f849450c65801f2d45ca9085af81a1f11269d --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision dbcbd145954ef52510ac929a412e0dba42f0941b) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision a42f849450c65801f2d45ca9085af81a1f11269d) @@ -179,26 +179,26 @@ 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 VALVE_STATUS_T valvesStatus[ NUM_OF_VALVES ]; 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 static const U16 valvesControlModesSetBits[ NUM_OF_VALVES ][ NUM_OF_VALVE_CONTROL_MODES ] = - { { VDI_SET_PID_BIT_MASK, VDI_SET_BYPASS_BIT_MASK, VALVES_DISABLE_BIT_MASK }, - { VDO_SET_PID_BIT_MASK, VDO_SET_BYPASS_BIT_MASK, VALVES_DISABLE_BIT_MASK }, - { VBA_SET_PID_BIT_MASK, VBA_SET_BYPASS_BIT_MASK, VALVES_DISABLE_BIT_MASK }, - { VBV_SET_PID_BIT_MASK, VBV_SET_BYPASS_BIT_MASK, VALVES_DISABLE_BIT_MASK } }; ///< Valves control mode set bits + { { VDI_SET_PID_BIT_MASK, VDI_SET_BYPASS_BIT_MASK, VDI_RESET_CONTROL_BIT_MASK }, + { VDO_SET_PID_BIT_MASK, VDO_SET_BYPASS_BIT_MASK, VDO_RESET_CONTROL_BIT_MASK }, + { VBA_SET_PID_BIT_MASK, VBA_SET_BYPASS_BIT_MASK, VBA_RESET_CONTROL_BIT_MASK }, + { VBV_SET_PID_BIT_MASK, VBV_SET_BYPASS_BIT_MASK, VBV_RESET_CONTROL_BIT_MASK } }; ///< Valves control mode set bits static const U16 valvesControlModesResetBits[ NUM_OF_VALVES ] = { VDI_RESET_CONTROL_BIT_MASK, VDO_RESET_CONTROL_BIT_MASK, VBA_RESET_CONTROL_BIT_MASK, VBV_RESET_CONTROL_BIT_MASK }; ///< Valves control modes rest bits static const U16 valvesControlStatusBits[ NUM_OF_VALVES ][ NUM_OF_VALVE_CONTROL_STATUS ] = - { { VDI_INIT_STATUS_BIT_MASK, VDI_ENABLE_PID_STATUS_BIT_MASK, VDI_ENABLE_BYPASS_STATUS_BIT_MASK, VALVES_DISABLED_BIT_MASK }, - { VDO_INIT_STATUS_BIT_MASK, VDO_ENABLE_PID_STATUS_BIT_MASK, VDO_ENABLE_BYPASS_STATUS_BIT_MASK, VALVES_DISABLED_BIT_MASK }, - { VBA_INIT_STATUS_BIT_MASK, VBA_ENABLE_PID_STATUS_BIT_MASK, VBA_ENABLE_BYPASS_STATUS_BIT_MASK, VALVES_DISABLED_BIT_MASK }, - { 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 + { { VDI_INIT_STATUS_BIT_MASK, VDI_ENABLE_PID_STATUS_BIT_MASK, VDI_ENABLE_BYPASS_STATUS_BIT_MASK, VDI_RESET_CONTROL_BIT_MASK }, + { VDO_INIT_STATUS_BIT_MASK, VDO_ENABLE_PID_STATUS_BIT_MASK, VDO_ENABLE_BYPASS_STATUS_BIT_MASK, VDO_RESET_CONTROL_BIT_MASK }, + { VBA_INIT_STATUS_BIT_MASK, VBA_ENABLE_PID_STATUS_BIT_MASK, VBA_ENABLE_BYPASS_STATUS_BIT_MASK, VBA_RESET_CONTROL_BIT_MASK }, + { VBV_INIT_STATUS_BIT_MASK, VBV_ENABLE_PID_STATUS_BIT_MASK, VBV_ENABLE_BYPASS_STATUS_BIT_MASK, VBV_RESET_CONTROL_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) @@ -245,13 +245,15 @@ void initValves( void ) { 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 ); + // Set the air trap valve to be in closed state setValveAirTrap( STATE_CLOSED ); // Initialize some of the variables @@ -279,6 +281,10 @@ valvesStatus[ valve ].hasHomingBeenRequested = TRUE; result = TRUE; } + else + { + SET_ALARM_WITH_1_U32_DATA( ALARM_ID_INVALID_VALVE_SELECTED, (U32)valve ); + } return result; } @@ -306,6 +312,10 @@ result = TRUE; } + else + { + SET_ALARM_WITH_1_U32_DATA( ALARM_ID_INVALID_VALVE_SELECTED, (U32)valve ); + } return result; } @@ -327,6 +337,10 @@ { position = valvesStatus[ valve ].currentPosition; } + else + { + SET_ALARM_WITH_1_U32_DATA( ALARM_ID_INVALID_VALVE_SELECTED, (U32)valve ); + } return position; } @@ -356,7 +370,7 @@ // 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 ); + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_HD_VALVES_INVALID_AIR_TRAP_REQUEST, state ); } valveAirTrapStatus = state; @@ -568,13 +582,8 @@ // 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 ( 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 - isDoorClosed && -#endif - ! valvesStatus[ valve ].hasHomingFailed ) + if ( valveSelfTestState == VALVE_SELF_TEST_COMPLETE && ( valvesStatus[ valve ].hasHomingBeenRequested || isDoorClosed ) + && ( ! valvesStatus[ valve ].hasHomingFailed ) ) { // Get ready for the energized state valvesStatus[ valve ].homingEdgeDetectionCounter = 0; @@ -607,25 +616,26 @@ S16 deltaPosition = targetPosition - currentPosition; // 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 ) + if ( deltaPosition > MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) { - // Current position (positive or negative) will be stored in the Position B of the current valve - valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] = currentPosition; - valvesStatus[ valve ].homingEdgeDetectionCounter = 0; - valvesStatus[ valve ].targetPositionInCounts = currentPosition - HOMING_STEP_CHANGE_IN_COUNTS; - setFPGAValveSetPoint( valve, valvesStatus[ valve ].targetPositionInCounts, FALSE ); + if ( valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TIME_INTERVAL ) + { + // Current position (positive or negative) will be stored in the Position B of the current valve + valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] = currentPosition; + valvesStatus[ valve ].homingEdgeDetectionCounter = 0; + valvesStatus[ valve ].targetPositionInCounts = currentPosition - HOMING_STEP_CHANGE_IN_COUNTS; + setFPGAValveSetPoint( valve, valvesStatus[ valve ].targetPositionInCounts, FALSE ); - state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; + state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; + } + else + { + valvesStatus[ valve ].homingEdgeDetectionCounter++; + } + } - // 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 ) + else { valvesStatus[ valve ].homingEdgeDetectionCounter = 0; valvesStatus[ valve ].targetPositionInCounts = currentPosition + HOMING_STEP_CHANGE_IN_COUNTS; @@ -653,57 +663,59 @@ S16 targetPosition = valvesStatus[ valve ].targetPositionInCounts; S16 deltaPosition = currentPosition - targetPosition; - // 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 ) + // If there has not been any major travel + if ( deltaPosition > MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) { - S16 energizedEdge = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; - S16 deltaEdges = energizedEdge - currentPosition; - - if ( deltaEdges <= INITIAL_ENERGIZED_EDGE_UPPER_RANGE && deltaEdges >= INITIAL_ENERGIZED_EDGE_LOWER_RANGE ) + // Check if the specified time has elapsed + if ( valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TIME_INTERVAL ) { - S16 positionB = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; + S16 energizedEdge = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; + S16 deltaEdges = energizedEdge - currentPosition; - // 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; - // Position A is the average of the Position B that was read last time and position C that was the target of this state - valvesStatus[ valve ].positions[ VALVE_POSITION_A_INSERT_EJECT ] = ( valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] - - valvesStatus[ valve ].positions[ VALVE_POSITION_C_CLOSE ] ) / 2; + // The range from energized to de-energized should be in between 9000 to 13000 steps + if ( deltaEdges <= INITIAL_ENERGIZED_EDGE_UPPER_RANGE && deltaEdges >= INITIAL_ENERGIZED_EDGE_LOWER_RANGE ) + { + S16 positionB = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; - // Set the current position to Position C and the commanded position to Position A - valvesStatus[ valve ].hasValveBeenHomed = TRUE; - valvesStatus[ valve ].currentPosition = VALVE_POSITION_C_CLOSE; - valvesStatus[ valve ].commandedPosition = VALVE_POSITION_A_INSERT_EJECT; - valvesStatus[ valve ].hasTransitionBeenRequested = TRUE; - valvesStatus[ valve ].hasHomingBeenRequested = FALSE; - valvesStatus[ valve ].numberOfFailedHomings = 0; + // 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; + // Position A is the average of the Position B that was read last time and position C that was the target of this state + valvesStatus[ valve ].positions[ VALVE_POSITION_A_INSERT_EJECT ] = ( valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] - + valvesStatus[ valve ].positions[ VALVE_POSITION_C_CLOSE ] ) / 2; - // Idle will initiate a transition - state = VALVE_STATE_IDLE; - } - // Max number of failed homing. Fault and go back to homing not started - else if ( valvesStatus[ valve ].numberOfFailedHomings >= MAX_ALLOWED_FAILED_HOMINGS ) - { - valvesStatus[ valve ].hasValveBeenHomed = FALSE; - valvesStatus[ valve ].hasHomingFailed = TRUE; - state = VALVE_STATE_HOMING_NOT_STARTED; - SET_ALARM_WITH_1_U32_DATA( ALARM_ID_HD_VALVE_HOMING_FAILED, (U32)valve ); - } - else - { - valvesStatus[ valve ].numberOfFailedHomings++; - state = VALVE_STATE_HOMING_NOT_STARTED; - } + // Set the current position to Position C and the commanded position to Position A + valvesStatus[ valve ].hasValveBeenHomed = TRUE; + valvesStatus[ valve ].currentPosition = VALVE_POSITION_C_CLOSE; + valvesStatus[ valve ].commandedPosition = VALVE_POSITION_A_INSERT_EJECT; + valvesStatus[ valve ].hasTransitionBeenRequested = TRUE; + valvesStatus[ valve ].hasHomingBeenRequested = FALSE; + valvesStatus[ valve ].numberOfFailedHomings = 0; + + // Idle will initiate a transition + state = VALVE_STATE_IDLE; + } + // Max number of failed homing. Fault and go back to homing not started + else if ( ++valvesStatus[ valve ].numberOfFailedHomings >= MAX_ALLOWED_FAILED_HOMINGS ) + { + valvesStatus[ valve ].hasValveBeenHomed = FALSE; + valvesStatus[ valve ].hasHomingFailed = TRUE; + state = VALVE_STATE_HOMING_NOT_STARTED; + SET_ALARM_WITH_1_U32_DATA( ALARM_ID_HD_VALVE_HOMING_FAILED, (U32)valve ); + } + else + { + state = VALVE_STATE_HOMING_NOT_STARTED; + } + } + // The time has not elapsed, increment it + else + { + valvesStatus[ valve ].homingEdgeDetectionCounter++; + } } - // 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 ) - { - valvesStatus[ valve ].homingEdgeDetectionCounter++; - } // Reached to target, go for the next target - else if ( deltaPosition < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) + else { valvesStatus[ valve ].homingEdgeDetectionCounter = 0; valvesStatus[ valve ].targetPositionInCounts = currentPosition - HOMING_STEP_CHANGE_IN_COUNTS; @@ -959,12 +971,17 @@ } else if ( mode == VALVE_CONTROL_MODE_DISABLE_ALL ) { - if ( ! ( status & valvesControlStatusBits[ valve ][ VALVE_CONTROL_STATUS_DISABLED ] ) ) + if ( ! ( status & ( ~valvesControlStatusBits[ valve ][ VALVE_CONTROL_STATUS_DISABLED ] ) ) ) { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_VALVE_NOT_FUNCTIONAL, (U32)valve, (U32)mode ); result = FALSE; } } + // Invalid mode was selected + else + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_HD_VALVES_INVALID_CONTROL_MODE_SELECTED, (U32)valve ); + } } return result; @@ -1113,7 +1130,7 @@ if ( valvesStatus[ valve ].positionOutOfRangeCounter > MAX_POS_DEVIATION_TIME_INTERVAL_COUNTER ) { - SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_VALVE_POSITION_OUT_OF_RANGE, (F32)valve, currentPostion ); //TODO is S16 ok with F32? + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_VALVE_POSITION_OUT_OF_RANGE, (U32)valve, currentPostion ); } else if ( abs( currentPostion - commandedPoistion ) < MAX_DEVIATION_FROM_TARGET_IN_COUNTS && valvesStatus[ valve ].positionOutOfRangeCounter > 0 ) @@ -1214,12 +1231,44 @@ { nextStep = stepChange; } - // If the valve is currently in position A and its commanded position is position B, - // add the defined number of steps for the next transition - // If the valve is currently in position A and its commanded position is position C, - // subtract the defined number of steps for the next transition - if ( currentPositionEnum == VALVE_POSITION_A_INSERT_EJECT ) + + switch ( currentPositionEnum ) { + case VALVE_POSITION_NOT_IN_POSITION: + // Do nothing. It will be faulted in the monitor function if it is out of position + break; + + case VALVE_POSITION_A_INSERT_EJECT: + // If the valve is currently in position A and its commanded position is position B, + // add the defined number of steps for the next transition + // If the valve is currently in position A and its commanded position is position C, + // subtract the defined number of steps for the next transition + if ( commandedPositionEnum == VALVE_POSITION_B_OPEN ) + { + valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts + nextStep; + } + if ( commandedPositionEnum == VALVE_POSITION_C_CLOSE ) + { + valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts - nextStep; + } + break; + + case VALVE_POSITION_B_OPEN: + // If the valve is currently in position B, subtract the defined number of steps for the next transition + valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts - nextStep; + break; + + case VALVE_POSITION_C_CLOSE: + // If the valve is currently in position C, add the defined number of steps for the next transition + valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts + nextStep; + break; + + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_HD_VAVLES_INVALID_POSITION_SELECTED, (U32)valve ); + break; + } + /*if ( currentPositionEnum == VALVE_POSITION_A_INSERT_EJECT ) + { if ( commandedPositionEnum == VALVE_POSITION_B_OPEN ) { valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts + nextStep; @@ -1229,16 +1278,16 @@ valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts - nextStep; } } - // If the valve is currently in position B, subtract the defined number of steps for the next transition + else if ( currentPositionEnum == VALVE_POSITION_B_OPEN ) { valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts - nextStep; } - // If the valve is currently in position C, add the defined number of steps for the next transition + else if ( currentPositionEnum == VALVE_POSITION_C_CLOSE ) { valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts + nextStep; - } + }*/ // Call the function to send the set point to FPGA. Current relaxation is not // enabled here, so it is always false