Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -r62db7c6218b47db4ef80e1b528228a8887545aad -r28dd261825b2c742df73042ac2902db0c8f6f2b7 --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision 62db7c6218b47db4ef80e1b528228a8887545aad) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 28dd261825b2c742df73042ac2902db0c8f6f2b7) @@ -1,7 +1,11 @@ +// Includes + #include "Valves.h" #include "FPGA.h" #include "Timers.h" +#include "TaskPriority.h" +#include "SystemCommMessages.h" /** * @addtogroup Valves @@ -12,47 +16,55 @@ // Valve dialyzer inlet defines #define VDI_ENABLE_PID_INTERFACE 0x0010 -#define VDI_ENABLE_BYPASS_INTERFACE 0x00EF +#define VDI_ENABLE_BYPASS_INTERFACE 0x0020 -#define VDI_INITIALIZATION_STATUS_BIT_MASK 0x0040 +#define VDI_INIT_STATUS_BIT_MASK 0x0040 #define VDI_ENABLE_PID_STATUS_BIT_MASK 0x0080 #define VDI_ENABLE_BYPASS_STATUS_BIT_MASK 0x00100 // Valve dialyzer outlet defines #define VDO_ENABLE_PID_INTERFACE 0x0040 -#define VDO_ENABLE_BYPASS_INTERFACE 0x00BF +#define VDO_ENABLE_BYPASS_INTERFACE 0x0080 -#define VDO_INITIALIZATION_STATUS_BIT_MASK 0x0200 +#define VDO_INIT_STATUS_BIT_MASK 0x0200 #define VDO_ENABLE_PID_STATUS_BIT_MASK 0x0400 #define VDO_ENABLE_BYPASS_STATUS_BIT_MASK 0x0800 // Valve blood arterial defines #define VBA_ENABLE_PID_INTERFACE 0x0001 -#define VBA_ENABLE_BYPASS_INTERFACE 0x00FE +#define VBA_ENABLE_BYPASS_INTERFACE 0x0002 -#define VBA_INITIALIZATION_STATUS_BIT_MASK 0x0001 +#define VBA_INIT_STATUS_BIT_MASK 0x0001 #define VBA_ENABLE_PID_STATUS_BIT_MASK 0x0002 #define VBA_ENABLE_BYPASS_STATUS_BIT_MASK 0x0004 // Valve blood venous defines #define VBV_ENABLE_PID_INTERFACE 0x0004 -#define VBV_ENABLE_BYPASS_INTERFACE 0x00FB +#define VBV_ENABLE_BYPASS_INTERFACE 0x0008 -#define VBV_INITIALIZATION_STATUS_BIT_MASK 0x0008 +#define VBV_INIT_STATUS_BIT_MASK 0x0008 #define VBV_ENABLE_PID_STATUS_BIT_MASK 0x0010 #define VBV_ENABLE_BYPASS_STATUS_BIT_MASK 0x0020 +#define ADC_TO_VOLTAGE_CONVERSION(adc) ( (F32)( adc / 2048 ) -1 ) +#define VOLTAGE_TO_AMPS_CONVERSION(vol) ( ( 2 * vol) - 1 ) + #define INITIAL_EDGE_OFFSET_READ_COUNT 100 #define INITIAL_DEENERGIZED_POSITION ( 0 - INITIAL_EDGE_OFFSET_READ_COUNT ) -#define INITIAL_ENERGIZED_POSITION (12000 + INITIAL_EDGE_OFFSET_READ_COUNT ) +#define INITIAL_ENERGIZED_POSITION ( 12000 + INITIAL_EDGE_OFFSET_READ_COUNT ) -#define CONTINUOUS_CLOSED_DOOR_TIME_MS 1000U -#define STEP_CHANGE_IN_COUNTS 100U -#define MAX_POS_DEVIATION_FROM_TARGET_COUNTS 1000U //TODO test this tolerance -#define VALVE_TRANSITION_TIMEOUT_MS 50U //TODO test the timeout +#define STEP_CHANGE_IN_COUNTS 100 +#define MAX_POS_DEVIATION_FROM_TARGET_COUNTS 1000 //TODO test this tolerance +#define VALVE_TRANSITION_TIMEOUT_MS 50 //TODO test the timeout #define MAX_POSITION_DEVIATION_FROM_EDGES_COUNTS 50U #define MAX_ALLOWED_FAILED_HOMINGS 3U +#define HOMING_EDGE_DETECTION_TARGET_COUNTER 5U +#define VALVES_CURRENT_THRESHOLD_AMPS 1U +#define OVER_CURRENT_COUNTER 100U +#define OUT_OF_RANGE_COUNTER 100U +#define VALVES_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) + // ********** private data ********** /// Exec valve self test states @@ -73,85 +85,163 @@ VALVE_STATE_HOMING_NOT_STARTED, VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE, VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE, - VALVE_STATE_HOMING_TRANISTION_TO_POS_A, NUM_OF_VALVE_STATES, } VALVE_STATE_T; - -typedef enum homingStatus -{ - HOMING_NOT_STARTED = 0, - HOMING_IN_PROGRESS, - HOMING_COMPLETE, - NUM_OF_HOMING_STATES -} HOMING_STATUS_T; //TODO remove - +#pragma pack(push, 1) +/// Valve status struct typedef struct { VALVE_POSITION_T commandedPosition; VALVE_POSITION_T currentPosition; S16 currentPositionInCounts; S16 previousPositionInCounts; BOOL hasTransitionBeenRequested; - HOMING_STATUS_T homingStatus; //TODO remove? - - BOOL hasHomingBeenRequested; - S16 positions[ NUM_OF_VALVE_POSITIONS ]; - U32 numberOfFailedHoming; + VALVE_STATE_T execState; U32 transitionStartTime; + S16 positions[ NUM_OF_VALVE_POSITIONS ]; + F32 valveCurrent; + U32 valveOverCurrentCounter; + U32 valvePositionOutOfRangeCounter; + // Homing variables + U32 numberOfFailedHomings; + U32 homingEdgeDetectionCounter; + BOOL hasHomingBeenRequested; + BOOL hasValveBeenHomed; } VALVE_STATUS_T; +#pragma pack(pop) -static VALVE_STATE_T dialyzerInletValveState = VALVE_STATE_WAIT_FOR_POST; //TODO Remove these variables? -static VALVE_STATE_T dialyzerOutletValveState = VALVE_STATE_WAIT_FOR_POST; -static VALVE_STATE_T bloodArterialValveState = VALVE_STATE_WAIT_FOR_POST; -static VALVE_STATE_T bloodVenousValveState = VALVE_STATE_WAIT_FOR_POST; - static VALVE_SELF_TEST_STATE_T valveSelfTestState = VALVE_SELF_TEST_ENABLE_VALVES; static SELF_TEST_STATUS_T valvesSelfTestResult = SELF_TEST_STATUS_IN_PROGRESS; -static VALVE_STATUS_T valvesStatus[ NUM_OF_VALVE_POSITIONS ]; -static BOOL isDoorBeingChecked = FALSE; -static U32 generalTimer = 0; +static VALVE_STATUS_T valvesStatus[ NUM_OF_VALVES ]; -static void execMonitorValves( void ); -static BOOL areValvesEnabled( void ); -static BOOL isDoorClosed( void ); +static OVERRIDE_U32_T valvesDataPublishInterval = { VALVES_DATA_PUB_INTERVAL, VALVES_DATA_PUB_INTERVAL, 0, 0 }; +static U32 valvesDataPublishCounter = 0; // Self test function prototypes static VALVE_SELF_TEST_STATE_T handleValveSelfTestEnableValves( void ); static VALVE_SELF_TEST_STATE_T handleValveSelfTestConfirmEnable( void ); // Exec function prototypes -static VALVE_STATE_T handleValveStateWaitForPost( VALVE_T valve ); -static VALVE_STATE_T handleValveStateIdle( VALVE_T valve ); -static VALVE_STATE_T handleValveStateInTransition( VALVE_T valve ); -static VALVE_STATE_T handleValveStateHomingNotStarted( VALVE_T valve ); -static VALVE_STATE_T handleValveStateHomingFindEnergizedEdge( VALVE_T valve ); -static VALVE_STATE_T handleValveStateHomingFindDeenergizedEdge( VALVE_T valve ); -static VALVE_STATE_T handleValveStateHomingTransitionToPosA( VALVE_T valve ); +static void handleValveStateWaitForPost( VALVE_T valve ); +static void handleValveStateIdle( VALVE_T valve ); +static void handleValveStateInTransition( VALVE_T valve ); +static void handleValveStateHomingNotStarted( VALVE_T valve ); +static void handleValveStateHomingFindEnergizedEdge( VALVE_T valve ); +static void handleValveStateHomingFindDeenergizedEdge( VALVE_T valve ); -static VALVE_STATE_T processHomingEnergizedState( VALVE_T valve ); -static VALVE_STATE_T processHomingDeenergizedState( VALVE_T valve ); -static VALVE_STATE_T processTransitionToPositions( VALVE_T valve ); // Use this to the last state of homing and transition states +// Private function prototypes +static void execMonitorValves( void ); +static BOOL areValvesEnabled( void ); +static void setFPGAValue( VALVE_T valve, S16 position ); +static void convertAndMonitorValvesCurrent( void ); +static void getAndMonitorValvesCurrentPosition( void ); +static DATA_GET_PROTOTYPE( U32, getPublishValvesDataInterval ); +static void publishValvesData( void ); +/*********************************************************************//** + * @brief + * The initValves function initializes the valves driver + * @details + * Inputs: valveSelfTestState, valvesSelfTestResult, valvesStatus + * Outputs: valveSelfTestState, valvesSelfTestResult, valvesStatus + * @return: none + *************************************************************************/ void initValves( void ) { U08 i; - dialyzerInletValveState = VALVE_STATE_WAIT_FOR_POST; - dialyzerOutletValveState = VALVE_STATE_WAIT_FOR_POST; - bloodArterialValveState = VALVE_STATE_WAIT_FOR_POST; - bloodVenousValveState = VALVE_STATE_WAIT_FOR_POST; - valveSelfTestState = VALVE_SELF_TEST_ENABLE_VALVES; - valvesSelfTestResult = SELF_TEST_STATUS_IN_PROGRESS; + valveSelfTestState = VALVE_SELF_TEST_ENABLE_VALVES; + valvesSelfTestResult = SELF_TEST_STATUS_IN_PROGRESS; // Initialize some of the variables - for ( i = 0; i < NUM_OF_VALVE_POSITIONS; i++ ) + for ( i = 0; i < NUM_OF_VALVES; i++ ) { - valvesStatus[ i ].homingStatus = HOMING_NOT_STARTED; + valvesStatus[ i ].execState = VALVE_STATE_WAIT_FOR_POST; } } +/*********************************************************************//** + * @brief + * The homeValve function sets the homing request flag of a valve to TRUE + * @details + * Inputs: valvesStatus + * Outputs: valvesStatus (The flag that sets the homing request) + * @return: none (TODO DO WE HAVE TO RETURN ANYTHING?) + *************************************************************************/ +BOOL homeValve( VALVE_T valve ) +{ + BOOL result = FALSE; + + valvesStatus[ valve ].hasHomingBeenRequested = TRUE; + + return result; +} + +/*********************************************************************//** + * @brief + * The setValvePosition function sets the requested position of a valve + * @details + * Inputs: valvesStatus + * Outputs: valvesStatus (commanded position) + * @param: valve : Enumeration of the valve + * @param: position : Enumeration of the commanded position + * @return: returns TRUE if the requested position was legal + *************************************************************************/ +BOOL setValvePosition( VALVE_T valve, VALVE_POSITION_T position ) +{ + BOOL result = FALSE; + + // Valve not in position cannot be requested + // All the other positions are valid + if ( position != VALVE_POSITION_NOT_IN_POSITION ) + { + valvesStatus[ valve ].commandedPosition = position; + result = TRUE; + } + + return result; +} + +/*********************************************************************//** + * @brief + * The getValvePosition function returns the current position of a valve + * @details + * Inputs: none + * Outputs: valvesStatus (current position) + * @param: valve : Enumeration of the valve + * @param: position : Enumeration of the commanded position + * @return: returns TRUE if the requested position was legal + *************************************************************************/ +VALVE_POSITION_T getValvePosition( VALVE_T valve) +{ + return valvesStatus[ valve ].currentPosition; +} + +/*********************************************************************//** + * @brief + * The setValveBloodTrap function set the blood trap valve to open or close + * position + * @details + * Inputs: TODO + * Outputs: TODO + * @param: state : Enumeration of open or close + * @return: TODO : What should we return? + *************************************************************************/ +BOOL setValveBloodTrap( OPN_CLS_STATE_T state ) +{ + return TRUE; +} + +/*********************************************************************//** + * @brief + * The execValvesSelfTest function executes the valves self test + * @details + * Inputs: valveSelfTestState + * Outputs: valveSelfTestState + * @return: valvesSelfTestResult : The status of the self test + *************************************************************************/ SELF_TEST_STATUS_T execValvesSelfTest( void ) { switch( valveSelfTestState ) @@ -176,119 +266,82 @@ return valvesSelfTestResult; } +/*********************************************************************//** + * @brief + * The execValves function executes the valves exec state machine + * @details + * Inputs: none + * Outputs: none (All the values are stored in the structure of each valve) + * @return: none + *************************************************************************/ void execValves( void ) { + U08 i; + VALVE_STATE_T state; + VALVE_T valve; + execMonitorValves(); - // VDi state machine - switch ( dialyzerInletValveState ) + // TODO uncomment the for loop + //for ( i = 0; i < NUM_OF_VALVES; i++ ) { - case VALVE_STATE_WAIT_FOR_POST: + //TODO FOR TESTING ONLY REMOVE + valve = VDI; + i = 0; + //TODO FOR TESTING ONLY REMOVE - dialyzerInletValveState = handleValveStateWaitForPost( VDI ); - break; + state = valvesStatus[ i ].execState; + //valve = (VALVE_T)i; - case VALVE_STATE_IDLE: + // VDi state machine + switch ( state ) + { + case VALVE_STATE_WAIT_FOR_POST: - dialyzerInletValveState = handleValveStateIdle( VDI ); - break; + handleValveStateWaitForPost( valve ); + break; - case VALVE_STATE_IN_TRANSITION: + case VALVE_STATE_IDLE: - dialyzerInletValveState = handleValveStateInTransition( VDI ); - break; + handleValveStateIdle( valve ); + break; - case VALVE_STATE_HOMING_NOT_STARTED: + case VALVE_STATE_IN_TRANSITION: - dialyzerInletValveState = handleValveStateHomingNotStarted( VDI ); - break; + handleValveStateInTransition( valve ); + break; - case VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE: + case VALVE_STATE_HOMING_NOT_STARTED: - dialyzerInletValveState = handleValveStateHomingFindEnergizedEdge( VDI ); - break; + handleValveStateHomingNotStarted( valve ); + break; - case VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE: + case VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE: - dialyzerInletValveState = handleValveStateHomingFindDeenergizedEdge( VDI ); - break; + handleValveStateHomingFindEnergizedEdge( valve ); + break; - default: - break; - } -} + case VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE: -BOOL homeValve( VALVE_T valve ) -{ - BOOL result = FALSE; + handleValveStateHomingFindDeenergizedEdge( valve ); + break; - valvesStatus[ valve ].hasHomingBeenRequested = TRUE; - - return result; -} - -BOOL setValvePosition( VALVE_T valve, VALVE_POSITION_T position ) -{ - valvesStatus[ valve ].commandedPosition = position; - - return TRUE; -} - -S16 getValvePosition( VALVE_T valve) -{ - -} - - -BOOL setValveBloodTrap( OPN_CLS_STATE_T state ) -{ - -} - -static void execMonitorValves( void ) -{ - // TODO check current sense - // TODO check enable status - - // Get the current position of the valves in counts and store them - valvesStatus [ VDI ].currentPositionInCounts = getFPGAValveDialyzerInletPosition(); - valvesStatus [ VDO ].currentPositionInCounts = getFPGAValveDialyzerOutletPosition(); - valvesStatus [ VBA ].currentPositionInCounts = getFPGAValveBloodArterialPosition(); - valvesStatus [ VBV ].currentPositionInCounts = getFPGAValveBloodVenousPosition(); -} - - -static VALVE_STATE_T handleValveStateWaitForPost( VALVE_T valve ) -{ - VALVE_STATE_T state = VALVE_STATE_WAIT_FOR_POST; - - switch ( valve ) - { - case VDI: - - if ( valveSelfTestState == VALVE_SELF_TEST_COMPLETE ) - { - state = VALVE_STATE_HOMING_NOT_STARTED; - } - - break; - - case VDO: - break; - - case VBA: - break; - - case VBV: - break; - - default: - break; + default: + break; + } } - return state; + publishValvesData(); } +/*********************************************************************//** + * @brief + * The handleValveSelfTestEnableValves function starts the valves self test + * @details + * Inputs: valvesSelfTestResult + * Outputs: nvalvesSelfTestResult + * @return: Next state of the self test state machine + *************************************************************************/ static VALVE_SELF_TEST_STATE_T handleValveSelfTestEnableValves( void ) { VALVE_SELF_TEST_STATE_T state = VALVE_SELF_TEST_CONFIRM_ENABLE; @@ -304,6 +357,15 @@ return state; } +/*********************************************************************//** + * @brief + * The handleValveSelfTestConfirmEnable function checks to make sure the + * valves are enabled properly + * @details + * Inputs: valvesSelfTestResult + * Outputs: valvesSelfTestResult + * @return: Next state of the self test state machine + *************************************************************************/ static VALVE_SELF_TEST_STATE_T handleValveSelfTestConfirmEnable( void ) { VALVE_SELF_TEST_STATE_T state = VALVE_SELF_TEST_CONFIRM_ENABLE; @@ -318,158 +380,212 @@ return state; } -static VALVE_STATE_T handleValveStateIdle( VALVE_T valve ) +/*********************************************************************//** + * @brief + * The handleValveStateWaitForPost function handles the wait for POST \n + * state + * valves are enabled properly + * @details + * Inputs: valveSelfTestState, valveSelfTestState + * Outputs: valveSelfTestState (the next exec state of a valve) + * @return: none + *************************************************************************/ +static void handleValveStateWaitForPost( VALVE_T valve ) { + VALVE_STATE_T state = VALVE_STATE_WAIT_FOR_POST; + + if ( valveSelfTestState == VALVE_SELF_TEST_COMPLETE ) + { + state = VALVE_STATE_HOMING_NOT_STARTED; + } + + valvesStatus[ valve ].execState = state; +} + +/*********************************************************************//** + * @brief + * The handleValveStateIdle function handles the idle state of the valves \n + * state machine + * valves are enabled properly + * @details + * Inputs: valveSelfTestState, valveSelfTestState + * Outputs: valveSelfTestState (the next exec state of a valve) + * @return: none + *************************************************************************/ +static void handleValveStateIdle( VALVE_T valve ) +{ VALVE_STATE_T state = VALVE_STATE_IDLE; if ( valvesStatus[ valve ].hasHomingBeenRequested ) { state = VALVE_STATE_HOMING_NOT_STARTED; } - else if ( valvesStatus[ valve ].homingStatus == HOMING_COMPLETE && + else if ( valvesStatus[ valve ].hasValveBeenHomed && valvesStatus[ valve ].hasTransitionBeenRequested ) { if ( valvesStatus[ valve ].currentPosition != valvesStatus[ valve ].commandedPosition ) { - //processHomingEnergizedState( valve, valvesStatus[ valve ].commandedPosition ); + VALVE_POSITION_T commandedPositionEnum = valvesStatus[ valve ].commandedPosition; + S16 nextStep = valvesStatus[ valve ].positions[commandedPositionEnum ]; + // Just set the valves to transition so it will not be in a known position for a while + valvesStatus[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; + setFPGAValue( valve, nextStep ); + + state = VALVE_STATE_IN_TRANSITION; } } - return state; + valvesStatus[ valve ].execState = state; } -static VALVE_STATE_T handleValveStateInTransition( VALVE_T valve ) +/*********************************************************************//** + * @brief + * The handleValveStateInTransition function handles the in transition \n + * state of the state machine + * @details + * Inputs: valvesStatus (commanded position, current position, current position \n + * in counts, has transition been requested flag) + * Outputs: valvesStatus + * @return: none + *************************************************************************/ +static void handleValveStateInTransition( VALVE_T valve ) { + VALVE_STATE_T state = VALVE_STATE_IN_TRANSITION; + S16 nextStep = 0; + VALVE_POSITION_T commandedPositionEnum = valvesStatus[ valve ].commandedPosition; + VALVE_POSITION_T currentPositionEnum = valvesStatus[ valve ].currentPosition; -} + // Get the corresponding counts of the positions + S16 commandedPositionCounts = fabs( valvesStatus[ valve ].positions[ commandedPositionEnum ] ); + S16 currentPositionCounts = fabs( valvesStatus[ valve ].currentPositionInCounts ); - -static VALVE_STATE_T handleValveStateHomingNotStarted( VALVE_T valve ) -{ - VALVE_STATE_T state = VALVE_STATE_HOMING_NOT_STARTED; - - switch ( valve ) + // Check if the valve is within range of the commanded position + if ( fabs( commandedPositionCounts - currentPositionCounts ) <= STEP_CHANGE_IN_COUNTS - 1 ) { - case VDI: - - // 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 ) - && isDoorClosed() ) + // The valve is in range. Set the current position to the commanded position + valvesStatus[ valve ].currentPosition = commandedPositionEnum; + // Set the transition request to false as it has been serviced + valvesStatus[ valve ].hasTransitionBeenRequested = FALSE; + // Go back to Idle state + state = VALVE_STATE_IDLE; + } + // Check if the valve's transition time has timed out + else if ( didTimeout( valvesStatus[ valve ].transitionStartTime, VALVE_TRANSITION_TIMEOUT_MS ) ) + { + // TODO raise an alarm + valvesStatus[ valve ].hasTransitionBeenRequested = FALSE; + // Go back to Idle state + state = VALVE_STATE_IDLE; + } + // If everything is good, decide what is the next step + else + { + // If the valve is currently in position A and its commanded position is position B, + // subtract the defined number of steps for the next transition + // If the valve is currently in position A and its commanded position is position C, + // add the defined number of steps for the next transition + if ( currentPositionEnum == VALVE_POSITION_A_INSERT_EJECT ) + { + if ( commandedPositionEnum == VALVE_POSITION_B_OPEN ) { - setFPGAValveDialyzerInletPosition( STEP_CHANGE_IN_COUNTS ); - valvesStatus[ valve ].transitionStartTime = getMSTimerCount(); - state = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; + nextStep = valvesStatus[ valve ].currentPositionInCounts - STEP_CHANGE_IN_COUNTS; } + if ( commandedPositionEnum == VALVE_POSITION_C_CLOSE ) + { + nextStep = valvesStatus[ valve ].currentPositionInCounts + STEP_CHANGE_IN_COUNTS; + } + } + // If the valve is currently in position B, add the defined number of steps for the next transition + else if ( currentPositionEnum == VALVE_POSITION_B_OPEN ) + { + nextStep = valvesStatus[ valve ].currentPositionInCounts + STEP_CHANGE_IN_COUNTS; + } + // If the valve is currently in position C, subtract the defined number of steps for the next transition + else if ( currentPositionEnum == VALVE_POSITION_C_CLOSE ) + { + nextStep = valvesStatus[ valve ].currentPositionInCounts - STEP_CHANGE_IN_COUNTS; + } - break; - - case VDO: - break; - - case VBA: - break; - - case VBV: - break; - - default: - break; + setFPGAValue( valve, nextStep ); } - return state; + valvesStatus[ valve ].execState = state; } - - -static VALVE_STATE_T handleValveStateHomingFindEnergizedEdge( VALVE_T valve ) +/*********************************************************************//** + * @brief + * The handleValveStateHomingNotStarted function handles homing not started \n + * state of the state machine + * @details + * Inputs: valvesStatus, valveSelfTestState + * Outputs: valvesStatus + * @return: none + *************************************************************************/ +static void handleValveStateHomingNotStarted( VALVE_T valve ) { - VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; + VALVE_STATE_T state = VALVE_STATE_HOMING_NOT_STARTED; + // TODO connect this flag to the doors driver later + BOOL isDoorClosed = TRUE; - switch ( valve ) + // 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 ) + && isDoorClosed ) { - case VDI: - - state = processHomingEnergizedState( valve ); - break; - - case VDO: - break; - - case VBA: - break; - - case VBV: - break; - - default: - break; + // Get ready for the energized state + setFPGAValue( valve, STEP_CHANGE_IN_COUNTS ); + valvesStatus[ valve ].transitionStartTime = getMSTimerCount(); + valvesStatus[ valve ].homingEdgeDetectionCounter = 0; + valvesStatus[ valve ].numberOfFailedHomings = 0; + valvesStatus[ valve ].hasValveBeenHomed = FALSE; + state = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; } - return state; + valvesStatus[ valve ].execState = state; } -static VALVE_STATE_T handleValveStateHomingFindDeenergizedEdge( VALVE_T valve ) +/*********************************************************************//** + * @brief + * The handleValveStateHomingFindEnergizedEdge function handles find \n + * energized state of the state machine + * @details + * Inputs: valvesStatus, valveSelfTestState + * Outputs: valvesStatus + * @return: none + *************************************************************************/ +static void handleValveStateHomingFindEnergizedEdge( VALVE_T valve ) { - VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; - - switch ( valve ) - { - case VDI: - - state = processHomingDeenergizedState( valve ); - break; - - case VDO: - break; - - case VBA: - break; - - case VBV: - break; - - default: - break; - } - - return state; -} - -static VALVE_STATE_T processHomingEnergizedState( VALVE_T valve ) -{ VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; S16 currentPosition = 0; S16 previousPosition = 0; - U32 elapsedTime = 0; - BOOL hasTimeElapsed = FALSE; // Read the energized position back currentPosition = fabs( valvesStatus[ valve ].currentPositionInCounts ); previousPosition = fabs( valvesStatus[ valve ].previousPositionInCounts ); - elapsedTime = valvesStatus[ valve ].transitionStartTime; //TODO this is wrong - hasTimeElapsed = didTimeout( elapsedTime, VALVE_TRANSITION_TIMEOUT_MS ); //TODO use the right timeout - //TODO get the timing right (50ms should be started correctly) - if ( fabs( currentPosition - previousPosition ) <= MAX_POSITION_DEVIATION_FROM_EDGES_COUNTS ) + // Check if the valve is within accepted range of the target + if ( fabs( currentPosition - INITIAL_ENERGIZED_POSITION ) <= MAX_POS_DEVIATION_FROM_TARGET_COUNTS ) { - if( fabs( currentPosition - INITIAL_ENERGIZED_POSITION ) <= MAX_POS_DEVIATION_FROM_TARGET_COUNTS ) + // Check if the valve is within the range from the energized edge and it has been there for the defined time + if( fabs( currentPosition - previousPosition ) <= MAX_POSITION_DEVIATION_FROM_EDGES_COUNTS && + ++valvesStatus[ valve ].homingEdgeDetectionCounter > HOMING_EDGE_DETECTION_TARGET_COUNTER ) { // Current position (positive or negative) will be stored in the Position B of the current valve // Do not use the above variables because they absolute valves using fabs valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] = valvesStatus[ valve ].currentPositionInCounts; // Reset the current time and transition to the next state valvesStatus[ valve ].transitionStartTime = getMSTimerCount(); + valvesStatus[ valve ].homingEdgeDetectionCounter = 0; state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; } } + // Check if the transition has timed out else if ( didTimeout( valvesStatus[ valve ].transitionStartTime, VALVE_TRANSITION_TIMEOUT_MS ) ) { - U32 failedHomingCount = valvesStatus[ valve ].numberOfFailedHoming; - - if ( failedHomingCount >= MAX_ALLOWED_FAILED_HOMINGS ) + // If the valve has exceeded the max number of failed homings, raise an alarm + if ( ++valvesStatus[ valve ].numberOfFailedHomings >= MAX_ALLOWED_FAILED_HOMINGS ) { //TODO fault alarm } @@ -478,32 +594,41 @@ } else { + // Everything is normal and the valve is in transition + // Reset the number of failed homing attempts in case there was lingering failures from before + //valvesStatus[ valve ].homingEdgeDetectionCounter = 0; //TODO we should not do this S16 nextStep = valvesStatus[ valve ].currentPositionInCounts + STEP_CHANGE_IN_COUNTS; - setFPGAValveDialyzerInletPosition ( nextStep ); + setFPGAValue( valve, nextStep ); } - return state; + valvesStatus[ valve ].execState = state; } -static VALVE_STATE_T processHomingDeenergizedState( VALVE_T valve ) +/*********************************************************************//** + * @brief + * The handleValveStateHomingFindDeenergizedEdge function handles find \n + * de-energized state of the state machine + * @details + * Inputs: valvesStatus, valveSelfTestState + * Outputs: valvesStatus + * @return: none + *************************************************************************/ +static void handleValveStateHomingFindDeenergizedEdge( VALVE_T valve ) { VALVE_STATE_T state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; S16 currentPosition = 0; S16 previousPosition = 0; - U32 elapsedTime = 0; - BOOL hasTimeElapsed = FALSE; // Read the energized position back currentPosition = fabs( valvesStatus[ valve ].currentPositionInCounts ); previousPosition = fabs( valvesStatus[ valve ].previousPositionInCounts ); - elapsedTime = valvesStatus[ valve ].transitionStartTime; //TODO this is wrong - hasTimeElapsed = didTimeout( elapsedTime, VALVE_TRANSITION_TIMEOUT_MS ); //TODO use the right timeout - //TODO get the timing right (50ms should be started correctly) + // TODO test this statement if ( fabs( currentPosition - previousPosition ) <= MAX_POSITION_DEVIATION_FROM_EDGES_COUNTS ) { - if( fabs( currentPosition - INITIAL_DEENERGIZED_POSITION ) <= MAX_POS_DEVIATION_FROM_TARGET_COUNTS ) + if( fabs( currentPosition - INITIAL_DEENERGIZED_POSITION ) <= MAX_POS_DEVIATION_FROM_TARGET_COUNTS && + ++valvesStatus[ valve ].homingEdgeDetectionCounter > HOMING_EDGE_DETECTION_TARGET_COUNTER ) { S16 positionB = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; // Position A is the average of the Position B that was read last time and position C that was @@ -518,19 +643,25 @@ valvesStatus[ valve ].positions[ VALVE_POSITION_C_CLOSE ] = valvesStatus[ valve ].currentPositionInCounts - INITIAL_EDGE_OFFSET_READ_COUNT; - // Reset the number of failed homing for the next homing sequence - valvesStatus[ valve ].numberOfFailedHoming = 0; - // Reset the current time and transition to the next state valvesStatus[ valve ].transitionStartTime = getMSTimerCount(); - state = VALVE_STATE_HOMING_TRANISTION_TO_POS_A; + + // Homing successfully finished + valvesStatus[ valve ].hasValveBeenHomed = TRUE; + + // Set the current position to Position C and the commanded position to Position A + // Request a transition + valvesStatus[ valve ].currentPosition = VALVE_POSITION_C_CLOSE; + valvesStatus[ valve ].commandedPosition = VALVE_POSITION_A_INSERT_EJECT; + valvesStatus[ valve ].hasTransitionBeenRequested = TRUE; + + // Idle will initiate a transition + state = VALVE_STATE_IDLE; } } else if ( didTimeout( valvesStatus[ valve ].transitionStartTime, VALVE_TRANSITION_TIMEOUT_MS ) ) { - U32 failedHomingCount = valvesStatus[ valve ].numberOfFailedHoming; - - if ( failedHomingCount >= MAX_ALLOWED_FAILED_HOMINGS ) + if ( ++valvesStatus[ valve ].numberOfFailedHomings >= MAX_ALLOWED_FAILED_HOMINGS ) { //TODO fault alarm } @@ -539,15 +670,45 @@ } else { + //valvesStatus[ valve ].homingEdgeDetectionCounter = 0; //TODO do we need this? S16 nextStep = valvesStatus[ valve ].currentPositionInCounts - STEP_CHANGE_IN_COUNTS; - setFPGAValveDialyzerInletPosition ( nextStep ); + setFPGAValue( valve, nextStep ); } - return state; + valvesStatus[ valve ].execState = state; } +/*********************************************************************//** + * @brief + * The execMonitorValves function handles monitoring of the valves. The \n + * function calls other functions to monitor the values + * @details + * Inputs: none + * Outputs: none + * @return: none + *************************************************************************/ +static void execMonitorValves( void ) +{ + // TODO check enable status of the valves here too + // Get the current position of the valves in counts and store them + getAndMonitorValvesCurrentPosition(); + // Get the current in ADC and convert them to amps + // Check whether any of the valves are over current + convertAndMonitorValvesCurrent(); +} + +/*********************************************************************//** + * @brief + * The areValvesEnabled function checks the current valves status from FPGA \n + * to the enable bit mask of each valve. If any of the valves is not enabled \n + * the function raises an alarm + * @details + * Inputs: none + * Outputs: none + * @return: Returns TRUE if all the valves are enabled properly + *************************************************************************/ static BOOL areValvesEnabled( void ) { //TODO alarm for the valve status @@ -556,7 +717,7 @@ U16 status = getFPGAValvesStatus(); // VDi status - if ( ! status & VDI_INITIALIZATION_STATUS_BIT_MASK ) + if ( ! status & VDI_INIT_STATUS_BIT_MASK ) { result = FALSE; } @@ -570,7 +731,7 @@ } // VDo status - if ( ! status & VDO_INITIALIZATION_STATUS_BIT_MASK ) + if ( ! status & VDO_INIT_STATUS_BIT_MASK ) { result = FALSE; } @@ -584,7 +745,7 @@ } // VBA status - if ( ! status & VBA_INITIALIZATION_STATUS_BIT_MASK ) + if ( ! status & VBA_INIT_STATUS_BIT_MASK ) { result = FALSE; } @@ -598,7 +759,7 @@ } // VBV status - if ( ! status & VBV_INITIALIZATION_STATUS_BIT_MASK ) + if ( ! status & VBV_INIT_STATUS_BIT_MASK ) { result = FALSE; } @@ -614,34 +775,195 @@ return result; } -static BOOL isDoorClosed( void ) +/*********************************************************************//** + * @brief + * The setFPGAValue function sets the position of a valve to FPGA + * @details + * Inputs: none + * Outputs: none + * @param: valve : Enumeration of the valve + * @param: position : Position in counts + * @return: none + *************************************************************************/ +static void setFPGAValue( VALVE_T valve, S16 position ) { - BOOL result = FALSE; + switch ( valve ) + { + case VDI: - // TODO change this real door status - // Check the door status - BOOL doorStatus = TRUE; + setFPGAValveDialyzerInletPosition( position ); + break; - // Checking the doors status for the first time - if ( isDoorBeingChecked == FALSE ) - { - generalTimer = getMSTimerCount(); - isDoorBeingChecked = TRUE; + case VDO: + + setFPGAValveDialyzerOutletPosition( position ); + break; + + case VBA: + + setFPGAValveBloodVenousPosition( position ); + break; + + case VBV: + + setFPGAValveBloodVenousPosition( position ); + break; + + default: + break; } - else +} + +/*********************************************************************//** + * @brief + * The convertAndMonitorValvesCurrent function gets the current values \n + * in counts from FPGA and converts them to current. If any of the values \n + * is above the threshold, the function will raise an alarm + * @details + * Inputs: valvesStatus + * Outputs: valvesStatus + * @return: none + *************************************************************************/ +static void convertAndMonitorValvesCurrent( void ) +{ + U08 i; + U16 currentInADC = 0; + F32 ADC2Voltage = 0; + F32 voltage2Current = 0; + + // Get the current from FPGA in ADC + currentInADC = getFPGAValveDialyzerInletCurrentCounts(); + ADC2Voltage = ADC_TO_VOLTAGE_CONVERSION( currentInADC ); + voltage2Current = ADC_TO_VOLTAGE_CONVERSION( ADC2Voltage ); + valvesStatus[ VDI ].valveCurrent = voltage2Current; + + currentInADC = getFPGAValveDialyzerOutletCurrentCounts(); + ADC2Voltage = ADC_TO_VOLTAGE_CONVERSION( currentInADC ); + voltage2Current = ADC_TO_VOLTAGE_CONVERSION( ADC2Voltage ); + valvesStatus[ VDO ].valveCurrent = voltage2Current; + + currentInADC = getFPGAValveBloodVenousCurrentCounts(); + ADC2Voltage = ADC_TO_VOLTAGE_CONVERSION( currentInADC ); + voltage2Current = ADC_TO_VOLTAGE_CONVERSION( ADC2Voltage ); + valvesStatus[ VBA ].valveCurrent = voltage2Current; + + currentInADC = getFPGAValveBloodArterialCurrentCounts(); + ADC2Voltage = ADC_TO_VOLTAGE_CONVERSION( currentInADC ); + voltage2Current = ADC_TO_VOLTAGE_CONVERSION( ADC2Voltage ); + valvesStatus[ VBV ].valveCurrent = voltage2Current; + + // The current of all the valves + for ( i = 0; i < NUM_OF_VALVES; i++) { - // If the door status is true and the time has elapsed - if ( doorStatus && didTimeout( generalTimer, CONTINUOUS_CLOSED_DOOR_TIME_MS ) ) + // Check if the current is over the threshold for the defined amount of time + if ( valvesStatus[ i ].valveCurrent > VALVES_CURRENT_THRESHOLD_AMPS && + ++valvesStatus[ i ].valveOverCurrentCounter > OVER_CURRENT_COUNTER ) { - isDoorBeingChecked = FALSE; - result = TRUE; + //TODO fault } - // If door status is not true, then we should exit and reset - else if ( doorStatus == FALSE ) + // If the current is below the threshold again and the counter for the time is greater than + // zero, set the counter to zero + else if ( valvesStatus[ i ].valveCurrent < VALVES_CURRENT_THRESHOLD_AMPS && + valvesStatus[ i ].valveOverCurrentCounter > 0 ) { - isDoorBeingChecked = FALSE; + valvesStatus[ i ].valveOverCurrentCounter = 0; } } +} +/*********************************************************************//** + * @brief + * The getAndMonitorValvesCurrentPosition function gets the current position \n + * of the valves and stores them into the structure of each valve. The function \n + * checks whether any of the valves have deviated from their current position \n + * if they are in idle state. If any of the valves have deviated, the function \n + * raises an alarm + * @details + * Inputs: valvesStatus + * Outputs: valvesStatus + * @return: none + *************************************************************************/ +static void getAndMonitorValvesCurrentPosition( void ) +{ + U08 i; + S16 currentPostion = 0; + S16 commandedPoistion = 0; + VALVE_POSITION_T commandedPositionEnum; + + // Get the position of the valves and update the structure of each valve + valvesStatus[ VDI ].currentPositionInCounts = getFPGAValveDialyzerInletPosition(); + valvesStatus[ VDO ].currentPositionInCounts = getFPGAValveDialyzerOutletPosition(); + valvesStatus[ VBA ].currentPositionInCounts = getFPGAValveBloodArterialPosition(); + valvesStatus[ VBV ].currentPositionInCounts = getFPGAValveBloodVenousPosition(); + + // Check the position of each valve + for ( i = 0; i < NUM_OF_VALVES; i++ ) + { + // Check the valve is in Idle state, otherwise the position should not be checked + if ( valvesStatus[ i ].execState == VALVE_STATE_IDLE ) + { + currentPostion = fabs( valvesStatus[ i ].currentPositionInCounts ); + commandedPositionEnum = valvesStatus[ i ].commandedPosition; + commandedPoistion = fabs( valvesStatus[ i ].positions[ commandedPositionEnum ] ); + + // 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 + if ( fabs( currentPostion - commandedPoistion ) > MAX_POS_DEVIATION_FROM_TARGET_COUNTS && + ++valvesStatus[ i ].valvePositionOutOfRangeCounter > OUT_OF_RANGE_COUNTER ) + { + // TODO fault + } + // If the deviation came back to within the maximum amount and the time counter was > 0, + // set the counter to 0 + else if ( fabs( currentPostion - commandedPoistion ) < MAX_POS_DEVIATION_FROM_TARGET_COUNTS && + valvesStatus[ i ].valvePositionOutOfRangeCounter > 0 ) + { + valvesStatus[ i ].valvePositionOutOfRangeCounter = 0; + } + } + } +} + +/*********************************************************************//** + * @brief + * The getPublishValvesDataInterval function gets the valves data publish \n + * interval + * @details + * Inputs: valvesDataPublishInterval + * Outputs: none + * @return: The data publish time interval + *************************************************************************/ +U32 getPublishValvesDataInterval( void ) +{ + U32 result = valvesDataPublishInterval.data; + + if ( OVERRIDE_KEY == valvesDataPublishInterval.override ) + { + result = valvesDataPublishInterval.ovData; + } + return result; } + +/*********************************************************************//** + * @brief + * The publishValvesData function publishes the data of the valves at the \n + * specified time interval + * @details + * Inputs: valvesDataPublishCounter + * Outputs: valvesDataPublishCounter + * @return: none + *************************************************************************/ +static void publishValvesData( void ) +{ + if ( ++valvesDataPublishCounter > getPublishValvesDataInterval() ) + { + broadcastHDValves(); + + valvesDataPublishCounter = 0; + } +} + +/**@}*/ + +