/************************************************************************** * * 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 * * @author (last) Sean Nash * @date (last) 19-Jun-2020 * * @author (original) Sean * @date (original) 24-Jan-2020 * ***************************************************************************/ #include #include "etpwm.h" #include "gio.h" #include "mibspi.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 "DialOutFlow.h" /** * @addtogroup DialysateOutlet * @{ */ // ********** private definitions ********** #define DIAL_OUT_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) ///< Interval (ms/task time) at which the dialysate outlet pump data is published on the CAN bus. #define MAX_DIAL_OUT_FLOW_RATE 650 ///< Maximum dialysate outlet pump flow rate in mL/min. #define MIN_DIAL_OUT_FLOW_RATE 100 ///< Minimum dialysate outlet pump flow rate in mL/min. #define DPO_FLOW_ADJ_DUE_TO_HIGHER_INLET_PRES 0.875 ///< Adjustment factor to account for higher pump inlet pressure (than DPi pump inlet). #define MAX_DIAL_OUT_PUMP_PWM_STEP_UP_CHANGE 0.0133 ///< Maximum duty cycle change when ramping up ~ 200 mL/min/s. #define MAX_DIAL_OUT_PUMP_PWM_STEP_DN_CHANGE 0.02 ///< Maximum duty cycle change when ramping down ~ 300 mL/min/s. #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 ( 1000 / TASK_GENERAL_INTERVAL ) ///< Interval (ms/task time) at which the dialysate outlet pump is controlled. #define DOP_P_COEFFICIENT 0.0050 ///< P term for dialysate outlet pump control. #define DOP_I_COEFFICIENT 0.0001 ///< I term for dialysate outlet pump control. #define DOP_HOME_RATE 100 ///< target pump speed (in estimate mL/min) for homing. #define DOP_HOME_TIMEOUT_MS 10000 ///< maximum time allowed for homing to complete (in ms). #define DOP_SPEED_CALC_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) ///< interval (ms/task time) at which the dialysate outlet pump speed is calculated. #define DOP_HALL_EDGE_COUNTS_PER_REV 48 ///< number of hall sensor edge counts per motor revolution. #define DOP_MAX_MOTOR_SPEED_WHILE_OFF_RPM 100.0 ///< maximum motor speed (RPM) while motor is commanded off. #define DOP_MAX_ROTOR_VS_MOTOR_DIFF_RPM 5.0 ///< maximum difference in speed between motor and rotor (in rotor RPM). #define DOP_MAX_MOTOR_SPEED_ERROR_RPM 300.0 ///< maximum difference in speed between measured and commanded RPM. #define DOP_OFF_ERROR_PERSIST ((5 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL) ///< persist time (task intervals) for motor off error condition. #define DOP_MOTOR_SPEED_ERROR_PERSIST ((5 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL) ///< persist time (task intervals) motor speed error condition. #define DOP_ROTOR_SPEED_ERROR_PERSIST ((12 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL) ///< persist time (task intervals) rotor speed error condition. #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 DOP_SPEED_ADC_TO_RPM_FACTOR 1.280938 ///< Conversion factor from ADC counts to RPM for dialysate outlet pump motor. #define DOP_CURRENT_ADC_TO_MA_FACTOR 3.002 ///< Conversion factor from ADC counts to mA for dialysate outlet pump motor. #define DOP_ADC_FULL_SCALE_V 3.0 ///< DPo analog signals are 0-3V (while int. ADC ref V may be different). #define DOP_ADC_ZERO 1998 ///< Mid-point (zero) for ADC readings. #define SIGN_FROM_12_BIT_VALUE(v) ( (S16)(v) - (S16)DOP_ADC_ZERO ) ///< Macro converts a 12-bit ADC reading to a signed 16-bit value. /*** setDialOutFlowRxTotalVolumeAndRxTime ***/ #define DOP_REV_PER_LITER 150.24 ///< Rotor revolutions per liter. #define DOP_ML_PER_MIN_TO_PUMP_RPM_FACTOR ( DOP_REV_PER_LITER / ML_PER_LITER ) ///< Conversion factor from mL/min to pump motor RPM. #define DOP_GEAR_RATIO 32.0 ///< Pump motor to pump gear ratio. #define DOP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.00028 ///< ~28 DPo 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 ) ///< Macro converts a flow rate to an estimated PWM duty cycle %. /// Enumeration of dialysate outlet pump controller states. typedef enum DialOutPump_States { DIAL_OUT_PUMP_OFF_STATE = 0, ///< Off state of the dialysate outlet pump state machine. DIAL_OUT_PUMP_RAMPING_UP_STATE, ///< Ramping up state of the dialysate outlet pump state machine. DIAL_OUT_PUMP_RAMPING_DOWN_STATE, ///< Ramping down state of the dialysate outlet pump state machine. DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE, ///< Control state of the dialysate outlet pump state machine. NUM_OF_DIAL_OUT_PUMP_STATES ///< Number of dialysate outlet pump control states. } DIAL_OUT_PUMP_STATE_T; /// Enumeration of dialysate outlet pump self test states. typedef enum DialOut_Pump_Self_Test_States { DIAL_OUT_PUMP_SELF_TEST_STATE_START = 0, ///< Start state of the dialysate outlet pump self test state machine. DIAL_OUT_PUMP_TEST_STATE_IN_PROGRESS, ///< In progress state of the dialysate outlet pump self test state machine. DIAL_OUT_PUMP_TEST_STATE_COMPLETE, ///< Completed state of the dialysate outlet pump self test state machine. NUM_OF_DIAL_OUT_PUMP_SELF_TEST_STATES ///< Number of dialysate outlet pumpt self test states. } DIAL_OUT_PUMP_SELF_TEST_STATE_T; // pin assignments and macros for pump stop and direction outputs #define STOP_DO_PUMP_MIBSPI1_PORT_MASK 0x00000400 ///< MIBSPI1 SIMO[0] - re-purposed as GPIOoutput for pump controller run/stop pin. #ifndef BREADBOARD_TARGET #define SET_DOP_STOP() {mibspiREG1->PC3 &= ~STOP_DO_PUMP_MIBSPI1_PORT_MASK;} ///< Macro sets pump controller run/stop signal to stop. #define CLR_DOP_STOP() {mibspiREG1->PC3 |= STOP_DO_PUMP_MIBSPI1_PORT_MASK;} ///< Macro sets pump controller run/stop signal to run. #else #define SET_DOP_STOP() {mibspiREG1->PC3 |= STOP_DO_PUMP_MIBSPI1_PORT_MASK;} ///< Macro sets pump controller run/stop signal to stop. #define CLR_DOP_STOP() {mibspiREG1->PC3 &= ~STOP_DO_PUMP_MIBSPI1_PORT_MASK;} ///< Macro sets pump controller run/stop signal to run. #endif #define STOP_DO_PUMP_GIO_PORT_PIN 6U ///< GIO port A pin used for pump controller direction pin. #define SET_DOP_DIR() gioSetBit( gioPORTA, STOP_DO_PUMP_GIO_PORT_PIN, PIN_SIGNAL_HIGH ) ///< Macro sets pump controller direction to forward direction. #define CLR_DOP_DIR() gioSetBit( gioPORTA, STOP_DO_PUMP_GIO_PORT_PIN, PIN_SIGNAL_LOW ) ///< Macro sets pump controller direction to reverse direction. // ********** private data ********** static DIAL_OUT_PUMP_STATE_T dialOutPumpState = DIAL_OUT_PUMP_OFF_STATE; ///< Current state of the dialysate outlet pump controller state machine. static U32 dialOutFlowDataPublicationTimerCounter = 6; ///< Timer counter controlling when to publish dialysate outlet data. Set non-zero to phase data publication to CAN bus. static BOOL isDialOutPumpOn = FALSE; ///< Flag set to TRUE when dialysate outlet pump is running. static U32 lastGivenRate = 0; ///< Remembers the last given set point rate for the dialysate outlet pump. static F32 dialOutPumpPWMDutyCyclePct = 0.0; ///< Requested PWM duty cycle for dialysate outlet pump (based on given rate). static F32 dialOutPumpPWMDutyCyclePctSet = 0.0; ///< Currently set PWM duty cycle for dialysate outlet pump. static MOTOR_DIR_T dialOutPumpDirection = MOTOR_DIR_FORWARD; ///< Requested direction for dialysate outlet pump. static MOTOR_DIR_T dialOutPumpDirectionSet = MOTOR_DIR_FORWARD; ///< Currently set direction for dialysate outlet pump. static PUMP_CONTROL_MODE_T dialOutPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; ///< Requested control mode for dialysate outlet pump (open or closed loop). static PUMP_CONTROL_MODE_T dialOutPumpControlModeSet = PUMP_CONTROL_MODE_CLOSED_LOOP; ///< Currently set control mode for dialysate outlet pump. static OVERRIDE_F32_T loadCellWeightInGrams[ NUM_OF_LOAD_CELLS ]; ///< Measured weight from load cells. static OVERRIDE_U32_T dialOutDataPublishInterval = { DIAL_OUT_DATA_PUB_INTERVAL, DIAL_OUT_DATA_PUB_INTERVAL, DIAL_OUT_DATA_PUB_INTERVAL, 0 }; ///< Interval (in ms) at which to publish dialysate outlet data to CAN bus. static OVERRIDE_F32_T referenceUFVolumeInMl = { 0.0, 0.0, 0.0, 0 }; ///< Target ultrafiltration volume (in mL). static OVERRIDE_F32_T totalMeasuredUFVolumeInMl = { 0.0, 0.0, 0.0, 0 }; ///< Total measured ultrafiltration volume (in mL). static OVERRIDE_F32_T dialOutPumpMCSpeedRPM = { 0.0, 0.0, 0.0, 0 }; ///< Measured dialysate outlet pump motor controller speed. static OVERRIDE_F32_T dialOutPumpMCCurrentmA = { 0.0, 0.0, 0.0, 0 }; ///< Measured dialysate outlet pump motor controller current. static OVERRIDE_F32_T dialOutPumpRotorSpeedRPM = { 0.0, 0.0, 0.0, 0 }; ///< Measured dialysate outlet pump rotor speed. static OVERRIDE_F32_T dialOutPumpSpeedRPM = { 0.0, 0.0, 0.0, 0 }; ///< Measured dialysate outlet pump motor speed. static U32 dopControlTimerCounter = 0; ///< Timer counter to determine when to control dialysate outlet pump. static U32 dopCurrErrorDurationCtr = 0; ///< Timer counter for motor current error persistence. static U32 dopRotorRevStartTime = 0; ///< dialysate outlet pump rotor rotation start time (in ms) static BOOL dopStopAtHomePosition = FALSE; ///< stop dialysate outlet pump at next home position static U32 dopHomeStartTime = 0; ///< when did dialysate outlet pump home command begin? (in ms) static U16 dopLastMotorHallSensorCount = 0; ///< last hall sensor count for the dialysate outlet pump motor static MOTOR_DIR_T dopMotorDirectionFromHallSensors = MOTOR_DIR_FORWARD; ///< pump direction according to hall sensor count static U32 dopMotorSpeedCalcTimerCtr = 0; ///< counter determines interval for calculating dialysate outlet pump motor speed from hall sensor count. static U32 errorDialOutMotorOffPersistTimerCtr = 0; ///< persistence timer counter for motor off check error condition. static U32 errorDialOutMotorSpeedPersistTimerCtr = 0; ///< persistence timer counter for motor speed error condition. static U32 errorDialOutRotorSpeedPersistTimerCtr = 0; ///< persistence timer counter for rotor speed error condition. static DIAL_OUT_PUMP_SELF_TEST_STATE_T dialOutPumpSelfTestState = DIAL_OUT_PUMP_SELF_TEST_STATE_START; ///< Current state of the dialysate outlet pump self test state machine. static U32 dialOutPumpSelfTestTimerCount = 0; ///< Timer counter for time reference during self test. // Broadcasting record DIAL_OUT_FLOW_DATA_T dialOutBroadCastVariables; ///< Record containing latest dialysate outlet data for broadcasting. // ********** private function prototypes ********** static DIAL_OUT_PUMP_STATE_T handleDialOutPumpOffState( void ); static DIAL_OUT_PUMP_STATE_T handleDialOutPumpRampingUpState( void ); static DIAL_OUT_PUMP_STATE_T handleDialOutPumpRampingDownState( void ); static DIAL_OUT_PUMP_STATE_T handleDialOutPumpControlToTargetState( void ); static void setDialOutPumpControlSignalPWM( F32 newPWM ); static void stopDialOutPump( void ); static void releaseDialOutPumpStop( void ); static void setDialOutPumpDirection( MOTOR_DIR_T dir ); static void publishDialOutFlowData( void ); static void updateDialOutPumpSpeedAndDirectionFromHallSensors( void ); static void checkDialOutPumpRotor( void ); static void checkDialOutPumpDirection( void ); static void checkDialOutPumpSpeeds( void ); static void checkDialOutPumpMCCurrent( void ); static DATA_GET_PROTOTYPE( U32, getPublishDialOutDataInterval ); /*********************************************************************//** * @brief * The initDialOutFlow function initializes the DialOutFlow module. * @details * Inputs : none * Outputs : DialOutFlow module initialized. * @return none *************************************************************************/ void initDialOutFlow( void ) { U32 i; dopLastMotorHallSensorCount = getFPGADialOutPumpHallSensorCount(); stopDialOutPump(); setDialOutPumpDirection( MOTOR_DIR_FORWARD ); // initialize load cell weights for ( i = 0; i < NUM_OF_LOAD_CELLS; i++ ) { loadCellWeightInGrams[ i ].data = 0.0; } // initialize broadcast data dialOutBroadCastVariables.refUFVolMl = 0.0; dialOutBroadCastVariables.measUFVolMl = 0.0; dialOutBroadCastVariables.measRotSpdRPM = 0.0; dialOutBroadCastVariables.measSpdRPM = 0.0; dialOutBroadCastVariables.measMCSpdRPM = 0.0; dialOutBroadCastVariables.measMCCurrmA = 0.0; dialOutBroadCastVariables.setPWMpct = 0.0; // initialize dialysate outlet flow PI controller initializePIController( PI_CONTROLLER_ID_ULTRAFILTRATION, MIN_DIAL_OUT_PUMP_PWM_DUTY_CYCLE, DOP_P_COEFFICIENT, DOP_I_COEFFICIENT, MIN_DIAL_OUT_PUMP_PWM_DUTY_CYCLE, MAX_DIAL_OUT_PUMP_PWM_DUTY_CYCLE ); } /*********************************************************************//** * @brief * The setDialOutPumpTargetRate function sets a new target flow rate, pump * direction, and control mode. * @details * Inputs : isDialOutPumpOn, dialOutPumpDirectionSet * Outputs : targetDialOutFlowRate, dialOutPumpdirection, dialOutPumpPWMDutyCyclePct * @param flowRate new target dialysate outlet flow rate * @param dir new dialysate outlet flow direction * @param mode new control mode * @return TRUE if new flow rate & dir are set, FALSE if not *************************************************************************/ BOOL setDialOutPumpTargetRate( U32 flowRate, MOTOR_DIR_T dir, PUMP_CONTROL_MODE_T mode ) { BOOL result = FALSE; // direction change while pump is running is not allowed if ( ( FALSE == isDialOutPumpOn ) || ( 0 == flowRate ) || ( dir == dialOutPumpDirectionSet ) ) { // verify flow rate if ( flowRate <= MAX_DIAL_OUT_FLOW_RATE ) { F32 adjFlow = (F32)flowRate; if ( PUMP_CONTROL_MODE_CLOSED_LOOP == mode ) { adjFlow *= DPO_FLOW_ADJ_DUE_TO_HIGHER_INLET_PRES; } lastGivenRate = flowRate; dialOutPumpDirection = dir; dialOutPumpControlMode = mode; // set PWM duty cycle target to an estimated initial target to ramp to based on target flow rate - then we'll control to flow when ramp completed dialOutPumpPWMDutyCyclePct = DOP_PWM_FROM_ML_PER_MIN(adjFlow); switch ( dialOutPumpState ) { case DIAL_OUT_PUMP_RAMPING_UP_STATE: // see if we need to reverse direction of ramp if ( dialOutPumpPWMDutyCyclePct < dialOutPumpPWMDutyCyclePctSet ) { dialOutPumpState = DIAL_OUT_PUMP_RAMPING_DOWN_STATE; } break; case DIAL_OUT_PUMP_RAMPING_DOWN_STATE: // see if we need to reverse direction of ramp if ( dialOutPumpPWMDutyCyclePct > dialOutPumpPWMDutyCyclePctSet ) { dialOutPumpState = DIAL_OUT_PUMP_RAMPING_UP_STATE; } break; case DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE: // start ramp to new target in appropriate direction if ( dialOutPumpPWMDutyCyclePctSet > dialOutPumpPWMDutyCyclePct ) { dialOutPumpState = DIAL_OUT_PUMP_RAMPING_DOWN_STATE; } else { dialOutPumpState = DIAL_OUT_PUMP_RAMPING_UP_STATE; } break; default: // ok - not all states need to be handled here break; } result = TRUE; } else // requested flow rate too high { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_DIAL_OUT_FLOW_SET_TOO_HIGH, flowRate ) } } return result; } /*********************************************************************//** * @brief * The setDialOutUFVolumes function sets the ultrafiltration reference and * measured total volumes (in mL). * @details * Inputs : none * Outputs : referenceUFVolumeInMl and totalMeasuredUFVolumeInMl * @param refVol New ultrafiltration reference volume (in mL) * @param totVol New ultrafiltration total volume (in mL) * @return none *************************************************************************/ void setDialOutUFVolumes( F32 refVol, F32 totVol ) { referenceUFVolumeInMl.data = refVol; totalMeasuredUFVolumeInMl.data = totVol; } /*********************************************************************//** * @brief * The signalDialOutPumpHardStop function stops the dialysate outlet pump * immediately. * @details * Inputs : none * Outputs : Dialysate outlet pump stopped, set point reset, state changed to off * @return none *************************************************************************/ void signalDialOutPumpHardStop( void ) { lastGivenRate = 0; stopDialOutPump(); dialOutPumpState = DIAL_OUT_PUMP_OFF_STATE; dialOutPumpPWMDutyCyclePct = 0.0; dopControlTimerCounter = 0; resetPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, MIN_DIAL_OUT_PUMP_PWM_DUTY_CYCLE ); } /*********************************************************************//** * @brief * The signalDialOutPumpRotorHallSensor function handles the dialysate outlet * pump rotor hall sensor detection. Calculates rotor speed (in RPM). * Stops pump if there is a pending request to home the pump. * @details * Inputs : dopRotorRevStartTime, dopStopAtHomePosition * Outputs : dopRotorRevStartTime, dialOutPumpRotorSpeedRPM * @return none *************************************************************************/ void signalDialOutPumpRotorHallSensor( void ) { U32 rotTime = getMSTimerCount(); U32 deltaTime = calcTimeBetween( dopRotorRevStartTime, rotTime ); // calculate rotor speed (in RPM) dialOutPumpRotorSpeedRPM.data = ( 1.0 / (F32)deltaTime ) * (F32)MS_PER_SECOND * (F32)SEC_PER_MIN; dopRotorRevStartTime = rotTime; // if we're supposed to stop pump at home position, stop pump now. if ( TRUE == dopStopAtHomePosition ) { signalDialOutPumpHardStop(); dopStopAtHomePosition = FALSE; } } /*********************************************************************//** * @brief * The homeDialOutPump function initiates a dialysate outlet pump home operation. * @details * Inputs : dialOutPumpState * Outputs : dopStopAtHomePosition, dopHomeStartTime, dialysate outlet pump started (slow) * @return none *************************************************************************/ BOOL homeDialOutPump( void ) { BOOL result = FALSE; if ( DIAL_OUT_PUMP_OFF_STATE == dialOutPumpState ) { dopStopAtHomePosition = TRUE; dopHomeStartTime = getMSTimerCount(); result = setDialOutPumpTargetRate( DOP_HOME_RATE, MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_OPEN_LOOP ); } return result; } /*********************************************************************//** * @brief * The setDialOutUFVolumes function sets the ultrafiltration reference and * measured total volumes (in mL). * @details * Inputs : none * Outputs : loadCellWeightInGrams[] * @param res1Primary New weight from primary load cell of reservoir 1 * @param res1Backup New weight from backup load cell of reservoir 1 * @param res2Primary New weight from primary load cell of reservoir 2 * @param res2Backup New weight from backup load cell of reservoir 2 * @return TRUE if successful, FALSE if not *************************************************************************/ BOOL setNewLoadCellReadings( F32 res1Primary, F32 res1Backup, F32 res2Primary, F32 res2Backup ) { BOOL result = TRUE; loadCellWeightInGrams[ LOAD_CELL_RESERVOIR_1_PRIMARY ].data = res1Primary; loadCellWeightInGrams[ LOAD_CELL_RESERVOIR_1_BACKUP ].data = res1Backup; loadCellWeightInGrams[ LOAD_CELL_RESERVOIR_2_PRIMARY ].data = res2Primary; loadCellWeightInGrams[ LOAD_CELL_RESERVOIR_2_BACKUP ].data = res2Backup; return result; } /*********************************************************************//** * @brief * The execDialOutFlowMonitor function executes the dialysate outlet pump * and load cell sensor monitor. Checks are performed. Data is published * at appropriate interval. * @details * Inputs : latest sensor data * Outputs : dialOutPumpMCSpeedRPM, dialOutPumpMCCurrentmA * @return none *************************************************************************/ void execDialOutFlowMonitor( void ) { U16 bpRPM = getIntADCReading( INT_ADC_DIAL_OUT_PUMP_SPEED ); U16 bpmA = getIntADCReading( INT_ADC_DIAL_OUT_PUMP_MOTOR_CURRENT ); dialOutPumpMCSpeedRPM.data = (F32)(SIGN_FROM_12_BIT_VALUE(bpRPM)) * DOP_SPEED_ADC_TO_RPM_FACTOR; dialOutPumpMCCurrentmA.data = (F32)(SIGN_FROM_12_BIT_VALUE(bpmA)) * DOP_CURRENT_ADC_TO_MA_FACTOR; // calculate dialysate outlet pump motor speed/direction from hall sensor count updateDialOutPumpSpeedAndDirectionFromHallSensors(); // don't start enforcing checks until out of init/POST mode if ( getCurrentOperationMode() != MODE_INIT ) { // check pump direction checkDialOutPumpDirection(); // check pump controller current checkDialOutPumpMCCurrent(); // check pump speeds checkDialOutPumpSpeeds(); // check for home position, zero/low speed checkDialOutPumpRotor(); } publishDialOutFlowData(); } /*********************************************************************//** * @brief * The execDialOutFlowController function executes the dialysate outlet pump * ultrafiltration controller state machine. * @details * Inputs : dialOutPumpState * Outputs : dialOutPumpState * @return none *************************************************************************/ void execDialOutFlowController( void ) { switch ( dialOutPumpState ) { case DIAL_OUT_PUMP_OFF_STATE: dialOutPumpState = handleDialOutPumpOffState(); break; case DIAL_OUT_PUMP_RAMPING_UP_STATE: dialOutPumpState = handleDialOutPumpRampingUpState(); break; case DIAL_OUT_PUMP_RAMPING_DOWN_STATE: dialOutPumpState = handleDialOutPumpRampingDownState(); break; case DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE: dialOutPumpState = handleDialOutPumpControlToTargetState(); break; default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_DIAL_OUT_FLOW_INVALID_DIAL_OUT_PUMP_STATE, dialOutPumpState ); break; } } /*********************************************************************//** * @brief * The handleDialOutPumpOffState function handles the dialOut pump off state * of the dialOut pump controller state machine. * @details * Inputs : targetDialOutFlowRate, dialOutPumpDirection * Outputs : dialOutPumpPWMDutyCyclePctSet, dialOutPumpDirectionSet, isDialOutPumpOn * @return next state *************************************************************************/ static DIAL_OUT_PUMP_STATE_T handleDialOutPumpOffState( void ) { DIAL_OUT_PUMP_STATE_T result = DIAL_OUT_PUMP_OFF_STATE; // if we've been given a flow rate, setup ramp up and transition to ramp up state if ( lastGivenRate > 0 ) { // set initial PWM duty cycle dialOutPumpPWMDutyCyclePctSet = DOP_PWM_ZERO_OFFSET + MAX_DIAL_OUT_PUMP_PWM_STEP_UP_CHANGE; setDialOutPumpControlSignalPWM( dialOutPumpPWMDutyCyclePctSet ); // allow dialOut pump to run in requested direction setDialOutPumpDirection( dialOutPumpDirection ); releaseDialOutPumpStop(); isDialOutPumpOn = TRUE; result = DIAL_OUT_PUMP_RAMPING_UP_STATE; } else { dialOutPumpPWMDutyCyclePct = 0.0; } return result; } /*********************************************************************//** * @brief * The handleDialOutPumpRampingUpState function handles the ramp up state * of the dialOut pump controller state machine. * @details * Inputs : dialOutPumpPWMDutyCyclePctSet * Outputs : dialOutPumpPWMDutyCyclePctSet * @return next state *************************************************************************/ static DIAL_OUT_PUMP_STATE_T handleDialOutPumpRampingUpState( void ) { DIAL_OUT_PUMP_STATE_T result = DIAL_OUT_PUMP_RAMPING_UP_STATE; // have we been asked to stop the dialOut pump? if ( dialOutPumpPWMDutyCyclePct < NEARLY_ZERO ) { // start ramp down to stop dialOutPumpPWMDutyCyclePctSet -= MAX_DIAL_OUT_PUMP_PWM_STEP_DN_CHANGE; setDialOutPumpControlSignalPWM( dialOutPumpPWMDutyCyclePctSet ); result = DIAL_OUT_PUMP_RAMPING_DOWN_STATE; } // have we reached end of ramp up? else if ( dialOutPumpPWMDutyCyclePctSet >= dialOutPumpPWMDutyCyclePct ) { resetPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, dialOutPumpPWMDutyCyclePctSet ); dialOutPumpControlModeSet = dialOutPumpControlMode; // if open loop mode, set PWM to requested duty cycle where it will stay during control state if ( dialOutPumpControlModeSet == PUMP_CONTROL_MODE_OPEN_LOOP ) { dialOutPumpPWMDutyCyclePctSet = dialOutPumpPWMDutyCyclePct; setDialOutPumpControlSignalPWM( dialOutPumpPWMDutyCyclePct ); } result = DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE; } // continue ramp up else { dialOutPumpPWMDutyCyclePctSet += MAX_DIAL_OUT_PUMP_PWM_STEP_UP_CHANGE; setDialOutPumpControlSignalPWM( dialOutPumpPWMDutyCyclePctSet ); } return result; } /*********************************************************************//** * @brief * The handleDialOutPumpRampingDownState function handles the ramp down state * of the dialOut pump controller state machine. * @details * Inputs : dialOutPumpPWMDutyCyclePctSet * Outputs : dialOutPumpPWMDutyCyclePctSet * @return next state *************************************************************************/ static DIAL_OUT_PUMP_STATE_T handleDialOutPumpRampingDownState( void ) { DIAL_OUT_PUMP_STATE_T result = DIAL_OUT_PUMP_RAMPING_DOWN_STATE; // have we essentially reached zero speed if ( dialOutPumpPWMDutyCyclePctSet < (MAX_DIAL_OUT_PUMP_PWM_STEP_UP_CHANGE + DOP_PWM_ZERO_OFFSET) ) { stopDialOutPump(); result = DIAL_OUT_PUMP_OFF_STATE; } // have we reached end of ramp down? else if ( dialOutPumpPWMDutyCyclePctSet <= dialOutPumpPWMDutyCyclePct ) { resetPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, dialOutPumpPWMDutyCyclePctSet ); dialOutPumpControlModeSet = dialOutPumpControlMode; // if open loop mode, set PWM to requested duty cycle where it will stay during control state if ( dialOutPumpControlModeSet == PUMP_CONTROL_MODE_OPEN_LOOP ) { dialOutPumpPWMDutyCyclePctSet = dialOutPumpPWMDutyCyclePct; setDialOutPumpControlSignalPWM( dialOutPumpPWMDutyCyclePct ); } result = DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE; } // continue ramp down else { dialOutPumpPWMDutyCyclePctSet -= MAX_DIAL_OUT_PUMP_PWM_STEP_DN_CHANGE; setDialOutPumpControlSignalPWM( dialOutPumpPWMDutyCyclePctSet ); } return result; } /*********************************************************************//** * @brief * The handleDialOutPumpControlToTargetState function handles the "control to * target" state of the dialOut pump controller state machine. * @details * Inputs : dopControlTimerCounter, dialOutPumpControlModeSet, volumes. * Outputs : dopControlTimerCounter, pump controlled. * @return next state *************************************************************************/ static DIAL_OUT_PUMP_STATE_T handleDialOutPumpControlToTargetState( void ) { DIAL_OUT_PUMP_STATE_T result = DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE; // control at set interval if ( ++dopControlTimerCounter >= DOP_CONTROL_INTERVAL ) { if ( dialOutPumpControlModeSet == PUMP_CONTROL_MODE_CLOSED_LOOP ) { F32 refVol = getTotalTargetDialOutUFVolumeInMl(); F32 totVol = getTotalMeasuredUFVolumeInMl(); F32 newPWM; F32 deltaPWM; // get new PWM from PI controller newPWM = runPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, refVol, totVol ); // limit PWM change to max deltaPWM = newPWM - dialOutPumpPWMDutyCyclePctSet; if ( fabs( deltaPWM ) > MAX_DIAL_OUT_PUMP_PWM_STEP_UP_CHANGE ) { newPWM = ( deltaPWM < 0.0 ? dialOutPumpPWMDutyCyclePctSet - MAX_DIAL_OUT_PUMP_PWM_STEP_UP_CHANGE : dialOutPumpPWMDutyCyclePctSet + MAX_DIAL_OUT_PUMP_PWM_STEP_UP_CHANGE ); } // set new PWM dialOutPumpPWMDutyCyclePctSet = newPWM; setDialOutPumpControlSignalPWM( newPWM ); } dopControlTimerCounter = 0; } return result; } /*********************************************************************//** * @brief * The setDialOutPumpControlSignalPWM function set the PWM duty cycle of * the dialysate outlet pump to a given %. * @details * Inputs : none * Outputs : dialysis outlet pump PWM duty cycle is set. * @param newPWM A percentage duty cycle between 0.0 and 1.0 * @return none *************************************************************************/ static void setDialOutPumpControlSignalPWM( F32 newPWM ) { etpwmSetCmpA( etpwmREG3, (U32)( (S32)( ( newPWM * (F32)(etpwmREG3->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) ); } /*********************************************************************//** * @brief * 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 * @return none *************************************************************************/ static void stopDialOutPump( void ) { dialOutPumpPWMDutyCyclePctSet = 0.0; setDialOutPumpControlSignalPWM( 0 ); SET_DOP_STOP(); isDialOutPumpOn = FALSE; } /*********************************************************************//** * @brief * The releaseDialOutPumpStop function clears the dialysate outlet pump stop signal. * @details * Inputs : none * Outputs : dialysate outlet pump stop signal is cleared. * @return none *************************************************************************/ static void releaseDialOutPumpStop( void ) { CLR_DOP_STOP(); } /*********************************************************************//** * @brief * The setDialOutPumpDirection function sets the set dialOut pump direction to * the given direction. * @details * Inputs : none * Outputs : dialOutPumpDirectionSet, pump direction signal set to match given direction. * @param dir dialysate outlet pump direction to set * @return none *************************************************************************/ static void setDialOutPumpDirection( MOTOR_DIR_T dir ) { switch ( dir ) { case MOTOR_DIR_FORWARD: dialOutPumpDirectionSet = dir; SET_DOP_DIR(); break; case MOTOR_DIR_REVERSE: dialOutPumpDirectionSet = dir; CLR_DOP_DIR(); break; default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_DIAL_OUT_FLOW_INVALID_DIAL_OUT_PUMP_DIRECTION, dir ); break; } } /*********************************************************************//** * @brief * The publishDialOutFlowData function publishes dialysate outlet data at * the set interval. * @details * Inputs : Dialysate outlet pump data * Outputs : Dialysate outlet pump data is published to CAN bus. * @return none *************************************************************************/ static void publishDialOutFlowData( void ) { // publish dialysate outlet pump and UF volume data on interval if ( ++dialOutFlowDataPublicationTimerCounter >= getPublishDialOutDataInterval() ) { dialOutBroadCastVariables.refUFVolMl = getTotalTargetDialOutUFVolumeInMl(); dialOutBroadCastVariables.measUFVolMl = getTotalMeasuredUFVolumeInMl(); dialOutBroadCastVariables.measRotSpdRPM = getMeasuredDialOutPumpRotorSpeed(); dialOutBroadCastVariables.measSpdRPM = getMeasuredDialOutPumpSpeed(); dialOutBroadCastVariables.measMCSpdRPM = getMeasuredDialOutPumpMCSpeed(); dialOutBroadCastVariables.measMCCurrmA = getMeasuredDialOutPumpMCCurrent(); dialOutBroadCastVariables.setPWMpct = dialOutPumpPWMDutyCyclePctSet * FRACTION_TO_PERCENT_FACTOR; broadcastDialOutFlowData( &dialOutBroadCastVariables); dialOutFlowDataPublicationTimerCounter = 0; } } /*********************************************************************//** * @brief * The updateDialOutPumpSpeedAndDirectionFromHallSensors function calculates * the dialysate outlet pump motor speed and direction from hall sensor counter on * a 1 second interval. * @details * Inputs : dopLastMotorHallSensorCount, dopMotorSpeedCalcTimerCtr, current count from FPGA * Outputs : dopMotorDirectionFromHallSensors, dialOutPumpSpeedRPM * @return none *************************************************************************/ static void updateDialOutPumpSpeedAndDirectionFromHallSensors( void ) { if ( ++dopMotorSpeedCalcTimerCtr >= DOP_SPEED_CALC_INTERVAL ) { U16 dopMotorHallSensorCount = getFPGADialOutPumpHallSensorCount(); U16 incDelta = ( dopMotorHallSensorCount >= dopLastMotorHallSensorCount ? dopMotorHallSensorCount - dopLastMotorHallSensorCount : ( HEX_64_K - dopLastMotorHallSensorCount ) + dopMotorHallSensorCount ); U16 decDelta = HEX_64_K - incDelta; U16 delta; // determine dialysate outlet pump speed/direction from delta hall sensor count since last interval if ( incDelta < decDelta ) { dopMotorDirectionFromHallSensors = MOTOR_DIR_FORWARD; delta = incDelta; dialOutPumpSpeedRPM.data = ( (F32)delta / (F32)DOP_HALL_EDGE_COUNTS_PER_REV ) * (F32)SEC_PER_MIN; } else { dopMotorDirectionFromHallSensors = MOTOR_DIR_REVERSE; delta = decDelta; dialOutPumpSpeedRPM.data = ( (F32)delta / (F32)DOP_HALL_EDGE_COUNTS_PER_REV ) * (F32)SEC_PER_MIN * -1.0; } // update last count for next time dopLastMotorHallSensorCount = dopMotorHallSensorCount; dopMotorSpeedCalcTimerCtr = 0; } } /*********************************************************************//** * @brief * The checkDialOutPumpRotor function checks the rotor for the dialysate outlet * pump. If homing, this function will stop when hall sensor detected. If pump * is off or running very slowly, rotor speed will be set to zero. * @details * Inputs : dopStopAtHomePosition, dopHomeStartTime, dopRotorRevStartTime * Outputs : pump may be stopped if homing, dialOutPumpRotorSpeedRPM may be set to zero. * @return none *************************************************************************/ static void checkDialOutPumpRotor( void ) { // if homing, check timeout if ( ( TRUE == dopStopAtHomePosition ) && ( TRUE == didTimeout( dopHomeStartTime, DOP_HOME_TIMEOUT_MS ) ) ) { signalDialOutPumpHardStop(); dopStopAtHomePosition = FALSE; // TODO - alarm??? } // if pump is stopped or running very slowly, set rotor speed to zero if ( TRUE == didTimeout( dopRotorRevStartTime, DOP_HOME_TIMEOUT_MS ) ) { dialOutPumpRotorSpeedRPM.data = 0.0; } } /*********************************************************************//** * @brief * The checkDialOutPumpDirection function checks the set direction vs. * the direction implied by the sign of the measured MC speed. * @details * Inputs : adcDialOutPumpMCSpeedRPM, dialOutPumpDirectionSet, dialOutPumpState * Outputs : none * @return none *************************************************************************/ static void checkDialOutPumpDirection( void ) { MOTOR_DIR_T dopMCDir; if ( DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE == dialOutPumpState ) { // check set direction vs. direction from hall sensors or sign of motor controller speed dopMCDir = ( getMeasuredDialOutPumpMCSpeed() >= 0.0 ? MOTOR_DIR_FORWARD : MOTOR_DIR_REVERSE ); if ( ( dialOutPumpDirectionSet != dopMCDir ) || ( dialOutPumpDirectionSet != dopMotorDirectionFromHallSensors ) ) { #ifndef DISABLE_PUMP_DIRECTION_CHECKS SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DIAL_OUT_PUMP_MC_DIRECTION_CHECK, (U32)dialOutPumpDirectionSet, (U32)dopMotorDirectionFromHallSensors ) #endif } } } /*********************************************************************//** * @brief * The checkDialOutPumpSpeeds function checks several aspects of the dialysate * outlet pump speed. * 1. while pump is commanded off, measured motor speed should be < limit. * 2. while pump is controlling, measured motor speed should be within allowed range of measured motor controller speed. * 3. measured motor speed should be within allowed range of measured rotor speed. * All 3 checks have a persistence time that must be met before an alarm is triggered. * @details * Inputs : targetDialOutFlowRate, dialOutPumpSpeedRPM, dialOutPumpRotorSpeedRPM * Outputs : alarm(s) may be triggered * @return none *************************************************************************/ static void checkDialOutPumpSpeeds( void ) { F32 measMotorSpeed = getMeasuredDialOutPumpSpeed(); // check for pump running while commanded off if ( FALSE == isDialOutPumpOn ) { if ( measMotorSpeed > DOP_MAX_MOTOR_SPEED_WHILE_OFF_RPM ) { if ( ++errorDialOutMotorOffPersistTimerCtr >= DOP_OFF_ERROR_PERSIST ) { #ifndef DISABLE_PUMP_SPEED_CHECKS SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DIAL_OUT_PUMP_OFF_CHECK, measMotorSpeed ); #endif } } else { errorDialOutMotorOffPersistTimerCtr = 0; } } else { errorDialOutMotorOffPersistTimerCtr = 0; } if ( DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE == dialOutPumpState ) { F32 controllerMotorSpeed = getMeasuredDialOutPumpMCSpeed(); F32 deltaMotorSpeed = fabs( measMotorSpeed - controllerMotorSpeed ); F32 measRotorSpeed = getMeasuredDialOutPumpRotorSpeed(); F32 measMotorSpeedInRotorRPM = measMotorSpeed / DOP_GEAR_RATIO; F32 deltaRotorSpeed = fabs( measRotorSpeed - measMotorSpeedInRotorRPM ); // check measured motor speed vs. commanded motor speed while controlling to target if ( deltaMotorSpeed > DOP_MAX_MOTOR_SPEED_ERROR_RPM ) { if ( ++errorDialOutMotorSpeedPersistTimerCtr >= DOP_MOTOR_SPEED_ERROR_PERSIST ) { #ifndef DISABLE_PUMP_SPEED_CHECKS SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DIAL_OUT_PUMP_MOTOR_SPEED_CHECK, controllerMotorSpeed, measMotorSpeed ); #endif } } else { errorDialOutMotorSpeedPersistTimerCtr = 0; } // check measured rotor speed vs. measured motor speed while controlling to target if ( deltaRotorSpeed > DOP_MAX_ROTOR_VS_MOTOR_DIFF_RPM ) { if ( ++errorDialOutRotorSpeedPersistTimerCtr >= DOP_ROTOR_SPEED_ERROR_PERSIST ) { #ifndef DISABLE_PUMP_SPEED_CHECKS SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DIAL_OUT_PUMP_ROTOR_SPEED_CHECK, measRotorSpeed, measMotorSpeed ); #endif } } else { errorDialOutRotorSpeedPersistTimerCtr = 0; } } else { errorDialOutMotorSpeedPersistTimerCtr = 0; errorDialOutRotorSpeedPersistTimerCtr = 0; } } /*********************************************************************//** * @brief * The checkDialOutPumpMCCurrent function checks the measured MC current vs. * the set state of the dialOut pump (stopped or running). * @details * Inputs : dialOutPumpState, dopCurrErrorDurationCtr, adcDialOutPumpMCCurrentmA * Outputs : none * @return none *************************************************************************/ static void checkDialOutPumpMCCurrent( void ) { F32 dopCurr; // dialOut pump should be off if ( DIAL_OUT_PUMP_OFF_STATE == dialOutPumpState ) { dopCurr = fabs( getMeasuredDialOutPumpMCCurrent() ); if ( dopCurr > DOP_MAX_CURR_WHEN_STOPPED_MA ) { dopCurrErrorDurationCtr += TASK_PRIORITY_INTERVAL; if ( dopCurrErrorDurationCtr > DOP_MAX_CURR_ERROR_DURATION_MS ) { #ifndef DISABLE_MOTOR_CURRENT_CHECKS SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DIAL_OUT_PUMP_MC_CURRENT_CHECK, getMeasuredDialOutPumpMCCurrent() ); #endif } } else { dopCurrErrorDurationCtr = 0; } } // dialOut pump should be running else { dopCurr = fabs( getMeasuredDialOutPumpMCCurrent() ); if ( ( dopCurr < DOP_MIN_CURR_WHEN_RUNNING_MA ) || ( dopCurr > DOP_MAX_CURR_WHEN_RUNNING_MA ) ) { dopCurrErrorDurationCtr += TASK_PRIORITY_INTERVAL; if ( dopCurrErrorDurationCtr > DOP_MAX_CURR_ERROR_DURATION_MS ) { #ifndef DISABLE_MOTOR_CURRENT_CHECKS SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DIAL_OUT_PUMP_MC_CURRENT_CHECK, getMeasuredDialOutPumpMCCurrent() ); #endif } } else { dopCurrErrorDurationCtr = 0; } } } /************************************************************************* * GET SUPPORT FUNCTIONS *************************************************************************/ /*********************************************************************//** * @brief * The getPublishDialOutFlowDataInterval function gets the dialysate out flow * data publication interval. * @details * Inputs : dialOutDataPublishInterval * Outputs : none * @return the current dialysate out flow data publication interval (in ms). *************************************************************************/ static U32 getPublishDialOutDataInterval( void ) { U32 result = dialOutDataPublishInterval.data; if ( OVERRIDE_KEY == dialOutDataPublishInterval.override ) { result = dialOutDataPublishInterval.ovData; } return result; } /*********************************************************************//** * @brief * The getLoadCellWeightInGrams function gets the load cell weight. * @details * Inputs : loadCellWeightInGrams * Outputs : none * @param loadCellID ID of load cell to get * @return the current load cell weight in grams *************************************************************************/ F32 getLoadCellWeightInGrams( U32 loadCellID ) { F32 result = 0.0; if ( loadCellID < NUM_OF_LOAD_CELLS ) { if ( OVERRIDE_KEY == loadCellWeightInGrams[ loadCellID ].override ) { result = loadCellWeightInGrams[ loadCellID ].ovData; } else { result = loadCellWeightInGrams[ loadCellID ].data; } } else { activateAlarmNoData( ALARM_ID_HD_SOFTWARE_FAULT ); } return result; } /*********************************************************************//** * @brief * The getTotalTargetDialOutUFVolumeInMl function gets the target UF volume. * @details * Inputs : referenceUFVolumeInMl * Outputs : none * @return the current target UF volume in mL. *************************************************************************/ F32 getTotalTargetDialOutUFVolumeInMl( void ) { F32 result = referenceUFVolumeInMl.data; if ( OVERRIDE_KEY == referenceUFVolumeInMl.override ) { result = referenceUFVolumeInMl.ovData; } return result; } /*********************************************************************//** * @brief * The getTotalMeasuredUFVolumeInMl function gets the measured UF volume. * @details * Inputs : totalMeasuredUFVolumeInMl * Outputs : none * @return the current UF volume (in mL). *************************************************************************/ F32 getTotalMeasuredUFVolumeInMl( void ) { F32 result = totalMeasuredUFVolumeInMl.data; if ( OVERRIDE_KEY == totalMeasuredUFVolumeInMl.override ) { result = totalMeasuredUFVolumeInMl.ovData; } return result; } /*********************************************************************//** * @brief * The getMeasuredDialOutPumpRotorSpeed function gets the measured dialysate * outlet pump rotor speed. * @details * Inputs : dialOutPumpRotorSpeedRPM * Outputs : none * @return the current dialysate outlet pump rotor speed (in RPM). *************************************************************************/ F32 getMeasuredDialOutPumpRotorSpeed( void ) { F32 result = dialOutPumpRotorSpeedRPM.data; if ( OVERRIDE_KEY == dialOutPumpRotorSpeedRPM.override ) { result = dialOutPumpRotorSpeedRPM.ovData; } return result; } /*********************************************************************//** * @brief * The getMeasuredDialOutPumpSpeed function gets the measured dialysate outlet * pump motor speed. * @details * Inputs : dialOutPumpSpeedRPM * Outputs : none * @return the current dialysate outlet pump motor speed (in RPM). *************************************************************************/ F32 getMeasuredDialOutPumpSpeed( void ) { F32 result = dialOutPumpSpeedRPM.data; if ( OVERRIDE_KEY == dialOutPumpSpeedRPM.override ) { result = dialOutPumpSpeedRPM.ovData; } return result; } /*********************************************************************//** * @brief * The getMeasuredDialOutPumpMCSpeed function gets the measured dialOut pump * speed. * @details * Inputs : dialOutPumpMCSpeedRPM * Outputs : none * @return the current dialOut pump speed (in RPM). *************************************************************************/ F32 getMeasuredDialOutPumpMCSpeed( void ) { F32 result = dialOutPumpMCSpeedRPM.data; if ( OVERRIDE_KEY == dialOutPumpMCSpeedRPM.override ) { result = dialOutPumpMCSpeedRPM.ovData; } return result; } /*********************************************************************//** * @brief * The getMeasuredDialOutPumpMCCurrent function gets the measured dialOut pump * current. * @details * Inputs : dialOutPumpMCCurrentmA * Outputs : none * @return the current dialOut pump current (in mA). *************************************************************************/ F32 getMeasuredDialOutPumpMCCurrent( void ) { F32 result = dialOutPumpMCCurrentmA.data; if ( OVERRIDE_KEY == dialOutPumpMCCurrentmA.override ) { result = dialOutPumpMCCurrentmA.ovData; } return result; } /************************************************************************* * TEST SUPPORT FUNCTIONS *************************************************************************/ /*********************************************************************//** * @brief * The testSetDialOutPumpAndLoadCellDataPublishIntervalOverride function overrides the * dialout data publish interval. * @details * Inputs : none * Outputs : dialOutDataPublishInterval * @param value override dialout data publish interval with (in ms) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetDialOutPumpAndLoadCellDataPublishIntervalOverride( U32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { U32 intvl = value / TASK_PRIORITY_INTERVAL; result = TRUE; dialOutDataPublishInterval.ovData = intvl; dialOutDataPublishInterval.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetDialOutPumpAndLoadCellDataPublishIntervalOverride function resets the override * of the dialout data publish interval. * @details * Inputs : none * Outputs : dialOutDataPublishInterval * @return TRUE if reset override successful, FALSE if not *************************************************************************/ BOOL testResetDialOutPumpAndLoadCellDataPublishIntervalOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; dialOutDataPublishInterval.override = OVERRIDE_RESET; dialOutDataPublishInterval.ovData = dialOutDataPublishInterval.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetTargetDialOutFlowRateOverride function overrides the target * dialysate outlet flow rate. * @details * Inputs : none * Outputs : pump started w/ set target rate and control mode * @param value override target dialysate outlet flow rate (in mL/min) * @param ctrlMode override pump control mode to this mode (0 = closed loop, 1 = open loop) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetTargetDialOutFlowRateOverride( S32 value, U32 ctrlMode ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { MOTOR_DIR_T dir; if ( value < 0 ) { dir = MOTOR_DIR_REVERSE; } else { dir = MOTOR_DIR_FORWARD; } if ( ctrlMode < NUM_OF_PUMP_CONTROL_MODES ) { result = setDialOutPumpTargetRate( abs(value), dir, (PUMP_CONTROL_MODE_T)ctrlMode ); } } return result; } /*********************************************************************//** * @brief * The testResetTargetDialOutFlowRateOverride function resets the override of the * target dialysate outlet flow rate. * @details * Inputs : none * Outputs : pump stopped * @return TRUE if override reset successful, FALSE if not *************************************************************************/ BOOL testResetTargetDialOutFlowRateOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = setDialOutPumpTargetRate( 0, dialOutPumpDirection, dialOutPumpControlMode ); } return result; } /*********************************************************************//** * @brief * The testSetDialOutUFRefVolumeOverride function overrides the target * dialout vol rate. * @details * Inputs : referenceUFVolumeInMl * Outputs : referenceUFVolumeInMl * @param value override target dialout vol rate (in mL/min) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetDialOutUFRefVolumeOverride( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; referenceUFVolumeInMl.ovData = value; referenceUFVolumeInMl.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetDialOutUFRefVolumeOverride function resets the override of the * target dialout vol rate. * @details * Inputs : referenceUFVolumeInMl * Outputs : referenceUFVolumeInMl * @return TRUE if reset override successful, FALSE if not *************************************************************************/ BOOL testResetDialOutUFRefVolumeOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; referenceUFVolumeInMl.override = OVERRIDE_RESET; referenceUFVolumeInMl.ovData = referenceUFVolumeInMl.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetDialOutUFTotVolumeOverride function overrides the measured * dialout vol rate. * @details * Inputs : totalMeasuredUFVolumeInMl * Outputs : totalMeasuredUFVolumeInMl * @param value override measured dialout vol rate (in mL/min) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetDialOutUFTotVolumeOverride( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; totalMeasuredUFVolumeInMl.ovData = value; totalMeasuredUFVolumeInMl.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetDialOutUFTotVolumeOverride function resets the override of the * measured dialout vol rate. * @details * Inputs : totalMeasuredUFVolumeInMl * Outputs : totalMeasuredUFVolumeInMl * @return TRUE if reset override successful, FALSE if not *************************************************************************/ BOOL testResetDialOutUFTotVolumeOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; totalMeasuredUFVolumeInMl.override = OVERRIDE_RESET; totalMeasuredUFVolumeInMl.ovData = totalMeasuredUFVolumeInMl.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetMeasuredDialOutPumpRotorSpeedOverride function overrides the measured * dialOut pump rotor speed. * @details * Inputs : DialOutPumpRotorSpeedRPM * Outputs : DialOutPumpRotorSpeedRPM * @param value override measured dialOut pump rotor speed (in RPM) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetMeasuredDialOutPumpRotorSpeedOverride( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; dialOutPumpRotorSpeedRPM.ovData = value; dialOutPumpRotorSpeedRPM.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetMeasuredDialOutPumpRotorSpeedOverride function resets the override of the * measured dialOut pump rotor speed. * @details * Inputs : DialOutPumpRotorSpeedRPM * Outputs : DialOutPumpRotorSpeedRPM * @return TRUE if reset override successful, FALSE if not *************************************************************************/ BOOL testResetMeasuredDialOutPumpRotorSpeedOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; dialOutPumpRotorSpeedRPM.override = OVERRIDE_RESET; dialOutPumpRotorSpeedRPM.ovData = dialOutPumpRotorSpeedRPM.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetMeasuredDialOutPumpSpeedOverride function overrides the measured * dialOut pump motor speed. * @details * Inputs : dialOutPumpSpeedRPM * Outputs : dialOutPumpSpeedRPM * @param value override measured dialOut pump motor speed (in RPM) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetMeasuredDialOutPumpSpeedOverride( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; dialOutPumpSpeedRPM.ovData = value; dialOutPumpSpeedRPM.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetMeasuredDialOutPumpSpeedOverride function resets the override of the * measured dialOut pump motor speed. * @details * Inputs : dialOutPumpSpeedRPM * Outputs : dialOutPumpSpeedRPM * @return TRUE if reset override successful, FALSE if not *************************************************************************/ BOOL testResetMeasuredDialOutPumpSpeedOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; dialOutPumpSpeedRPM.override = OVERRIDE_RESET; dialOutPumpSpeedRPM.ovData = dialOutPumpSpeedRPM.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetMeasuredDialOutPumpMCSpeedOverride function overrides the measured * dialOut pump motor speed. * @details * Inputs : dialOutPumpMCSpeedRPM * Outputs : dialOutPumpMCSpeedRPM * @param value override measured dialOut pump speed (in RPM) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetMeasuredDialOutPumpMCSpeedOverride( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; dialOutPumpMCSpeedRPM.ovData = value; dialOutPumpMCSpeedRPM.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetMeasuredDialOutPumpMCSpeedOverride function resets the override of the * measured dialOut pump motor speed. * @details * Inputs : dialOutPumpMCSpeedRPM * Outputs : dialOutPumpMCSpeedRPM * @return TRUE if reset override successful, FALSE if not *************************************************************************/ BOOL testResetMeasuredDialOutPumpMCSpeedOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; dialOutPumpMCSpeedRPM.override = OVERRIDE_RESET; dialOutPumpMCSpeedRPM.ovData = dialOutPumpMCSpeedRPM.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetMeasuredDialOutPumpMCCurrentOverride function overrides the measured * dialOut pump motor current. * @details * Inputs : dialOutPumpMCCurrentmA * Outputs : dialOutPumpMCCurrentmA * @param value override measured dialOut pump current (in mA) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetMeasuredDialOutPumpMCCurrentOverride( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; dialOutPumpMCCurrentmA.ovData = value; dialOutPumpMCCurrentmA.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetMeasuredDialOutPumpMCCurrentOverride function resets the override of the * measured dialOut pump motor current. * @details * Inputs : dialOutPumpMCCurrentmA * Outputs : dialOutPumpMCCurrentmA * @return TRUE if reset override successful, FALSE if not *************************************************************************/ BOOL testResetMeasuredDialOutPumpMCCurrentOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; dialOutPumpMCCurrentmA.override = OVERRIDE_RESET; dialOutPumpMCCurrentmA.ovData = dialOutPumpMCCurrentmA.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetDialOutLoadCellWeightOverride function overrides the value of the * load cell sensor with a given weight (in grams). * @details * Inputs : loadCellWeightInGrams[] * Outputs : loadCellWeightInGrams[] * @param sensor ID of load cell sensor to override weight for * @param value override weight (in grams) for the given sensor * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetDialOutLoadCellWeightOverride( U32 sensor, F32 value ) { BOOL result = FALSE; if ( sensor < NUM_OF_LOAD_CELLS ) { if ( TRUE == isTestingActivated() ) { result = TRUE; loadCellWeightInGrams[ sensor ].ovData = value; loadCellWeightInGrams[ sensor ].override = OVERRIDE_KEY; } } return result; } /*********************************************************************//** * @brief * The testResetDialOutLoadCellWeightOverride function resets the override of the * load cell sensor. * @details * Inputs : loadCellWeightInGrams[] * Outputs : loadCellWeightInGrams[] * @param sensor ID of load cell sensor to override weight for * @return TRUE if reset successful, FALSE if not *************************************************************************/ BOOL testResetDialOutLoadCellWeightOverride( U32 sensor ) { BOOL result = FALSE; if ( sensor < NUM_OF_LOAD_CELLS ) { if ( TRUE == isTestingActivated() ) { result = TRUE; loadCellWeightInGrams[ sensor ].override = OVERRIDE_RESET; loadCellWeightInGrams[ sensor ].ovData = loadCellWeightInGrams[ sensor ].ovInitData; } } return result; } /**@}*/