Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -rbe1f6ba8f58abfe098865c85ebca070eb0dde6ce -r9d11c51da60da3f8d2917433938a583f8a105318 --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision be1f6ba8f58abfe098865c85ebca070eb0dde6ce) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 9d11c51da60da3f8d2917433938a583f8a105318) @@ -25,7 +25,6 @@ #include "Timers.h" #include "Valves.h" - /** * @addtogroup Valves * @{ @@ -44,7 +43,7 @@ #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_VENOUS_OFFSET_FROM_ZERO_CNT 32 ///< Position D venous offset from zero position in counts. +#define POS_D_PARTIAL_CLOSE_FROM_ZERO_CNT 32 ///< Position D partial close from zero position in counts. /// Valve controller states typedef enum Valve_Control_States @@ -126,7 +125,7 @@ static VALVE_STATE_T handleValvesFindEnergizedEdgeState( 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 ); /*********************************************************************//** @@ -334,6 +333,8 @@ { VALVE_STATE_T nextState = VALVE_STATE_WAIT_FOR_POST; + checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_WAIT_FOR_POST_STATE ); + if ( getCurrentOperationMode() != MODE_INIT ) { nextState = VALVE_STATE_RESET_VALVE; @@ -354,24 +355,10 @@ static VALVE_STATE_T handleValvesResetValve( VALVE_T valve ) { VALVE_STATE_T nextState = VALVE_STATE_RESET_ENCODER; - U08 resetValve = FPGA_PINCH_VALVES_1_8_STEP | FPGA_PINCH_VALVES_ENABLE_ENCODER | FPGA_PINCH_VALVES_DISABLE | - FPGA_PINCH_VALVES_RESET | FPGA_PINCH_VALVES_NOT_SLEEP; - switch ( valve ) - { - case H1_VALV: - setH1Control( resetValve ); - break; + checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_RESET ); + resetValve( valve ); - case H19_VALV: - setH19Control( resetValve ); - break; - - default: - // Do nothing as the fault mode is checked in the exec function - break; - } - return nextState; } @@ -387,31 +374,12 @@ static VALVE_STATE_T handleValvesResetEncoder( VALVE_T valve ) { VALVE_STATE_T nextState = VALVE_STATE_ENABLE_VALVE; - U08 resetEnc = FPGA_PINCH_VALVES_1_8_STEP | FPGA_PINCH_VALVES_RESET_ENCODER | FPGA_PINCH_VALVES_DISABLE | - FPGA_PINCH_VALVES_RESET | FPGA_PINCH_VALVES_NOT_SLEEP; - switch ( valve ) - { - case H1_VALV: - setH1Control( resetEnc ); - break; - - case H19_VALV: - setH19Control( resetEnc ); - break; - - default: - // Do nothing as the fault mode is checked in the exec function - break; - } - + checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_RESET_ENCODER ); + resetValveEncoder( valve ); // Set the timer for the first debounce read currentValveStates[ valve ].valveOpsStartTime = getMSTimerCount(); -#ifdef ASN_DEMO - currentValveStates[ valve ].hasValveBeenReset = FALSE; -#endif - return nextState; } @@ -427,81 +395,32 @@ static VALVE_STATE_T handleValvesEnableValve( VALVE_T valve ) { VALVE_STATE_T nextState = VALVE_STATE_ENABLE_VALVE; - U08 enableValve = FPGA_PINCH_VALVES_1_8_STEP | FPGA_PINCH_VALVES_ENABLE_ENCODER | FPAG_PINCH_VALVES_ENABLE | - FPGA_PINCH_VALVES_NOT_RESET | FPGA_PINCH_VALVES_NOT_SLEEP; S16 valveEncPosistion = getValveEncoderPosition( valve ); + checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_ENABLE ); + if ( FALSE == currentValveStates[ valve ].hasValveBeenReset ) { - switch ( valve ) - { - case H1_VALV: - setH1Control( enableValve ); - break; - - case H19_VALV: - setH19Control( enableValve ); - break; - - default: - // Do nothing as the fault mode is checked in the exec function - break; - } - + enableValve( valve ); currentValveStates[ valve ].hasValveBeenReset = TRUE; } else { - // First enable the valve and then set the position to 0 to clear the FPGA command registers - switch ( valve ) - { - case H1_VALV: - setH1Position( 0 ); - break; - - case H19_VALV: - setH19Position( 0 ); - break; - - default: - // Do nothing as the fault mode is checked in the exec function - break; - } + 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, 2000 /*ZERO_ENC_DEBOUNCE_TIMEOUT_MS*/ ) ) + else if ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, ZERO_ENC_DEBOUNCE_TIMEOUT_MS ) ) { -#ifndef ASN_DEMO // 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_VENOUS_OFFSET_FROM_ZERO_CNT; - nextState = VALVE_STATE_HOMING_NOT_STARTED; -#endif - -#ifdef ASN_DEMO - nextState = VALVE_STATE_HOMING_NOT_STARTED; - switch ( valve ) - { - case H1_VALV: - setH1Position( 880 ); - break; - - case H19_VALV: - setH19Position( 880 ); - break; - - default: - // Do nothing as the fault mode is checked in the exec function - break; - } - nextState = VALVE_STATE_HOMING_NOT_STARTED; -#endif + 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; @@ -520,28 +439,31 @@ { VALVE_STATE_T nextState = VALVE_STATE_HOMING_NOT_STARTED; -#ifndef ASN_DEMO + checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_NOT_HOMED ); + if ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) { - switch ( currentValveStates[ valve ].valveHomingStatus ) - { - case VALVE_HOMING_INIT: - currentValveStates[ valve ].proposedEnergizedPos = ROTARY_VALVE_INIT_FULL_SWING_COUNTS; - break; + currentValveStates[ valve ].proposedEnergizedPos = ROTARY_VALVE_INIT_FULL_SWING_COUNTS; - case VALVE_HOMING_FORWARD: - currentValveStates[ valve ].proposedEnergizedPos += ROTARY_VALVE_MICROSTEP_FRACTION; - break; + //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; + //} - case VALVE_HOMING_REVERSE: - currentValveStates[ valve ].proposedEnergizedPos -= ROTARY_VALVE_MICROSTEP_FRACTION; - break; - - default: - // Do nothing as complete has nothing to do - break; - } - if ( ( currentValveStates[ valve ].proposedEnergizedPos <= ROTARY_VALVE_ENERGIZED_EDGE_MIN_COUNTS ) || ( currentValveStates[ valve ].proposedEnergizedPos >= ROTARY_VALVE_ENERGIZED_EDGE_MAX_COUNTS ) ) { @@ -559,7 +481,6 @@ setValveCmdChangePosition( valve, (U16)currentValveStates[ valve ].proposedEnergizedPos, MOTOR_DIR_FORWARD ); } } -#endif return nextState; } @@ -578,6 +499,8 @@ VALVE_STATE_T nextState = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; S16 curPos = currentValveStates[ valve ].currentEncPosition; + checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_FIND_ENERGIZED_EDGE ); + // have we found forward edge of travel? if ( abs( curPos - currentValveStates[ valve ].priorEncPosition ) < VALVE_HOME_MIN_POS_CHG ) { @@ -587,22 +510,22 @@ } else if ( TRUE == didTimeout( currentValveStates[ valve ].valveOpsStartTime, HOMING_EDGE_DETECTION_TIMEOUT_MS ) ) { - if ( VALVE_HOMING_INIT == currentValveStates[ valve ].valveHomingStatus ) + //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 { - 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 - { S16 posC = currentValveStates[ valve ].positionsABC[ VALVE_POSITION_C_CLOSE ]; S16 posA = posC + ( ( curPos - posC ) / 2 ); @@ -655,6 +578,8 @@ { VALVE_STATE_T nextState = VALVE_STATE_IDLE; + checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_IDLE ); + // handle a home request if ( TRUE == currentValveStates[ valve ].hasHomingBeenRequested ) { @@ -668,6 +593,7 @@ 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 ); @@ -718,6 +644,8 @@ S16 tgtPos = currentValveStates[ valve ].positionsABC[ currentValveStates[ valve ].commandedPosition ]; S16 delta = tgtPos - curPos; + checkValveInRange( valve, SW_FAULT_ID_TD_VALVES_INVALID_IN_TRANSITION ); + // have we completed the transition? if ( abs( delta ) < VALVE_TRANSITION_MIN_TGT_DELTA ) { @@ -770,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