Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -rfd12aa494e60cc32852a32d1c3aa2b49eb013fdf -r2d5157c59ced0866aaa32d9722d6f1ba28b0b18d --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision fd12aa494e60cc32852a32d1c3aa2b49eb013fdf) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 2d5157c59ced0866aaa32d9722d6f1ba28b0b18d) @@ -52,9 +52,6 @@ #define VBV_ENABLE_PID_STATUS_BIT_MASK 0x0010 ///< VBV PID status bit mask #define VBV_ENABLE_BYPASS_STATUS_BIT_MASK 0x0020 ///< VBV bypass status bit mask -#define VALVES_DISABLE_BIT_MASK 0x0000 ///< Valves disable bit mask (command) -#define VALVES_DISABLED_BIT_MASK 0x0000 ///< Valves disabled bit mask (check) - #define ENABLE_VALVE_CURRENT_RELAXATION 0x8000 ///< Enable valves current relaxation #define DISABLE_VALVE_CURRENT_RELAXATION 0x7FFF ///< Disable valves current relaxation @@ -65,30 +62,28 @@ #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 #define MAX_DEVIATION_FROM_TARGET_IN_COUNTS 150 ///< Maximum deviation from target in counts +#define MAX_DEVIATION_FROM_TRAGET_IN_POS_B 1000 ///< Maximum allowed deviation from position B in counts #define MAX_ALLOWED_FAILED_HOMINGS 3U ///< Maximum allowed failed homings #define HOMING_EDGE_DETECTION_TIME_INTERVAL ( MS_PER_SECOND / ( 2 * TASK_PRIORITY_INTERVAL ) ) ///< The time that the valve must be at the edge to be considered for edge detection #define VALVES_CURRENT_THRESHOLD_AMPS 1.0 ///< Valves current threshold #define MAX_OVER_CURRENT_TIME_INTERVAL_COUNTER ( MS_PER_SECOND / ( TASK_PRIORITY_INTERVAL ) ) ///< Valves over current time interval counter #define MAX_POS_DEVIATION_TIME_INTERVAL_COUNTER ( MS_PER_SECOND / ( TASK_PRIORITY_INTERVAL ) ) ///< Valves out of range time interval counter #define VALVE_TRANSITION_TIMEOUT_MS 3000U ///< Valves transition time out in ms -// TODO changed the interval count to every 10 calls in priority task which is 100 ms -#define VALVES_DATA_PUB_INTERVAL ( MS_PER_SECOND / ( 10 * TASK_PRIORITY_INTERVAL ) ) ///< Valve data publication time interval +#define VALVES_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) ///< Valve data publication time interval -#define VALVE_MAX_ALLOWED_PWM_PERCENT 100U ///< Valve maximum allowed PWM in percent +#define VALVE_MAX_ALLOWED_PWM_PERCENT 100U ///< Valve maximum allowed PWM in percent #define VALVE_CW_PWM_TO_CNT_CONVERSION( pwm ) ( ( 20 * pwm ) + 2500 ) ///< Valve clockwise PWM to count conversion #define VALVE_CW_CNT_TO_PWM_CONVERSION( cnt ) ( ( 0.05 * cnt ) - 125 ) ///< Valve clockwise count to PWM conversion #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 { @@ -130,14 +125,14 @@ NUM_OF_VALVE_CONTROL_STATUS ///< Number of valve control status }; +/// 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; -#pragma pack(push, 1) /// Valve bypass (fixed PWM) structure typedef struct { @@ -153,54 +148,56 @@ { VALVE_POSITION_T commandedPosition; ///< Valve commanded position enum VALVE_POSITION_T currentPosition; ///< Valve current position enum - S16 currentPositionInCounts; ///< Valve current position n counts + S16 currentPositionInCounts; ///< Valve current position in counts S16 targetPositionInCounts; ///< Valve target position in counts - BOOL hasTransitionBeenRequested; ///< Valve transition has been requested flag + BOOL hasTransitionBeenRequested; ///< Valve transition request flag VALVE_STATE_T execState; ///< Valve execution state U32 transitionStartTime; ///< Valve transition start time S16 positions[ NUM_OF_VALVE_POSITIONS ]; ///< Valve positions array (Pos A, Pos B, Pos C) - VALVE_MODE_T controlMode; ///< Valve control mode + VALVE_MODE_T controlMode; ///< Valve control mode (PID, bypass, disable) F32 current; ///< Valve current 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 + U32 homingEdgeDetectionCounter; ///< Valve homing counter when there is not much travel BOOL hasHomingBeenRequested; ///< Valve homing request flag BOOL hasValveBeenHomed; ///< Valve homing completed flag BOOL hasHomingFailed; ///< Valve homing failed flag // Bypass (fixed PWM) mode VALVE_BYPASS_T bypassModeStatus; ///< Valve bypass (fixed PWM) mode } 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 ]; + 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) // Self test function prototypes static VALVE_SELF_TEST_STATE_T handleValveSelfTestEnableValves( void ); @@ -243,12 +240,16 @@ 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 for ( valve = VDI; valve < NUM_OF_VALVES; valve++ ) @@ -268,9 +269,21 @@ * @param valve that is set to be homed * @return none *************************************************************************/ -void homeValve( VALVE_T valve ) +BOOL homeValve( VALVE_T valve ) { - valvesStatus[ valve ].hasHomingBeenRequested = TRUE; + BOOL result = FALSE; + + if ( valve < NUM_OF_VALVES ) + { + valvesStatus[ valve ].hasHomingBeenRequested = TRUE; + result = TRUE; + } + else + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_HD_VALVES_INVALID_VALVE_SELECTED, (U32)valve ); + } + + return result; } /*********************************************************************//** @@ -288,13 +301,17 @@ // Valve not in position cannot be requested // All the other positions are valid - if ( position != VALVE_POSITION_NOT_IN_POSITION ) + if ( position != VALVE_POSITION_NOT_IN_POSITION && valve < NUM_OF_VALVES ) { valvesStatus[ valve ].commandedPosition = position; valvesStatus[ valve ].hasTransitionBeenRequested = TRUE; result = TRUE; } + else + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_HD_VALVES_INVALID_VALVE_SELECTED, (U32)valve ); + } return result; } @@ -309,32 +326,66 @@ *************************************************************************/ 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; + } + else + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_HD_VALVES_INVALID_VALVE_SELECTED, (U32)valve ); + } + + return position; } /*********************************************************************//** * @brief * The setValveBloodTrap function set the blood trap valve to open or close * position. - * @details Inputs: none - * @details Outputs: none + * @details Inputs: valveAirTrapStatus + * @details 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 == STATE_OPEN ) + // 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, SW_FAULT_ID_HD_VALVES_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 + * @details 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 * @details Outputs: valveSelfTestState @@ -375,53 +426,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++ ) { 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 ); } } @@ -462,18 +516,15 @@ *************************************************************************/ static VALVE_SELF_TEST_STATE_T handleValveSelfTestConfirmEnable( void ) { - VALVE_SELF_TEST_STATE_T state = VALVE_SELF_TEST_CONFIRM_ENABLE; + 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; - state = VALVE_SELF_TEST_COMPLETE; } else { valvesSelfTestResult = SELF_TEST_STATUS_FAILED; - state = VALVE_SELF_TEST_COMPLETE; } return state; @@ -518,13 +569,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 ( ( valvesStatus[ valve ].hasHomingBeenRequested || valveSelfTestState == VALVE_SELF_TEST_COMPLETE ) && -// 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; @@ -555,29 +601,32 @@ 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 ) + 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 ); - - state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; - } - else if ( deltaPosition < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) - { - if ( valvesStatus[ valve ].restCounter >= 0 ) // TODO add a #define for the rest time. Do we need the rest? + if ( valvesStatus[ valve ].homingEdgeDetectionCounter >= HOMING_EDGE_DETECTION_TIME_INTERVAL ) { - valvesStatus[ valve ].homingEdgeDetectionCounter = 0; - valvesStatus[ valve ].targetPositionInCounts = currentPosition + HOMING_STEP_CHANGE_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 ); + + state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; } + else + { + valvesStatus[ valve ].homingEdgeDetectionCounter++; + } + } + // Reached to target, go for the next target + else + { + valvesStatus[ valve ].homingEdgeDetectionCounter = 0; + valvesStatus[ valve ].targetPositionInCounts = currentPosition + HOMING_STEP_CHANGE_IN_COUNTS; + setFPGAValveSetPoint( valve, valvesStatus[ valve ].targetPositionInCounts, FALSE ); + } return state; } @@ -597,61 +646,66 @@ 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 && - 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 ]; - // 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; + S16 energizedEdge = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; + S16 deltaEdges = energizedEdge - currentPosition; - // 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; + // 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 ]; - // 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; - } + // 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; - } - else if ( abs(deltaPosition) < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) - { - if ( valvesStatus[ valve ].restCounter >= 0 ) // TODO add a #define for the rest time. Do we need the rest? + // 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 = 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 + { + valvesStatus[ valve ].homingEdgeDetectionCounter = 0; + valvesStatus[ valve ].targetPositionInCounts = currentPosition - HOMING_STEP_CHANGE_IN_COUNTS; + setFPGAValveSetPoint( valve, valvesStatus[ valve ].targetPositionInCounts, FALSE ); + } return state; } @@ -683,8 +737,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 ) { @@ -738,7 +791,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 @@ -763,6 +817,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 ); @@ -909,12 +964,18 @@ } 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 ); + result = FALSE; + } } return result; @@ -961,7 +1022,7 @@ break; default: - // TODO alarm or ignore it? + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_HD_VALVES_INVALID_VALVE_SELECTED, (U32)valve ); break; } } @@ -1047,21 +1108,29 @@ // Check the valve is in Idle state, otherwise the position should not be checked if ( valvesStatus[ valve ].execState == VALVE_STATE_IDLE ) { + U32 maxDeviation = MAX_DEVIATION_FROM_TARGET_IN_COUNTS; + currentPostion = valvesStatus[ valve ].currentPositionInCounts; commandedPositionEnum = valvesStatus[ valve ].commandedPosition; commandedPoistion = valvesStatus[ valve ].positions[ commandedPositionEnum ]; + // If the current position is B, current relaxation requires looser range check + if ( VALVE_POSITION_B_OPEN == commandedPositionEnum ) + { + maxDeviation = MAX_DEVIATION_FROM_TRAGET_IN_POS_B; + } + // Check if the current position has deviated from the position it is supposed to be in // for more than a certain amount of time. If it has, raise an alarm // Absolute value is used for comparison to cover +/- from the commanded position - if ( abs( currentPostion - commandedPoistion ) > MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) + if ( abs( currentPostion - commandedPoistion ) > maxDeviation ) { valvesStatus[ valve ].positionOutOfRangeCounter++; } if ( valvesStatus[ valve ].positionOutOfRangeCounter > MAX_POS_DEVIATION_TIME_INTERVAL_COUNTER ) { - SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_VALVE_CURRENT_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 ) @@ -1118,6 +1187,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 ); @@ -1151,31 +1221,42 @@ { 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 ) { - if ( commandedPositionEnum == VALVE_POSITION_B_OPEN ) - { - valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts + nextStep; - } - if ( commandedPositionEnum == VALVE_POSITION_C_CLOSE ) - { + 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 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 @@ -1227,7 +1308,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; } } @@ -1268,6 +1349,8 @@ } } #endif + + /************************************************************************* * TEST SUPPORT FUNCTIONS *************************************************************************/ @@ -1332,17 +1415,14 @@ { BOOL result = FALSE; - if ( valve < NUM_OF_VALVES ) + if ( TRUE == isTestingActivated() && valve < NUM_OF_VALVES ) { - if ( TRUE == isTestingActivated() ) - { - // Get the current position of the valve as the initial position - valvesPositionOverride[ valve ].ovInitData = (U32)valvesStatus[ valve ].currentPosition; - valvesPositionOverride[ valve ].ovData = position; - valvesPositionOverride[ valve ].override = OVERRIDE_KEY; + // Get the current position of the valve as the initial position + valvesPositionOverride[ valve ].ovInitData = (U32)valvesStatus[ valve ].currentPosition; + valvesPositionOverride[ valve ].ovData = position; + valvesPositionOverride[ valve ].override = OVERRIDE_KEY; - result = setValvePosition( (VALVE_T)valve, (VALVE_POSITION_T)position ); - } + result = setValvePosition( (VALVE_T)valve, (VALVE_POSITION_T)position ); } return result; @@ -1361,7 +1441,7 @@ { BOOL result = FALSE; - if ( TRUE == isTestingActivated() ) + if ( TRUE == isTestingActivated() && valve < NUM_OF_VALVES ) { valvesPositionOverride[ valve ].override = OVERRIDE_RESET; valvesPositionOverride[ valve ].ovData = valvesPositionOverride[ valve ].ovInitData; @@ -1389,27 +1469,23 @@ { BOOL result = FALSE; - if ( TRUE == isTestingActivated() ) + if ( TRUE == isTestingActivated() && valve < NUM_OF_VALVES ) { - // Check if the valve is in the range of the valves available - if ( valve < NUM_OF_VALVES ) + BOOL homingStatus = valvesStatus[ valve ].hasValveBeenHomed; + + // The PWM should be in range and also the valve direction should be legal and the valve must have been homed + // prior to running this command + if ( pwm <= VALVE_MAX_ALLOWED_PWM_PERCENT && direction < NUM_OF_VALVE_DIRECTION && homingStatus ) { - BOOL homingStatus = valvesStatus[ valve ].hasValveBeenHomed; + valvesStatus[ (VALVE_T)valve ].bypassModeStatus.commandedPWMInPercent = (U16)pwm; + valvesStatus[ (VALVE_T)valve ].bypassModeStatus.direction = (VALVE_DIRECTION_T)direction; + // This flag sets the valves to bypass mode in the idle state. After that the valve will be sitting in the + // bypass mode until this flag is set to FALSE in the reset function. + valvesStatus[ (VALVE_T)valve ].bypassModeStatus.hasBypassModeBeenRequeseted = TRUE; + // This flag is used to check if a change in PWM has been requested so the new PWM is processed and set + valvesStatus[ (VALVE_T)valve ].bypassModeStatus.hasChangeBeenRequested = TRUE; - // The PWM should be in range and also the valve direction should be legal and the valve must have been homed - // prior to running this command - if ( pwm <= VALVE_MAX_ALLOWED_PWM_PERCENT && direction < NUM_OF_VALVE_DIRECTION && homingStatus ) - { - valvesStatus[ (VALVE_T)valve ].bypassModeStatus.commandedPWMInPercent = (U16)pwm; - valvesStatus[ (VALVE_T)valve ].bypassModeStatus.direction = (VALVE_DIRECTION_T)direction; - // This flag sets the valves to bypass mode in the idle state. After that the valve will be sitting in the - // bypass mode until this flag is set to FALSE in the reset function. - valvesStatus[ (VALVE_T)valve ].bypassModeStatus.hasBypassModeBeenRequeseted = TRUE; - // This flag is used to check if a change in PWM has been requested so the new PWM is processed and set - valvesStatus[ (VALVE_T)valve ].bypassModeStatus.hasChangeBeenRequested = TRUE; - - result = TRUE; - } + result = TRUE; } } @@ -1430,7 +1506,7 @@ BOOL result = FALSE; // Check if the valve is in the range of the valves available - if ( valve < NUM_OF_VALVES ) + if ( TRUE == isTestingActivated() && valve < NUM_OF_VALVES ) { // Turn off the bypass mode request and the change request valvesStatus[ (VALVE_T)valve ].bypassModeStatus.hasBypassModeBeenRequeseted = FALSE;