/************************************************************************** * * Copyright (c) 2019-2020 Diality Inc. - All Rights Reserved. * * THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN * WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER. * * @file DialOutFlow.c * * @date 7-Jan-2020 * @author L. Baloa * * @brief Monitor/Controller for dialysate outlet pump and load cell sensor. * **************************************************************************/ #ifndef _VECTORCAST_ #include #endif #include "etpwm.h" #include "gio.h" #include "spi.h" #include "Common.h" #include "FPGA.h" #include "InternalADC.h" #include "OperationModes.h" #include "PIControllers.h" #include "SystemCommMessages.h" #include "TaskGeneral.h" #include "TaskPriority.h" #include "Timers.h" #include "FIRFilters.h" #include "DialOutFlow.h" // ********** private definitions ********** #define DIAL_OUT_FLOW_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) // interval (ms/task time) at which the dialIn flow data is published on the CAN bus #define MAX_DIAL_OUT_FLOW_RATE 500 // mL/min #define MIN_DIAL_OUT_FLOW_RATE 100 // mL/min #define DOP_P_COEFFICIENT 0.0002 // P term for dialOut pump control #define DOP_I_COEFFICIENT 0.00002 // I term for dialOut pump control #define MAX_DIAL_OUT_PUMP_PWM_DUTY_CYCLE 0.88 // controller will error if PWM duty cycle > 90%, so set max to 88% #define MIN_DIAL_OUT_PUMP_PWM_DUTY_CYCLE 0.12 // controller will error if PWM duty cycle < 10%, so set min to 12% #define DOP_CONTROL_INTERVAL ( 500 / TASK_GENERAL_INTERVAL ) // interval (ms/task time) at which the dialOut pump is controlled #define DOP_MAX_CURR_WHEN_STOPPED_MA 150.0 // motor controller current should not exceed this when pump should be stopped #define DOP_MIN_CURR_WHEN_RUNNING_MA 150.0 // motor controller current should always exceed this when pump should be running #define DOP_MAX_CURR_WHEN_RUNNING_MA 1000.0 // motor controller current should not exceed this when pump should be running #define DOP_MAX_CURR_ERROR_DURATION_MS 2000 // motor controller current errors persisting beyond this duration will trigger an alarm #define DIAL_OUT_FLOW_SAMPLE_FREQ ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) #define DIAL_OUT_CONTROLLER_SAMPLE_FREQ ( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) /*** dialOut pump stop and direction macros ***/ #define STOP_DO_PUMP_SPI4_PORT_MASK 0x00000200 // (SPI4CLK - re-purposed as output GPIO) #define SET_DOP_STOP() {spiREG4->PC3 |= STOP_DO_PUMP_SPI4_PORT_MASK;} #define CLR_DOP_STOP() {spiREG4->PC3 &= ~STOP_DO_PUMP_SPI4_PORT_MASK;} #define STOP_DO_PUMP_GIO_PORT_PIN 6U #define SET_DOP_DIR() gioSetBit( gioPORTA, STOP_DO_PUMP_GIO_PORT_PIN, PIN_SIGNAL_HIGH ) #define CLR_DOP_DIR() gioSetBit( gioPORTA, STOP_DO_PUMP_GIO_PORT_PIN, PIN_SIGNAL_LOW ) /*** setDialOutFlowRxTotalVolumeAndRxTime ***/ #define DOP_REV_PER_LITER 124.0 // rotor revolutions per liter #define DOP_ML_PER_MIN_TO_PUMP_RPM_FACTOR ( DOP_REV_PER_LITER / ML_PER_LITER ) #define DOP_GEAR_RATIO 32.0 // dialIn pump motor to dialIn pump gear ratio #define DOP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.0003717 // ~27 BP motor RPM = 1% PWM duty cycle #define DOP_PWM_ZERO_OFFSET 0.1 // 10% PWM duty cycle = zero speed #define DOP_PWM_FROM_ML_PER_MIN(rate) ( (rate) * DOP_ML_PER_MIN_TO_PUMP_RPM_FACTOR * DOP_GEAR_RATIO * DOP_MOTOR_RPM_TO_PWM_DC_FACTOR + DOP_PWM_ZERO_OFFSET ) #define MAX_RX_TOTAL_VOLUME_ML 8000 #define MIN_RX_TOTAL_VOLUME_ML 100 #define MAX_UF_RATE_ML_PER_HOUR 2500.0 /*** Used by Simulator ***/ #define DIALOUT_SIMULATOR 1 #define SIMULATOR_PWM_TO_ML_PER_MIN 678.0 #define SIMULATOR_ZERO_CROSSING_ML_PER_MIN 67.80 /*** getMeasuredVariable ***/ #define DIP_SPEED_ADC_TO_RPM_FACTOR 1.375 // conversion factor from ADC counts to RPM for dialIn pump motor #define DIP_CURRENT_ADC_TO_MA_FACTOR 2.65 // conversion factor from ADC counts to mA for dialIn pump motor typedef enum DialOutFlow_Measured_Signals { DIALOUT_LOAD_CELL_WEIGHT = 0, DIALOUT_MOTOR_SPEED, DIALOUT_MOTOR_CURRENT, NUM_OF_DIALOUT_MEASURED_SIGNALS } DIALOUT_MEASURED_SIGNALS_T; // ********** private data ********** volatile static DIALOUT_FLOW_STATE_T dialOutFlowState = DIALOUT_FLOW_STOP_STATE; // current state of dialOut flow state machine static U32 dialOutFlowDataPublicationTimerCounter = 5; // used to schedule dialIn flow data publication to CAN bus // Rx values static volatile F32 rxTotalTargetVolumeInMl; static volatile U32 rxTargetTimeInSamples; static volatile F32 rxInitialTargetFlowRatePWM; static volatile F32 targetVolumeUFRatePerSample; // Variables used in loop // Controller key values static F32 loadCellVolumeInMl; static F32 bagStartVolumeInMl; static F32 bagMeasuredUFVolumeInMl; static F32 accumulativeTotalUFVolumeInMl; static volatile F32 totalMeasuredUFVolumeInMl; static volatile F32 totalTargetUFVolumeInMl; static volatile F32 sentPWM; static volatile U32 targetVolumeTimer; // Pump variables static F32 dialOutPumpSpeedInRPM; static F32 dialOutPumpCurrentInMA; #ifdef DIALOUT_SIMULATOR // Simulator static volatile F32 simulator_volume_in_ml_per_min; static volatile F32 simulator_inlet_flow_rate; #endif // ********** private function prototypes ********** static DIALOUT_FLOW_STATE_T handleDialOutFlowStopState( void ); static DIALOUT_FLOW_STATE_T handleDialOutFlowRunUFState( void ); static DIALOUT_FLOW_STATE_T handleDialOutFlowPauseUFState( void ); static F32 getMeasuredVariable( DIALOUT_MEASURED_SIGNALS_T signal); static void setControlSignalPWM( F32 newPWM ); static void stopDialOutPump( void ); static void updateTargetVolume( void ); static void setDialOutPumpDirection( MOTOR_DIR_T dir ); static void publishDialOutFlowData( void ); #ifdef DIALOUT_SIMULATOR static F32 UFSimulator( F32 pwm ); static void ResetSimulator( void ); #endif /************************************************************************* * @brief setDialOutFlowNewState * The setDialOutFlowNewState function changes the state to STOP, PAUSE or RUN. * * @param newState is the newState to change the DialOut system to. * @param isNewBag is TRUE, if at the start of the new state a new bag will * be used. * @return TRUE if successful *************************************************************************/ BOOL setDialOutFlowNewState( DIALOUT_FLOW_STATE_T newState , BOOL isNewBag ) { BOOL isSuccess = TRUE; if(newState != dialOutFlowState) { switch( newState ) { case DIALOUT_FLOW_RUN_UF_STATE: break; case DIALOUT_FLOW_PAUSE_UF_STATE: if (dialOutFlowState == DIALOUT_FLOW_STOP_STATE) { // We won't allow change of state to pause newState = DIALOUT_FLOW_STOP_STATE; isSuccess = FALSE; } else { bagStartVolumeInMl = (isNewBag == TRUE) ? 0.0 : bagStartVolumeInMl; } break; case DIALOUT_FLOW_STOP_STATE: if (dialOutFlowState == DIALOUT_FLOW_PAUSE_UF_STATE) { // We won't allow change of state to stop newState = DIALOUT_FLOW_PAUSE_UF_STATE; isSuccess = FALSE; } else { // Reset total target volume totalTargetUFVolumeInMl = 0.0; accumulativeTotalUFVolumeInMl = 0.0; bagStartVolumeInMl = 0.0; sentPWM = 0.0; bagStartVolumeInMl = 0.0; ResetSimulator(); stopDialOutPump(); } break; default: newState = dialOutFlowState; isSuccess = FALSE; break; } dialOutFlowState = newState; } return isSuccess; } /************************************************************************* * @brief setControlSignalPWM * The setControlSignalPWM function set the PWM of the out flow pump. * * @param newPWM a fraction of between 0.0 and 1.0. * @return none *************************************************************************/ static void setControlSignalPWM( F32 newPWM ) { etpwmSetCmpA( etpwmREG3, (U16)( FLOAT_TO_INT_WITH_ROUND( newPWM * (F32)( etpwmREG3->TBPRD ) ) ) ); } /************************************************************************* * @brief updateTargetVolume * The updateTargetVolume function updates the next sample for total target * volume based on rate of UF * * @return none *************************************************************************/ static void updateTargetVolume( void ) { totalTargetUFVolumeInMl += targetVolumeUFRatePerSample; } /************************************************************************* * @brief setDialOutFlowRxTotalVolumeAndRxTime * The setDialOutFlowRxTotalVolumeAndRxTime function updates Rx for the UF * therapy * * @param rxTotalVolumeInMl Total Volume requested for ultra-filtration * @param rxTotalTimeInMinutes Total Rx time in minutes * @param rxFlowRateinMlPerMin * @return TRUE if settings are correct and save, false if parameters are * saved. *************************************************************************/ BOOL setDialOutFlowRxTotalVolumeAndRxTime( U16 rxTotalVolumeInMl, U16 rxTotalTimeInMinutes, U16 rxFlowRateinMlPerMin) { BOOL returnValue = TRUE; F32 rxUFPatientRate = ((F32) rxTotalVolumeInMl * MIN_PER_HOUR )/ rxTotalTimeInMinutes; if ( rxTotalVolumeInMl > MAX_RX_TOTAL_VOLUME_ML || rxTotalVolumeInMl < MIN_RX_TOTAL_VOLUME_ML ) { returnValue = FALSE; } else if ( rxUFPatientRate > MAX_UF_RATE_ML_PER_HOUR ) { returnValue = FALSE; } else { totalTargetUFVolumeInMl = 0.0; simulator_inlet_flow_rate = (F32) rxFlowRateinMlPerMin; rxTotalTargetVolumeInMl = (F32) rxTotalVolumeInMl; rxTargetTimeInSamples = (U32) rxTotalTimeInMinutes * SEC_PER_MIN * DIAL_OUT_CONTROLLER_SAMPLE_FREQ ; rxInitialTargetFlowRatePWM = RANGE( DOP_PWM_FROM_ML_PER_MIN( rxFlowRateinMlPerMin ), MIN_DIAL_OUT_PUMP_PWM_DUTY_CYCLE, MAX_DIAL_OUT_PUMP_PWM_DUTY_CYCLE ); resetPIController( PI_CONTROLLER_ID_LOAD_CELL, rxInitialTargetFlowRatePWM ); targetVolumeUFRatePerSample = (F32) rxTotalTargetVolumeInMl / (F32) rxTargetTimeInSamples; } return returnValue; } /************************************************************************* * @brief getMeasuredVariable * The getMeasuredVariable function returns measured signals needed by * the DialOutFlow module. * @param signal is a measured signal enum. * @return actual signal as float. *************************************************************************/ F32 getMeasuredVariable( DIALOUT_MEASURED_SIGNALS_T signal ) { F32 returnValue = 0.0; switch ( signal ) { case DIALOUT_LOAD_CELL_WEIGHT: returnValue = 0.0; // TODO: getFPGALoadCellWeight(); break; case DIALOUT_MOTOR_SPEED: returnValue = getIntADCReading( INT_ADC_DIAL_OUT_PUMP_SPEED ) * DIP_SPEED_ADC_TO_RPM_FACTOR; break; case DIALOUT_MOTOR_CURRENT: returnValue = getIntADCReading( INT_ADC_DIAL_OUT_PUMP_MOTOR_CURRENT ) * DIP_CURRENT_ADC_TO_MA_FACTOR; break; default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_DIAL_OUT_MEASURED_SIGNAL_STATE, signal ) break; } return returnValue; } /************************************************************************* * @brief execDialOutFlowMonitor * The execDialOutFlowMonitor function executes the dialIn flow monitor. * @details * Inputs : none * Outputs : measuredDialOutFlowRate, adcDialOutPumpMCSpeedRPM, adcDialOutPumpMCCurrentmA * @param none * @return none *************************************************************************/ void execDialOutFlowMonitor( void ) { loadCellVolumeInMl = getMeasuredVariable( DIALOUT_LOAD_CELL_WEIGHT ); dialOutPumpSpeedInRPM = getMeasuredVariable( DIALOUT_MOTOR_SPEED ); dialOutPumpCurrentInMA = getMeasuredVariable( DIALOUT_MOTOR_CURRENT ); // publish dialIn flow data on interval bagMeasuredUFVolumeInMl = loadCellVolumeInMl - bagStartVolumeInMl; totalMeasuredUFVolumeInMl = bagMeasuredUFVolumeInMl + accumulativeTotalUFVolumeInMl; totalMeasuredUFVolumeInMl = UFSimulator(sentPWM); publishDialOutFlowData(); } #ifdef DIALOUT_SIMULATOR /* * TODO: Delete UFSimulator when hardware is ready */ F32 UFSimulator(F32 pwm) { F32 flow; if(dialOutFlowState == DIALOUT_FLOW_STOP_STATE) { flow = ZERO; } else { flow = ( ((SIMULATOR_PWM_TO_ML_PER_MIN*pwm - simulator_inlet_flow_rate - SIMULATOR_ZERO_CROSSING_ML_PER_MIN) * (F32) TASK_PRIORITY_INTERVAL )/ (F32) MS_PER_SECOND ); } simulator_volume_in_ml_per_min += flow; return simulator_volume_in_ml_per_min; } /* * TODO: Delete ResetSimulator when hardware is ready. */ void ResetSimulator() { simulator_volume_in_ml_per_min = 0.0; } #endif /************************************************************************* * @brief initDialOutFlow * The initDialOutFlow function initializes the DialOutFlow module. * @param none * @return none *************************************************************************/ void initDialOutFlow( void ) { stopDialOutPump(); setDialOutPumpDirection( MOTOR_DIR_FORWARD ); initializeFilter(FILTER_ID_LOAD_CELL_WEIGHT, ZERO ); setDialOutFlowNewState( DIALOUT_FLOW_STOP_STATE , TRUE ); // initialize dialysate outlet flow PI controller initializePIController( PI_CONTROLLER_ID_LOAD_CELL, rxInitialTargetFlowRatePWM, DOP_P_COEFFICIENT, DOP_I_COEFFICIENT, MIN_DIAL_OUT_PUMP_PWM_DUTY_CYCLE, MAX_DIAL_OUT_PUMP_PWM_DUTY_CYCLE ); } /************************************************************************* * @brief execDialOutFlowController * The execDialOutFlowController function executes the dialIn flow controller. * @details * Inputs : dialOutPumpState * Outputs : dialOutPumpState * @param none * @return none *************************************************************************/ void execDialOutFlowController( void ) { switch ( dialOutFlowState ) { case DIALOUT_FLOW_STOP_STATE: dialOutFlowState = handleDialOutFlowStopState(); break; case DIALOUT_FLOW_RUN_UF_STATE: dialOutFlowState = handleDialOutFlowRunUFState(); break; case DIALOUT_FLOW_PAUSE_UF_STATE: dialOutFlowState = handleDialOutFlowPauseUFState(); break; default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_DIAL_OUT_FLOW_INVALID_DIAL_OUT_PUMP_STATE, dialOutFlowState ); break; } } /************************************************************************* * @brief handleDialOutFlowStopState * The handleDialOutFlowStopState function handles the dialout flow stop state \n * of the dialout flow state machine. * @details * Inputs : targetDialOutFlowRate, dialOutPumpDirection * Outputs : dialOutPumpPWMDutyCyclePctSet, dialOutPumpDirectionSet, isDialOutPumpOn * @param none * @return next state *************************************************************************/ static DIALOUT_FLOW_STATE_T handleDialOutFlowStopState( void ) { DIALOUT_FLOW_STATE_T result = DIALOUT_FLOW_STOP_STATE; targetVolumeTimer = 0; stopDialOutPump(); return result; } /************************************************************************* * @brief handleDialOutFlowPauseUFState * The handleDialOutPumpRampingDownState function handles the ramp down state \n * of the dialIn pump controller state machine. * @details * Inputs : dialOutPumpPWMDutyCyclePctSet * Outputs : dialOutPumpPWMDutyCyclePctSet * @param none * @return next state *************************************************************************/ static DIALOUT_FLOW_STATE_T handleDialOutFlowPauseUFState( void ) { sentPWM = runPIController( PI_CONTROLLER_ID_LOAD_CELL, totalTargetUFVolumeInMl, totalMeasuredUFVolumeInMl ); setControlSignalPWM( sentPWM ); return DIALOUT_FLOW_PAUSE_UF_STATE; } /************************************************************************* * @brief handleDialOutFlowRunUFState * The handleDialOutFlowRunUFState function handles the dialout flow * controller during run state. * @details * Inputs : none * Outputs : dialout flow State * @param none * @return next state *************************************************************************/ static DIALOUT_FLOW_STATE_T handleDialOutFlowRunUFState( void ) { updateTargetVolume(); sentPWM = runPIController( PI_CONTROLLER_ID_LOAD_CELL, totalTargetUFVolumeInMl, totalMeasuredUFVolumeInMl ); setControlSignalPWM( sentPWM ); return DIALOUT_FLOW_RUN_UF_STATE; } /************************************************************************* * @brief stopDialOutPump * The stopDialOutPump function sets the dialout flow stop signal and PWM * duty cycle to 0.0. * @details * Inputs : none * Outputs : dialOut pump stop signal activated, PWM duty cycle zeroed * @param none * @return none *************************************************************************/ static void stopDialOutPump( void ) { setControlSignalPWM(ZERO); SET_DOP_STOP(); } /************************************************************************* * @brief setDialOutPumpDirection * The setDialOutPumpDirection function sets the set dialIn pump direction to \n * the given direction. * @details * Inputs : dialOutPumpState * Outputs : dialOutPumpState * @param dir : dialIn pump direction to set * @return none *************************************************************************/ static void setDialOutPumpDirection( MOTOR_DIR_T dir ) { switch ( dir ) { case MOTOR_DIR_FORWARD: SET_DOP_DIR(); break; case MOTOR_DIR_REVERSE: CLR_DOP_DIR(); break; default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_DIAL_OUT_FLOW_INVALID_DIAL_OUT_PUMP_DIRECTION, dir ); break; } } /************************************************************************* * @brief publishDialOutFlowData * The publishDialOutFlowData function publishes dialIn flow data at the set \n * interval. * @details * Inputs : target flow rate, measured flow rate, measured MC speed, \n * measured MC current * Outputs : DialIn flow data is published to CAN bus. * @param none * @return none *************************************************************************/ static void publishDialOutFlowData( void ) { S16 doFlowState = (S16) dialOutFlowState; S16 doTotalTargetUFVolumeInMl = (S16) FLOAT_TO_INT_WITH_ROUND( totalTargetUFVolumeInMl ); S16 doTotalMeasuredUFVolumeInMl = (S16) FLOAT_TO_INT_WITH_ROUND( totalMeasuredUFVolumeInMl ); S16 doControlSignalPWM = (S16) FLOAT_TO_INT_WITH_ROUND( sentPWM * FRACTION_TO_PERCENT_FACTOR ); // publish dialIn flow data on interval if ( ++dialOutFlowDataPublicationTimerCounter > DIAL_OUT_FLOW_DATA_PUB_INTERVAL ) { broadcastDialOutFlowData( doFlowState, doTotalTargetUFVolumeInMl, doTotalMeasuredUFVolumeInMl, doControlSignalPWM ); dialOutFlowDataPublicationTimerCounter = 0; } }