Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -r3477dd649f67442ad4bdea810ce5a251c1b0a2bf -r306243570641956ce4e2303380c9c0a02801726d --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision 3477dd649f67442ad4bdea810ce5a251c1b0a2bf) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 306243570641956ce4e2303380c9c0a02801726d) @@ -69,7 +69,7 @@ #define STEP_CHANGE_IN_COUNTS 1000 ///< Step change in counts -#define MAX_DEVIATION_FROM_TARGET_IN_COUNTS 150 ///< Maximum deviation from target in counts +#define MAX_DEVIATION_FROM_TARGET_IN_COUNTS 150 ///< Maximum deviation from target 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 @@ -80,6 +80,13 @@ // 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 VALVE_MAX_ALLOWED_PWM_PERCENT 90U ///< Valve maximum allowed PWM in percent +#define VALVE_MIN_ALLOWED_PWM_PERCENT 10U ///< Valve minimum allowed PWM in percent +#define VALVE_CW_PWM_TO_CNT_CONVERSION( pwm ) ( ( 22.222 * pwm ) + 2500 ) ///< Valve clockwise PWM to count conversion +#define VALVE_CW_CNT_TO_PWM_CONVERSION( cnt ) ( ( 0.045 * cnt ) - 112.5 ) ///< Valve clockwise count to PWM conversion +#define VALVE_CCW_PWM_TO_CNT_CONVERSION( pwm ) ( ( -22.222 * pwm ) + 2500 ) ///< Valve counter clockwise PWM to count conversion +#define VALVE_CCW_CNT_TO_PWM_CONVERSION( cnt ) ( ( -0.045 * cnt ) + 112.5 ) ///< Valve counter clockwise count to PWM conversion + // ********** private data ********** /// Exec valve self test states @@ -100,6 +107,7 @@ VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE, ///< Valve state homing find de-energized edge VALVE_STATE_IDLE, ///< Valve state idle VALVE_STATE_IN_TRANSITION, ///< Valve state in transition + VALVE_STATE_IN_BYPASS_MODE, ///< Valve state in bypass mode (Fixed PWM control) NUM_OF_VALVE_STATES, ///< Number of valve exec states } VALVE_STATE_T; @@ -122,7 +130,24 @@ NUM_OF_VALVE_CONTROL_STATUS ///< Number of valve control status }; +typedef enum valve_bypass_mode_direction +{ + CLOCKWISE = 0, ///< Clockwise direction in bypass mode + COUNTERCLOCKWISE, ///< Counter clockwise direction in bypass mode + NUM_OF_VALVE_DIRECTION ///< Number of valve directions +} VALVE_DIRECTION_T; + #pragma pack(push, 1) +/// Valve bypass (fixed PWM) structure +typedef struct +{ + BOOL hasBypassModeBeenRequeseted; ///< Valve bypass mode request flag + BOOL hasChangeBeenRequested; ///< Valve bypass mode change of direction or PWM flag + U16 currentPWMInPercent; ///< Valve current PWM in percent + U16 commandedPWMInPercent; ///< Valve commanded PWM in percent + VALVE_DIRECTION_T direction; ///< Valve move direction (CW or CCW) +} VALVE_BYPASS_T; + /// Valve status structure typedef struct { @@ -146,6 +171,8 @@ 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) @@ -186,17 +213,20 @@ static VALVE_STATE_T handleValveStateHomingFindDeenergizedEdge( VALVE_T valve ); static VALVE_STATE_T handleValveStateIdle( VALVE_T valve ); static VALVE_STATE_T handleValveStateInTransition( VALVE_T valve ); +static VALVE_STATE_T handleValveStateInBypassMode( VALVE_T valve ); // Private function prototypes static void setValveControlMode( VALVE_T valve, VALVE_MODE_T mode ); static void execMonitorValves( void ); static BOOL areValvesFunctional( void ); -static void setFPGAValue( VALVE_T valve, S16 position, BOOL enableCurrentRelaxation ); +static void setFPGAValveSetPoint( VALVE_T valve, S16 position, BOOL enableCurrentRelaxation ); static void convertAndMonitorValvesCurrent( void ); static void getAndMonitorValvesCurrentPosition( void ); static DATA_GET_PROTOTYPE( U32, getPublishValvesDataInterval ); static void publishValvesData( VALVE_T valve ); static void setValveNextStep( VALVE_T valve ); +static void setFPGAValvePWM( VALVE_T valve ); +static void getValvesCurrentPWM( void ); /*********************************************************************//** * @brief @@ -326,7 +356,8 @@ break; default: - //TODO alarm + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_HD_VALVES_INVALID_SELF_TEST_STATE, + valveSelfTestState ); valveSelfTestState = VALVE_SELF_TEST_COMPLETE; } @@ -358,36 +389,37 @@ switch ( valvesStatus[ valve ].execState ) { case VALVE_STATE_WAIT_FOR_POST: - state = handleValveStateWaitForPost( valve ); break; case VALVE_STATE_HOMING_NOT_STARTED: - state = handleValveStateHomingNotStarted( valve ); break; case VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE: - state = handleValveStateHomingFindEnergizedEdge( valve ); break; case VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE: - state = handleValveStateHomingFindDeenergizedEdge( valve ); break; case VALVE_STATE_IDLE: - state = handleValveStateIdle( valve ); break; case VALVE_STATE_IN_TRANSITION: - state = handleValveStateInTransition( valve ); break; + case VALVE_STATE_IN_BYPASS_MODE: + state = handleValveStateInBypassMode( valve ); + break; + 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; break; } @@ -501,7 +533,7 @@ valvesStatus[ valve ].targetPositionInCounts = valvesStatus[ valve ].currentPositionInCounts + STEP_CHANGE_IN_COUNTS; valvesStatus[ valve ].hasValveBeenHomed = FALSE; valvesStatus[ valve ].currentPosition = VALVE_POSITION_NOT_IN_POSITION; - setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts, FALSE ); + setFPGAValveSetPoint( valve, valvesStatus[ valve ].targetPositionInCounts, FALSE ); state = VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE; } @@ -536,7 +568,7 @@ valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ] = currentPosition; valvesStatus[ valve ].homingEdgeDetectionCounter = 0; valvesStatus[ valve ].targetPositionInCounts = currentPosition - STEP_CHANGE_IN_COUNTS; - setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts, FALSE ); + setFPGAValveSetPoint( valve, valvesStatus[ valve ].targetPositionInCounts, FALSE ); state = VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE; } @@ -546,7 +578,7 @@ { valvesStatus[ valve ].homingEdgeDetectionCounter = 0; valvesStatus[ valve ].targetPositionInCounts = currentPosition + STEP_CHANGE_IN_COUNTS; - setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts, FALSE ); + setFPGAValveSetPoint( valve, valvesStatus[ valve ].targetPositionInCounts, FALSE ); } } @@ -603,10 +635,10 @@ // Max number of failed homing. Fault and go back to homing not started else if ( valvesStatus[ valve ].numberOfFailedHomings >= MAX_ALLOWED_FAILED_HOMINGS ) { - //TODO Alarm 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 { @@ -621,7 +653,7 @@ { valvesStatus[ valve ].homingEdgeDetectionCounter = 0; valvesStatus[ valve ].targetPositionInCounts = currentPosition - STEP_CHANGE_IN_COUNTS; - setFPGAValue( valve, valvesStatus[ valve ].targetPositionInCounts, FALSE ); + setFPGAValveSetPoint( valve, valvesStatus[ valve ].targetPositionInCounts, FALSE ); } } @@ -658,7 +690,20 @@ state = VALVE_STATE_IN_TRANSITION; } } + // TODO add build switch here + // Check if the valves have been homed and a bypass mode has been requested + else if ( valvesStatus[ valve ].hasValveBeenHomed && + valvesStatus[ valve ].bypassModeStatus.hasBypassModeBeenRequeseted ) + { + // Set the valve control mode to bypass and set the PWM + setValveControlMode( valve, VALVE_CONTORL_MODE_ENABLE_BYPASS ); + valvesStatus[ valve ].controlMode = VALVE_CONTORL_MODE_ENABLE_BYPASS; + valvesStatus[ valve ].bypassModeStatus.hasChangeBeenRequested = FALSE; + setFPGAValvePWM( valve ); + state = VALVE_STATE_IN_BYPASS_MODE; + } + return state; } @@ -693,7 +738,7 @@ if ( commandedPositionEnum == VALVE_POSITION_B_OPEN ) { - setFPGAValue( valve, currentPosition, TRUE ); + setFPGAValveSetPoint( valve, currentPosition, TRUE ); } // Go back to Idle state @@ -702,10 +747,10 @@ // 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; + SET_ALARM_WITH_1_U32_DATA( ALARM_ID_HD_VALVE_TRANSITION_TIMEOUT, (U32)valve ); } else if ( currentPosition - targetPosition < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) { @@ -717,6 +762,53 @@ /*********************************************************************//** * @brief + * The handleValveStateInBypassMode function handles the bypass mode + * state of the state machine. + * @details + * Inputs: valvesStatus + * Outputs: valvesStatus + * @param valve that is in position transition + * @return next state of the exec state machine + *************************************************************************/ +static VALVE_STATE_T handleValveStateInBypassMode( VALVE_T valve ) +{ + VALVE_STATE_T state = VALVE_STATE_IN_BYPASS_MODE; + + // Get the positions counts + S16 currentPos = valvesStatus[ valve ].currentPositionInCounts; + S16 posAInCounts = valvesStatus[ valve ].positions[ VALVE_POSITION_B_OPEN ]; + S16 posCInCounts = valvesStatus[ valve ].positions[ VALVE_POSITION_C_CLOSE ]; + + // If bypass mode has been turned off + if ( valvesStatus[ valve ].bypassModeStatus.hasBypassModeBeenRequeseted == FALSE ) + { + // Change the control mode to PID + setValveControlMode( valve, VALVE_CONTROL_MODE_ENABLE_PID ); + valvesStatus[ valve ].controlMode = VALVE_CONTROL_MODE_ENABLE_PID; + // Set the PWM to 0% and set it + valvesStatus[ valve ].bypassModeStatus.commandedPWMInPercent = 0; + setFPGAValvePWM( valve ); + + state = VALVE_STATE_IDLE; + } + // If a valve change either a change in CW or CCW or PWM has changed, set the new valve + else if ( valvesStatus[ valve ].bypassModeStatus.hasChangeBeenRequested ) + { + setFPGAValvePWM( valve ); + } + // If the current position is close to either pos B or pos C, stop the valve from moving + if( abs( currentPos - posAInCounts ) < MAX_DEVIATION_FROM_TARGET_IN_COUNTS || + abs( currentPos - posCInCounts ) < MAX_DEVIATION_FROM_TARGET_IN_COUNTS ) + { + valvesStatus[ valve ].bypassModeStatus.commandedPWMInPercent = 0; + setFPGAValvePWM( valve ); + } + + return state; +} + +/*********************************************************************//** + * @brief * The setValveControlMode function sets the valves control mode. * @details * Inputs: valvesControlSetBits, valvesControlModesSetBits, @@ -761,6 +853,11 @@ // Get the current in ADC and convert them to amps // Check whether any of the valves are over current convertAndMonitorValvesCurrent(); + + // Get the current PWM values back. + // This function is only available if the valves are in bypass mode + //TODO add build switch + getValvesCurrentPWM(); } /*********************************************************************//** @@ -775,7 +872,6 @@ *************************************************************************/ static BOOL areValvesFunctional( void ) { - //TODO alarm for the valve status VALVE_T valve; VALVE_MODE_T mode; BOOL result = TRUE; @@ -799,24 +895,28 @@ { if ( ! ( status & valvesControlStatusBits[ valve ][ VALVE_CONTROL_STATUS_INITIALIZED ] ) ) { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_VALVE_NOT_FUNCTIONAL, (U32)valve, (U32)mode ); result = FALSE; } if ( ! ( status & valvesControlStatusBits[ valve ][ VALVE_CONTROL_STATUS_PID_ENABLED ] ) ) { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_VALVE_NOT_FUNCTIONAL, (U32)valve, (U32)mode ); result = FALSE; } } else if ( mode == VALVE_CONTORL_MODE_ENABLE_BYPASS ) { if ( ! ( status & valvesControlStatusBits[ valve ][ VALVE_CONTROL_STATUS_BYPASS_ENABLED ] ) ) { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_VALVE_NOT_FUNCTIONAL, (U32)valve, (U32)mode ); result = FALSE; } } else if ( mode == VALVE_CONTROL_MODE_DISABLE_ALL ) { 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; } } @@ -837,7 +937,7 @@ * should be enabled for the valve * @return none *************************************************************************/ -static void setFPGAValue( VALVE_T valve, S16 position, BOOL enableCurrentRelaxation ) +static void setFPGAValveSetPoint( VALVE_T valve, S16 position, BOOL enableCurrentRelaxation ) { // Always set the MSB of the S16 to 0 position &= DISABLE_VALVE_CURRENT_RELAXATION; @@ -850,27 +950,23 @@ switch ( valve ) { case VDI: - setFPGAValveDialyzerInletPosition( position ); break; case VDO: - setFPGAValveDialyzerOutletPosition( position ); break; case VBA: - setFPGAValveBloodArterialPosition( position ); break; case VBV: - setFPGAValveBloodVenousPosition( position ); break; default: - // TODO alarm + // TODO alarm or ignore it? break; } } @@ -915,7 +1011,7 @@ // Check if the current is over the threshold for the defined amount of time if ( valvesStatus[ valve ].overCurrentCounter > MAX_OVER_CURRENT_TIME_INTERVAL_COUNTER ) { - //TODO fault + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_VALVE_CURRENT_OUT_OF_RANGE, (F32)valve, current ); } // If the current is below the threshold again and the counter for the time is greater than else if ( current < VALVES_CURRENT_THRESHOLD_AMPS && @@ -971,7 +1067,7 @@ if ( valvesStatus[ valve ].positionOutOfRangeCounter > MAX_POS_DEVIATION_TIME_INTERVAL_COUNTER ) { - //TODO fault + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_VALVE_CURRENT_OUT_OF_RANGE, (F32)valve, currentPostion ); //TODO is S16 ok with F32? } else { @@ -1119,9 +1215,100 @@ // Call the function to send the set point to FPGA. Current relaxation is not // enabled here, so it is always false - setFPGAValue( valve, targetPosition, FALSE ); + setFPGAValveSetPoint( valve, targetPosition, FALSE ); } +/*********************************************************************//** + * @brief + * The setFPGAValvePWM function checks whether the valve should be moving + * CW or CCW and converts the PWM in percent to PWM in counts accordingly. + * The converted PWM in counts is set via the FPGA functions. + * @details + * Inputs: valvesStatus + * Outputs: none + * @param valve which its PWM is being set + * @return none + *************************************************************************/ +static void setFPGAValvePWM( VALVE_T valve ) +{ + U16 pwmInCounts = 0; + U16 commandedPWMPct = valvesStatus[ valve ].bypassModeStatus.commandedPWMInPercent; + + // Check the requested direction and convert PWM in percent to counts + if ( valvesStatus[ valve ].bypassModeStatus.direction == CLOCKWISE ) + { + pwmInCounts = VALVE_CW_PWM_TO_CNT_CONVERSION( commandedPWMPct ); + } + else + { + pwmInCounts = VALVE_CCW_PWM_TO_CNT_CONVERSION( commandedPWMPct ); + } + + switch ( valve ) + { + case VDI: + + setFPGAValveDialyzerInletPWM( pwmInCounts ); + break; + + case VDO: + + setFPGAValveDialyzerOutletPWM( pwmInCounts ); + break; + + case VBA: + + setFPGAValveBloodVenousPWM( pwmInCounts ); + break; + + case VBV: + + setFPGAValveBloodArterialPWM( pwmInCounts ); + break; + + default: + // TODO alarm or ignore? + break; + } +} + +/*********************************************************************//** + * @brief + * The getValvesCurrentPWM function gets the PWM counts from the FPGA module + * and converts them in to duty cycle in percent. + * @details + * Inputs: valvesStatus + * Outputs: valvesStatus + * @return none + *************************************************************************/ +static void getValvesCurrentPWM( void ) +{ + VALVE_T valve; + U16 currentPWM = 0; + + // Get all the PWMs in count + U16 pwmInCounts[ NUM_OF_VALVES ] = { getFPGAValveDialyzerInletPWM(), getFPGAValveDialyzerOutletPWM(), + getFPGAValveBloodArterialPWM(), getFPGAValveBloodVenousPWM() }; + + for ( valve = VDI; valve < NUM_OF_VALVES; valve++ ) + { + // Get the count + currentPWM = pwmInCounts[ valve ]; + + // Check if the direction is clockwise or not to use the right conversion equation + if ( valvesStatus[ valve ].bypassModeStatus.direction == CLOCKWISE ) + { + currentPWM = VALVE_CW_CNT_TO_PWM_CONVERSION( currentPWM ); + } + else + { + currentPWM = VALVE_CCW_CNT_TO_PWM_CONVERSION( currentPWM ); + } + + valvesStatus[ valve ].bypassModeStatus.currentPWMInPercent = currentPWM * 100; + } +} + /************************************************************************* * TEST SUPPORT FUNCTIONS *************************************************************************/ @@ -1230,14 +1417,69 @@ return result; } -BOOL testSetValveControlMode( U32 valve, U32 mode ) +/*********************************************************************//** + * @brief + * The testSetValvePWMOverride function overrides the valves PWM in bypass + * control mode. + * @details + * Inputs: valvesStatus + * Outputs: valvesStatus + * @param valve to override its PWM + * @param pwm in percent + * @param direction of the movement of the valve in clockwise or counter + * clockwise + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +BOOL testSetValvePWMOverride( U32 valve, U32 pwm, U32 direction ) { + BOOL result = FALSE; + if ( TRUE == isTestingActivated() ) + { + // Check if the valve is in the range of the valves available + if ( valve < NUM_OF_VALVES ) + { + // The PWM should be in range and also the valve direction should be legal + if ( pwm >= VALVE_MIN_ALLOWED_PWM_PERCENT && pwm <= VALVE_MAX_ALLOWED_PWM_PERCENT && direction < NUM_OF_VALVE_DIRECTION ) + { + valvesStatus[ (VALVE_T)valve ].bypassModeStatus.commandedPWMInPercent = (U16)pwm; + valvesStatus[ (VALVE_T)valve ].bypassModeStatus.direction = (VALVE_DIRECTION_T)direction; + valvesStatus[ (VALVE_T)valve ].bypassModeStatus.hasBypassModeBeenRequeseted = TRUE; + valvesStatus[ (VALVE_T)valve ].bypassModeStatus.hasChangeBeenRequested = TRUE; + } + + result = TRUE; + } + } + + return result; } -BOOL testResetValveControlMode( U32 valve ) +/*********************************************************************//** + * @brief + * The testResetValvePWMOverride function resets the override PWM in bypass + * control mode. + * @details + * Inputs: valvesStatus + * Outputs: valvesStatus + * @param valve that its data publish will be overridden + * @return TRUE if override reset successful, FALSE if not + *************************************************************************/ +BOOL testResetValvePWMOverride( U32 valve ) { + BOOL result = FALSE; + // Check if the valve is in the range of the valves available + if ( valve < NUM_OF_VALVES ) + { + // Turn off the bypass mode request and the change request + valvesStatus[ (VALVE_T)valve ].bypassModeStatus.hasBypassModeBeenRequeseted = FALSE; + valvesStatus[ (VALVE_T)valve ].bypassModeStatus.hasChangeBeenRequested = FALSE; + + result = TRUE; + } + + return result; } /**@}*/