Index: firmware/App/Controllers/DialysatePumps.c =================================================================== diff -u -rf8bb43e62a4d10b64d8b197e372a56adfcfb88b9 -r60165f6d155831cfce06560fb45abaaaadea9798 --- firmware/App/Controllers/DialysatePumps.c (.../DialysatePumps.c) (revision f8bb43e62a4d10b64d8b197e372a56adfcfb88b9) +++ firmware/App/Controllers/DialysatePumps.c (.../DialysatePumps.c) (revision 60165f6d155831cfce06560fb45abaaaadea9798) @@ -17,6 +17,7 @@ #include +#include "BalancingChamber.h" #include "DialysatePumps.h" #include "FpgaDD.h" #include "MessageSupport.h" @@ -38,9 +39,16 @@ */ // ********** private definitions ********** - +#ifndef __OPENSPDTEST__ #define DIALYSATE_PUMP_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) ///< Interval (ms/task time) at which the dialysate pump data is published on the CAN bus. +#else +#define DIALYSATE_PUMP_DATA_PUB_INTERVAL ( 50 / TASK_PRIORITY_INTERVAL ) ///< Interval (ms/task time) at which the dialysate pump data is published on the CAN bus. +#endif #define DATA_PUBLISH_COUNTER_START_COUNT 60 ///< Data publish counter start count. +#if __OPENSPDTEST__ +#define DIALYSATE_PUMP_EXEC_INTERVAL ( 2000 / TASK_GENERAL_INTERVAL ) +#define DIALYSATE_PUMP_SPEED_INCREMENT 200.0F ///< Speed increase when controlling dialysate pump to target step speed. +#endif #define DP_CONTROL_INTERVAL ( 1000 / TASK_GENERAL_INTERVAL ) ///< Interval (ms/task time) at which the dialysate pump is controlled. #define DIALYSATE_PUMP_CONTROL_RUN 0x01 ///< Dialysate pump control run pump. @@ -60,20 +68,20 @@ #define MAX_FPGA_DIALYSATE_PUMP_DIRECTION_FAULT_WINDOW_MS ( 1 * SEC_PER_MIN * MS_PER_SECOND ) ///< FPGA dialysate pump direction fault window #define MAX_FPGA_DIALYSATE_PUMP_DIRECTION_FAULT_FAILURES 10 ///< FPGA dialysate pump direction fault failures per MAX_FPGA_DIALYSATE_PUMP_DIRECTION_FAULT_WINDOW_MS -#define FRESH_DIALYSATE_TARGET_PRESSURE_PSI (-12.0F) ///< Fresh dialysate pump recommended pressure(Pn/PHo) in psi. -#define FRESH_DIALYSATE_MAX_PRESSURE_PSI (-15.0F) ///< Fresh dialysate pump maximum allowed (Pn/PHo) pressure in psi. -#define SPENT_DIALYSATE_TARGET_PRESSURE_PSI 25.0F ///< Spent dialysate pump recommended pressure(PDs) in psi. +#define FRESH_DIALYSATE_TARGET_PRESSURE_PSI 25.0F ///< Fresh dialysate pump recommended pressure(D18) in psi. +#define FRESH_DIALYSATE_MAX_PRESSURE_PSI 26.0F ///< Fresh dialysate pump maximum allowed (D18) pressure in psi. +#define SPENT_DIALYSATE_TARGET_PRESSURE_PSI 29.0F ///< Spent dialysate pump recommended pressure(PDs) in psi. #define SPENT_DIALYSATE_MAX_PRESSURE_PSI 30.0F ///< Spent dialysate pump maximum allowed (PDs) pressure in psi. -#define DIALYSATE_PUMP_MIN_PRESSURE_PSI 0.0F ///< Minimum dialysate pump pressure in psi. -#define DIALYSATE_PUMP_MAX_PRESSURE_PSI 40.0F ///< Maximum dialysate pump pressure in psi. +#define FRESH_DIAL_OPEN_LOOP_SPEED_RPM 2000.0F ///< fresh dialysate pump open loop speed to meet target pressure. +#define SPENT_DIAL_OPEN_LOOP_SPEED_RPM 2500.0F ///< spent dialysate pump open loop speed to meet target pressure. #define DIALYSATE_PUMP_SPEED_ZERO_RPM 0 ///< Dialysate pump zero RPM speed. #define SPEED_COUNT_ZERO 0 ///< Measured speed count zero check. #define SPEED_CONV_FACTOR 1500000 ///< Measured speed count conversion to RPM -#define D12_PUMP_P_COEFFICIENT 1.0F ///< P term for fresh dialysate pump delta pressure control. -#define D12_PUMP_I_COEFFICIENT 0.0F ///< I term for fresh dialysate pump delta pressure control. -#define D48_PUMP_P_COEFFICIENT 1.0F ///< P term for spent dialysate pump delta pressure control. -#define D48_PUMP_I_COEFFICIENT 0.0F ///< I term for spent dialysate pump delta pressure control. +#define D12_PUMP_P_COEFFICIENT 20.0F ///< P term for fresh dialysate pump delta pressure control. +#define D12_PUMP_I_COEFFICIENT 60.0F ///< I term for fresh dialysate pump delta pressure control. +#define D48_PUMP_P_COEFFICIENT 20.0F ///< P term for spent dialysate pump delta pressure control. +#define D48_PUMP_I_COEFFICIENT 60.0F ///< I term for spent dialysate pump delta pressure control. #define MAX_ALLOWED_RPM_OUT_OF_RANGE 300 ///< Maximum allowed RPM out of range from target RPM in open loop. #define PUMP_TRANS_TO_RAMP_SPEED_THRESHOLD 20.0F ///< Speed change that alters the state to ramp during control state. @@ -133,8 +141,18 @@ static OVERRIDE_F32_T dialysatePumpMeasuredCurrentA[ NUM_OF_DIALYSATE_PUMPS ]; ///< Measured dialysate pump current feedback. static OVERRIDE_U32_T measuredDirection[ NUM_OF_DIALYSATE_PUMPS ]; ///< Measured dialysate pump direction. static OVERRIDE_F32_T pumpTargetPressure[ NUM_OF_DIALYSATE_PUMPS ]; ///< Target dialysate pumps' pressure (PSI). +static BOOL dialPumpsReadyToControl; ///< Flag use to indicate the minimum control interval is met for pump control -//TODO : Validate once HDD defines the conversion. +//Testing +#if __OPENSPDTEST__ +static U32 dialysatePumpExecTimerCounter; ///< Used to schedule dialysate pump data publication to CAN bus. +#endif + +//For testing +#ifdef __PITEST__ +static F32 pIControlSignal[ NUM_OF_CONTROLLER_SIGNAL ]; +#endif + static const F32 CURRENT_CONVERSION_COEFF = (F32)( 2.5F / ( BITS_12_FULL_SCALE - 1.0F ) ); // ********** private function prototypes ********** @@ -156,7 +174,15 @@ void initDialysatePump( void ) { DIALYSATE_PUMPS_T pumpId; +//Testing +#if __OPENSPDTEST__ + dialysatePumpExecTimerCounter = 9900; +#endif +#ifdef __PITEST__ + U32 i; +#endif + dialysatePumpDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; for ( pumpId = DIALYSATE_PUMPS_FIRST; pumpId < NUM_OF_DIALYSATE_PUMPS; pumpId++ ) @@ -185,6 +211,7 @@ dialysatePumps[ pumpId ].control = DIALYSATE_PUMP_CONTROL_STOP; dialysatePumps[ pumpId ].directionErrorCount = 0; dialysatePumps[ pumpId ].lastDirectionErrorCount = 0; + dialPumpsReadyToControl = FALSE; signalDialysatePumpHardStop( pumpId ); } @@ -199,18 +226,25 @@ pumpTargetPressure[D48_PUMP].override = OVERRIDE_RESET; // Initialize the fresh dialysate pump PI controller - initializePIController( PI_CONTROLLER_ID_D12_PUMP, DIALYSATE_PUMP_MIN_PRESSURE_PSI, D12_PUMP_P_COEFFICIENT, D12_PUMP_I_COEFFICIENT, - DIALYSATE_PUMP_MIN_PRESSURE_PSI, DIALYSATE_PUMP_MAX_PRESSURE_PSI ); + initializePIController( PI_CONTROLLER_ID_D12_PUMP, FRESH_DIAL_OPEN_LOOP_SPEED_RPM, D12_PUMP_P_COEFFICIENT, D12_PUMP_I_COEFFICIENT, + MIN_DIALYSATE_PUMP_RPM, MAX_DIALYSATE_PUMP_RPM ); // Initialize spent dialysate pump PI controller - initializePIController( PI_CONTROLLER_ID_D48_PUMP, DIALYSATE_PUMP_MIN_PRESSURE_PSI, D48_PUMP_P_COEFFICIENT, D48_PUMP_I_COEFFICIENT, - DIALYSATE_PUMP_MIN_PRESSURE_PSI, DIALYSATE_PUMP_MAX_PRESSURE_PSI ); + initializePIController( PI_CONTROLLER_ID_D48_PUMP, SPENT_DIAL_OPEN_LOOP_SPEED_RPM, D48_PUMP_P_COEFFICIENT, D48_PUMP_I_COEFFICIENT, + MIN_DIALYSATE_PUMP_RPM, MAX_DIALYSATE_PUMP_RPM ); // Init the dialysate pump with valid PWM while motor is disabled. // when enable triggers, we dont want invlid RPM set that triggers alarm in motor controller. setFPGAD12PumpSpeed( MIN_DIALYSATE_PUMP_RPM ); setFPGAD48PumpSpeed( MIN_DIALYSATE_PUMP_RPM ); +#ifdef __PITEST__ + for ( i = 0; i < NUM_OF_CONTROLLER_SIGNAL; i++ ) + { + pIControlSignal[ i ] = 0.0F; + } +#endif + // Initialize the persistent alarms for fresh dialysate pump initPersistentAlarm( ALARM_ID_DD_D12_PUMP_RPM_OUT_OF_RANGE, RPM_OUT_OF_RANGE_TIME_OUT, RPM_OUT_OF_RANGE_TIME_OUT ); initPersistentAlarm( ALARM_ID_DD_D12_PUMP_OFF_FAULT, SAFETY_SHUTDOWN_TIMEOUT, SAFETY_SHUTDOWN_TIMEOUT ); @@ -317,11 +351,11 @@ //Reset PI Controller if ( D12_PUMP == pumpId ) { - resetPIController( PI_CONTROLLER_ID_D12_PUMP, DIALYSATE_PUMP_MIN_PRESSURE_PSI ); + resetPIController( PI_CONTROLLER_ID_D12_PUMP, FRESH_DIAL_OPEN_LOOP_SPEED_RPM ); } else { - resetPIController( PI_CONTROLLER_ID_D48_PUMP, DIALYSATE_PUMP_MIN_PRESSURE_PSI ); + resetPIController( PI_CONTROLLER_ID_D48_PUMP, SPENT_DIAL_OPEN_LOOP_SPEED_RPM ); } } @@ -525,6 +559,10 @@ for ( pumpId = DIALYSATE_PUMPS_FIRST; pumpId < NUM_OF_DIALYSATE_PUMPS; pumpId++ ) { +#if __OPENSPDTEST__ + if ( ++dialysatePumpExecTimerCounter >= DIALYSATE_PUMP_EXEC_INTERVAL ) + { +#endif switch ( dialysatePumps[ pumpId ].dialysatePumpState ) { case DIALYSATE_PUMP_OFF_STATE: @@ -544,6 +582,10 @@ dialysatePumps[ pumpId ].dialysatePumpState = DIALYSATE_PUMP_OFF_STATE; break; } +#if __OPENSPDTEST__ + dialysatePumpExecTimerCounter = 0; + } +#endif } } @@ -650,11 +692,11 @@ { if ( D12_PUMP == pumpId ) { - resetPIController( PI_CONTROLLER_ID_D12_PUMP, DIALYSATE_PUMP_MIN_PRESSURE_PSI ); + resetPIController( PI_CONTROLLER_ID_D12_PUMP, FRESH_DIAL_OPEN_LOOP_SPEED_RPM ); } else { - resetPIController( PI_CONTROLLER_ID_D48_PUMP, DIALYSATE_PUMP_MIN_PRESSURE_PSI ); + resetPIController( PI_CONTROLLER_ID_D48_PUMP, SPENT_DIAL_OPEN_LOOP_SPEED_RPM ); } state = DIALYSATE_PUMP_CONTROL_TO_TARGET_STATE; } @@ -681,7 +723,12 @@ { if ( currentToTargetDiff > DIALYSATE_PUMP_RAMP_SPEED_INCREMENT ) { +//Testing +#ifndef __OPENSPDTEST__ speedIncrease = DIALYSATE_PUMP_RAMP_SPEED_INCREMENT; +#else + speedIncrease = DIALYSATE_PUMP_SPEED_INCREMENT; +#endif } else { @@ -733,69 +780,56 @@ { DIALYSATE_PUMP_STATE_T state = DIALYSATE_PUMP_CONTROL_TO_TARGET_STATE; - // control at set interval - if ( ++dialysatePumps[ pumpId ].controlTimerCounter >= DP_CONTROL_INTERVAL ) + // control at set minimum interval or interval is expired and balance chamber fill is complete + if ( ( ++dialysatePumps[ pumpId ].controlTimerCounter >= DP_CONTROL_INTERVAL ) || + ( TRUE == dialPumpsReadyToControl ) ) { - // Control based on the measured and target pressure - if ( D12_PUMP == pumpId ) + dialysatePumps[ pumpId ].controlTimerCounter = 0; + dialPumpsReadyToControl = TRUE; + + // Control happen only when balancing chamber fill is complete. + if ( FALSE == getBalancingChamberFillinProgressStatus() ) { - F32 measuredPressure = getFilteredPressure( D18_PRES ); - F32 targetPressure = getDialysatePumpTargetPressure( pumpId ); - F32 newSpeed = 0.0F; + dialPumpsReadyToControl = FALSE; - //F32 control = runPIController( PI_CONTROLLER_ID_D12_PUMP, targetPressure, measuredPressure ); - // TODO : get the equivalent speed for the given control - // newSpeed = control * ( MAX_DIALYSATE_PUMP_RPM / DIALYSATE_PUMP_MAX_PRESSURE_PSI ); - //TODO: Below code is placeholder to have minimal close loop control, to be reevaluated. -#if 0 - if ( measuredPressure < targetPressure ) + // Control based on the measured and target pressure + if ( D12_PUMP == pumpId ) { - newSpeed = dialysatePumps[ pumpId ].currentPumpSpeed + DIALYSATE_PUMP_RAMP_SPEED_INCREMENT; - } - else - { - newSpeed = dialysatePumps[ pumpId ].currentPumpSpeed - DIALYSATE_PUMP_RAMP_SPEED_INCREMENT; - } + F32 measuredPressure = getFilteredPressure( D18_PRES ); + F32 targetPressure = getDialysatePumpTargetPressure( pumpId ); - //Apply speed limit - newSpeed = MIN( newSpeed, MAX_DIALYSATE_PUMP_RPM ); - newSpeed = MAX( newSpeed, MIN_DIALYSATE_PUMP_RPM ); + F32 control = runPIController( PI_CONTROLLER_ID_D12_PUMP, targetPressure, measuredPressure ); +#ifdef __PITEST__ + U32 i; - dialysatePumps[ pumpId ].currentPumpSpeed = newSpeed; - //Set fresh dialyate pump speed - setFPGAD12PumpSpeed( (U16)dialysatePumps[ pumpId ].currentPumpSpeed ); + for ( i = 0; i < NUM_OF_CONTROLLER_SIGNAL; i++ ) + { + pIControlSignal[ i ] = getPIControllerSignals( PI_CONTROLLER_ID_D12_PUMP, (PI_CONTROLLER_SIGNALS_ID)i ); + } #endif - } - else - { - F32 measuredPressure = getFilteredPressure( D51_PRES ); - F32 targetPressure = getDialysatePumpTargetPressure( pumpId ); - F32 newSpeed = 0.0F; - - //F32 control = runPIController( PI_CONTROLLER_ID_D48_PUMP, targetPressure, measuredPressure ); - // TODO : get the equivalent speed for the given control - // newSpeed = control * ( MAX_DIALYSATE_PUMP_RPM / DIALYSATE_PUMP_MAX_PRESSURE_PSI ); - //TODO: Below code is placeholder to have minimal close loop control, to be reevaluated. -#if 0 - if ( measuredPressure < targetPressure ) - { - newSpeed = dialysatePumps[ pumpId ].currentPumpSpeed + DIALYSATE_PUMP_RAMP_SPEED_INCREMENT; + dialysatePumps[ pumpId ].currentPumpSpeed = control; + //Set fresh dialyate pump speed + setFPGAD12PumpSpeed( (U16)dialysatePumps[ pumpId ].currentPumpSpeed ); } else { - newSpeed = dialysatePumps[ pumpId ].currentPumpSpeed - DIALYSATE_PUMP_RAMP_SPEED_INCREMENT; - } + F32 measuredPressure = getFilteredPressure( D51_PRES ); + F32 targetPressure = getDialysatePumpTargetPressure( pumpId ); - //Apply speed limit - newSpeed = MIN( newSpeed, MAX_DIALYSATE_PUMP_RPM ); - newSpeed = MAX( newSpeed, MIN_DIALYSATE_PUMP_RPM ); + F32 control = runPIController( PI_CONTROLLER_ID_D48_PUMP, targetPressure, measuredPressure ); +#ifdef __PITEST__ + U32 i; - dialysatePumps[ pumpId ].currentPumpSpeed = newSpeed; - //Set spent dialyate pump speed - setFPGAD48PumpSpeed( (U16)dialysatePumps[ pumpId ].currentPumpSpeed ); + for ( i = 0; i < NUM_OF_CONTROLLER_SIGNAL; i++ ) + { + pIControlSignal[ i ] = getPIControllerSignals( PI_CONTROLLER_ID_D48_PUMP, (PI_CONTROLLER_SIGNALS_ID)i ); + } #endif + dialysatePumps[ pumpId ].currentPumpSpeed = control; + //Set spent dialyate pump speed + setFPGAD48PumpSpeed( (U16)dialysatePumps[ pumpId ].currentPumpSpeed ); + } } - dialysatePumps[ pumpId ].controlTimerCounter = 0; } return state; @@ -947,20 +981,35 @@ DIALYSATE_PUMPS_PUBLISH_DATA_T dialPump; // Populate the data structure for publication +#ifndef __PITEST__ dialPump.d12PumpTargetRPM = getDialysatePumpTargetSpeed( D12_PUMP ); dialPump.d48PumpTargetRPM = getDialysatePumpTargetSpeed( D48_PUMP ); +#else + dialPump.d12PumpTargetRPM = pIControlSignal[ 0 ]; + dialPump.d48PumpTargetRPM = pIControlSignal[ 1 ]; +#endif + dialPump.d12PumpMeasuredSpeed = getDialysatePumpMeasuredSpeed( D12_PUMP ); dialPump.d48PumpMeasuredSpeed = getDialysatePumpMeasuredSpeed( D48_PUMP ); dialPump.d12PumpCurrentSpeed = dialysatePumps[ D12_PUMP ].currentPumpSpeed; dialPump.d48PumpCurrentSpeed = dialysatePumps[ D48_PUMP ].currentPumpSpeed; dialPump.d12PumpState = (U32)dialysatePumps[ D12_PUMP ].dialysatePumpState; dialPump.d48PumpState = (U32)dialysatePumps[ D48_PUMP ].dialysatePumpState; +#ifndef __PITEST__ dialPump.d12PumpTargetPressure = getDialysatePumpTargetPressure( D12_PUMP ); dialPump.d48PumpTargetPressure = getDialysatePumpTargetPressure( D48_PUMP ); dialPump.d12PumpMeasuredPressure = getFilteredPressure( D18_PRES ); dialPump.d48PumpMeasuredPressure = getFilteredPressure( D51_PRES ); dialPump.d12PumpMeasuredCurrent = getDialysatePumpMeasuredCurrentA( D12_PUMP ); dialPump.d48PumpMeasuredCurrent = getDialysatePumpMeasuredCurrentA( D48_PUMP ); +#else + dialPump.d12PumpTargetPressure = pIControlSignal[ 2 ]; + dialPump.d48PumpTargetPressure = pIControlSignal[ 3 ]; + dialPump.d12PumpMeasuredPressure = pIControlSignal[ 4 ]; + dialPump.d48PumpMeasuredPressure = pIControlSignal[ 5 ]; + dialPump.d12PumpMeasuredCurrent = pIControlSignal[ 6 ]; + dialPump.d48PumpMeasuredCurrent = pIControlSignal[ 7 ]; +#endif dialPump.d12PumpControl = (U32)dialysatePumps[ D12_PUMP ].control; dialPump.d48PumpControl = (U32)dialysatePumps[ D48_PUMP ].control; dialPump.d12PumpDirErrCnt = dialysatePumps[ D12_PUMP ].directionErrorCount;