Index: firmware/App/Controllers/DialysatePumps.c =================================================================== diff -u -rab7c459d5cd92f74babd8ca1ce8aa47354570728 -re12eefac42a3806a34f3ddac6df30bbb5ca51f6f --- firmware/App/Controllers/DialysatePumps.c (.../DialysatePumps.c) (revision ab7c459d5cd92f74babd8ca1ce8aa47354570728) +++ firmware/App/Controllers/DialysatePumps.c (.../DialysatePumps.c) (revision e12eefac42a3806a34f3ddac6df30bbb5ca51f6f) @@ -17,6 +17,7 @@ #include +#include "BalancingChamber.h" #include "DialysatePumps.h" #include "FpgaDD.h" #include "MessageSupport.h" @@ -38,16 +39,14 @@ */ // ********** private definitions ********** -//TODO : Testing - changing control interval -//#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. -#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. +#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. #define DATA_PUBLISH_COUNTER_START_COUNT 60 ///< Data publish counter start count. -#define DP_CONTROL_INTERVAL ( 1000 / TASK_GENERAL_INTERVAL ) ///< Interval (ms/task time) at which the dialysate pump is controlled. -//#define DP_CONTROL_INTERVAL ( 250 / TASK_GENERAL_INTERVAL ) ///< Interval (ms/task time) at which the dialysate pump is controlled. +#define DP_CONTROL_INTERVAL_MS 1000 ///< Dialysate pump control interval in ms +#define DP_CONTROL_INTERVAL ( DP_CONTROL_INTERVAL_MS / 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. #define DIALYSATE_PUMP_CONTROL_STOP 0x00 ///< Dialysate pump control stop pump. #define DIALYSATE_PUMP_FORWARD_DIR 1 ///< Dialysate pump forward direction. reverse direction is not allowed. -#define DIALYSATE_PUMP_SPEED_INCREMENT 10.0F ///< Speed increase when controlling dialysate pump to target step speed. +#define DIALYSATE_PUMP_RAMP_SPEED_INCREMENT 10.0F ///< Speed increase when controlling dialysate pump ramp to target step speed. #define ZERO_SPEED 0.0F ///< Zero speed/RPM value. #define DIALYSATE_PUMP_MAX_CURRENT_WHEN_OFF_A 0.1F ///< Dialysate pump maximum current when the pump is off in amps. @@ -61,25 +60,21 @@ #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 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 4.0F ///< Minimum dialysate pump pressure in psi. -#define FRESH_DIAL_OPEN_LOOP_SPEED_RPM 2000.0F ///< fresh dialysate pump open loop speed to meet target speed. -#define SPENT_DIAL_OPEN_LOOP_SPEED_RPM 2500.0F ///< spent dialysate pump open loop speed to meet target speed. +#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_PRES_TO_SPD_CONV_FACTOR 106.0F ///< Pressure to speed conversion factor for fresh dialysate pump control. -#define D48_PRES_TO_SPD_CONV_FACTOR 91.4F ///< Pressure to speed conversion factor for fresh dialysate pump 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 DIAL_PUMP_NO_FEED_FORWARD 0.0F ///< Feedforward term for dialysate pump 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. @@ -123,6 +118,7 @@ U32 pumpID; ///< Pump ID Fresh(DGP) : 0, Spent ( SDP) : 1 U32 startStop; ///< Stop : 0, Start : 1 U32 rpm; ///< Speed range from 300 to 4500 RPM range + U32 pumpControl; ///< 1 : Open Loop control, 0 : Closed loop control } DIAL_PUMP_START_STOP_CMD_PAYLOAD_T; // ********** private data ********** @@ -138,12 +134,14 @@ 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 isDialPumpOpenLoopEnabled[ NUM_OF_DIALYSATE_PUMPS ]; ///< Flag to run in open loop control mode. +static BOOL dialPumpsReadyToControl; ///< Flag use to indicate the minimum control interval is met for pump control + //For testing #ifdef __PITEST__ static F32 pIControlSignal[ NUM_OF_CONTROLLER_SIGNAL ]; #endif -//TODO : Validate once HDD defines the conversion. static const F32 CURRENT_CONVERSION_COEFF = (F32)( 2.5F / ( BITS_12_FULL_SCALE - 1.0F ) ); // ********** private function prototypes ********** @@ -159,15 +157,17 @@ * @brief * The initDialysatePump function initializes the DialysatePumps unit. * @details \b Inputs: none - * @details \b Outputs: Dialysate unit variables initialized. + * @details \b Outputs: Dialysate pump unit variables initialized. * @return none *************************************************************************/ void initDialysatePump( void ) { DIALYSATE_PUMPS_T pumpId; + #ifdef __PITEST__ U32 i; #endif + dialysatePumpDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; for ( pumpId = DIALYSATE_PUMPS_FIRST; pumpId < NUM_OF_DIALYSATE_PUMPS; pumpId++ ) @@ -196,6 +196,8 @@ dialysatePumps[ pumpId ].control = DIALYSATE_PUMP_CONTROL_STOP; dialysatePumps[ pumpId ].directionErrorCount = 0; dialysatePumps[ pumpId ].lastDirectionErrorCount = 0; + isDialPumpOpenLoopEnabled[ pumpId ] = FALSE; + dialPumpsReadyToControl = FALSE; signalDialysatePumpHardStop( pumpId ); } @@ -211,11 +213,11 @@ // Initialize the fresh dialysate pump PI controller 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 ); + MIN_DIALYSATE_PUMP_RPM, MAX_DIALYSATE_PUMP_RPM, FALSE, DIAL_PUMP_NO_FEED_FORWARD ); // Initialize spent dialysate pump PI controller 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 ); + MIN_DIALYSATE_PUMP_RPM, MAX_DIALYSATE_PUMP_RPM,FALSE, DIAL_PUMP_NO_FEED_FORWARD ); // 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. @@ -228,6 +230,18 @@ pIControlSignal[ i ] = 0.0F; } #endif + + // 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 ); @@ -254,9 +268,10 @@ * @details \b Alarms: ALARM_ID_DD_SOFTWARE_FAULT when invalid pump id supplied. * @param pumpId pump id to set the new speed. * @param rpm new dialysate pump target RPM + * @param isOpenLoopControlEnabled dialysate pump control method * @return TRUE if new target RPM is set, FALSE if not *************************************************************************/ -BOOL setDialysatePumpTargetRPM( DIALYSATE_PUMPS_T pumpId, U32 rpm ) +BOOL setDialysatePumpTargetRPM( DIALYSATE_PUMPS_T pumpId, U32 rpm, BOOL isOpenLoopControlEnabled ) { BOOL result = FALSE; @@ -268,13 +283,17 @@ } else if ( rpm < MIN_DIALYSATE_PUMP_RPM ) { - pumpTargetSpeed[ pumpId ].data = 0.0; + // Lets assign minimum speed to make sure, driver is not getting into fault mode + pumpTargetSpeed[ pumpId ].data = MIN_DIALYSATE_PUMP_RPM; } else { pumpTargetSpeed[ pumpId ].data = MAX_DIALYSATE_PUMP_RPM; } + // Open loop or close loop control + isDialPumpOpenLoopEnabled[ pumpId ] = isOpenLoopControlEnabled; + //handle target speed update when pump is running if ( DIALYSATE_PUMP_CONTROL_TO_TARGET_STATE == dialysatePumps[ pumpId ].dialysatePumpState ) { @@ -329,15 +348,16 @@ dialysatePumps[ pumpId ].dialysatePumpState = DIALYSATE_PUMP_OFF_STATE; dialysatePumps[ pumpId ].controlTimerCounter = 0; dialysatePumps[ pumpId ].isDialPumpOn = FALSE; + isDialPumpOpenLoopEnabled[ pumpId ] = FALSE; //Reset PI Controller if ( D12_PUMP == pumpId ) { - resetPIController( PI_CONTROLLER_ID_D12_PUMP, FRESH_DIAL_OPEN_LOOP_SPEED_RPM ); + resetPIController( PI_CONTROLLER_ID_D12_PUMP, FRESH_DIAL_OPEN_LOOP_SPEED_RPM, DIAL_PUMP_NO_FEED_FORWARD ); } else { - resetPIController( PI_CONTROLLER_ID_D48_PUMP, SPENT_DIAL_OPEN_LOOP_SPEED_RPM ); + resetPIController( PI_CONTROLLER_ID_D48_PUMP, SPENT_DIAL_OPEN_LOOP_SPEED_RPM, DIAL_PUMP_NO_FEED_FORWARD ); } } @@ -437,7 +457,7 @@ isOffCurrentOut = ( getDialysatePumpMeasuredCurrentA( pumpId ) > DIALYSATE_PUMP_MAX_CURRENT_WHEN_OFF_A ? TRUE : FALSE ); isRPMTooHigh = ( getDialysatePumpMeasuredSpeed( pumpId ) > MIN_DIALYSATE_PUMP_RPM ? TRUE : FALSE ); - + //TODO : Enable the code later for alarm testing #if 0 if ( D12_PUMP == pumpId ) { @@ -472,6 +492,7 @@ BOOL isDirInvalid = ( ( direction != DIALYSATE_PUMP_FORWARD_DIR ) && ( rpm > DIALYSATE_PUMP_MIN_RPM_FOR_DIR_CHECK ) ? TRUE : FALSE ); isRPMOutOfRange = ( rpmDiff > MAX_ALLOWED_RPM_OUT_OF_RANGE ? TRUE : FALSE ); + //TODO : Enable the code later for alarm testing #if 0 if ( D12_PUMP == pumpId ) { @@ -500,6 +521,7 @@ // Check the persistent alarm for the maximum dialysate pump current F32 currentA = getDialysatePumpMeasuredCurrentA( pumpId ); isCurrentOutOfRange = ( currentA > DIALYSATE_PUMP_MAX_CURRENT_A ? TRUE : FALSE ) | isOffCurrentOut; + //TODO : Enable the code later for alarm testing #if 0 if ( D12_PUMP == pumpId ) { @@ -553,13 +575,10 @@ dialysatePumps[ pumpId ].dialysatePumpState = handleDialysatePumpControlToTargetState( pumpId ); break; -#ifndef _VECTORCAST_ - // The switch case is in a for loop so the default case cannot be covered in VectorCAST default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_DIALYSATE_PUMP_EXEC_INVALID_STATE, pumpId ) dialysatePumps[ pumpId ].dialysatePumpState = DIALYSATE_PUMP_OFF_STATE; break; -#endif } } } @@ -667,11 +686,11 @@ { if ( D12_PUMP == pumpId ) { - resetPIController( PI_CONTROLLER_ID_D12_PUMP, FRESH_DIAL_OPEN_LOOP_SPEED_RPM ); + resetPIController( PI_CONTROLLER_ID_D12_PUMP, FRESH_DIAL_OPEN_LOOP_SPEED_RPM, DIAL_PUMP_NO_FEED_FORWARD ); } else { - resetPIController( PI_CONTROLLER_ID_D48_PUMP, SPENT_DIAL_OPEN_LOOP_SPEED_RPM ); + resetPIController( PI_CONTROLLER_ID_D48_PUMP, SPENT_DIAL_OPEN_LOOP_SPEED_RPM, DIAL_PUMP_NO_FEED_FORWARD ); } state = DIALYSATE_PUMP_CONTROL_TO_TARGET_STATE; } @@ -696,9 +715,9 @@ if ( currentToTargetDiff > ZERO_SPEED ) { - if ( currentToTargetDiff > DIALYSATE_PUMP_SPEED_INCREMENT ) + if ( currentToTargetDiff > DIALYSATE_PUMP_RAMP_SPEED_INCREMENT ) { - speedIncrease = DIALYSATE_PUMP_SPEED_INCREMENT; + speedIncrease = DIALYSATE_PUMP_RAMP_SPEED_INCREMENT; } else { @@ -750,71 +769,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 ) ) && ( TRUE != isDialPumpOpenLoopEnabled[ pumpId ] ) ) { - // 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 ); + dialPumpsReadyToControl = FALSE; - F32 control = runPIController( PI_CONTROLLER_ID_D12_PUMP, targetPressure, measuredPressure ); + // Control based on the measured and target pressure + if ( D12_PUMP == pumpId ) + { + F32 measuredPressure = getFilteredPressure( D18_PRES ); + F32 targetPressure = getDialysatePumpTargetPressure( pumpId ); + + F32 control = runPIController( PI_CONTROLLER_ID_D12_PUMP, targetPressure, measuredPressure ); #ifdef __PITEST__ - U32 i; + U32 i; - for ( i = 0; i < NUM_OF_CONTROLLER_SIGNAL; i++ ) - { - pIControlSignal[ i ] = getPIControllerSignals( PI_CONTROLLER_ID_D12_PUMP, (PI_CONTROLLER_SIGNALS_ID)i ); - } + for ( i = 0; i < NUM_OF_CONTROLLER_SIGNAL; i++ ) + { + pIControlSignal[ i ] = getPIControllerSignals( PI_CONTROLLER_ID_D12_PUMP, (PI_CONTROLLER_SIGNALS_ID)i ); + } #endif - -#if 0 - if ( measuredPressure < targetPressure ) - { - newSpeed = dialysatePumps[ pumpId ].currentPumpSpeed + DIALYSATE_PUMP_SPEED_INCREMENT; + dialysatePumps[ pumpId ].currentPumpSpeed = control; + //Set fresh dialyate pump speed + setFPGAD12PumpSpeed( (U16)dialysatePumps[ pumpId ].currentPumpSpeed ); } else { - newSpeed = dialysatePumps[ pumpId ].currentPumpSpeed - DIALYSATE_PUMP_SPEED_INCREMENT; - } -#endif - dialysatePumps[ pumpId ].currentPumpSpeed = control; - //Set fresh dialyate pump speed - setFPGAD12PumpSpeed( (U16)dialysatePumps[ pumpId ].currentPumpSpeed ); + F32 measuredPressure = getFilteredPressure( D51_PRES ); + F32 targetPressure = getDialysatePumpTargetPressure( pumpId ); - } - else - { - F32 measuredPressure = getFilteredPressure( D51_PRES ); - F32 targetPressure = getDialysatePumpTargetPressure( pumpId ); - - F32 control = runPIController( PI_CONTROLLER_ID_D48_PUMP, targetPressure, measuredPressure ); - + F32 control = runPIController( PI_CONTROLLER_ID_D48_PUMP, targetPressure, measuredPressure ); #ifdef __PITEST__ - U32 i; + U32 i; - for ( i = 0; i < NUM_OF_CONTROLLER_SIGNAL; i++ ) - { - pIControlSignal[ i ] = getPIControllerSignals( PI_CONTROLLER_ID_D48_PUMP, (PI_CONTROLLER_SIGNALS_ID)i ); - } + for ( i = 0; i < NUM_OF_CONTROLLER_SIGNAL; i++ ) + { + pIControlSignal[ i ] = getPIControllerSignals( PI_CONTROLLER_ID_D48_PUMP, (PI_CONTROLLER_SIGNALS_ID)i ); + } #endif -#if 0 - if ( measuredPressure < targetPressure ) - { - newSpeed = dialysatePumps[ pumpId ].currentPumpSpeed + DIALYSATE_PUMP_SPEED_INCREMENT; + dialysatePumps[ pumpId ].currentPumpSpeed = control; + //Set spent dialyate pump speed + setFPGAD48PumpSpeed( (U16)dialysatePumps[ pumpId ].currentPumpSpeed ); } - else - { - newSpeed = dialysatePumps[ pumpId ].currentPumpSpeed - DIALYSATE_PUMP_SPEED_INCREMENT; - } -#endif - dialysatePumps[ pumpId ].currentPumpSpeed = control; - //Set spent dialyate pump speed - setFPGAD48PumpSpeed( (U16)dialysatePumps[ pumpId ].currentPumpSpeed ); - } - dialysatePumps[ pumpId ].controlTimerCounter = 0; } return state; @@ -973,7 +977,6 @@ 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; @@ -1130,7 +1133,7 @@ { BOOL result = FALSE; - // Verify tester has logged in with TD + // Verify tester has logged in with DD if ( TRUE == isTestingActivated() ) { // Verify payload length is valid @@ -1146,7 +1149,7 @@ if ( ( TRUE == payload.startStop ) && ( ( payload.rpm >= MIN_DIALYSATE_PUMP_RPM ) && ( payload.rpm <= MAX_DIALYSATE_PUMP_RPM ) ) ) { - setDialysatePumpTargetRPM( (DIALYSATE_PUMPS_T)payload.pumpID, payload.rpm ); + setDialysatePumpTargetRPM( (DIALYSATE_PUMPS_T)payload.pumpID, payload.rpm, (BOOL)payload.pumpControl ); result = TRUE; }