/************************************************************************** * * Copyright (c) 2020-2023 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) 11-Sep-2023 * * @author (original) Sean * @date (original) 24-Jan-2020 * ***************************************************************************/ #include // Using fabs() function #include "etpwm.h" #include "gio.h" #include "mibspi.h" #include "reg_het.h" #include "CPLD.h" #include "DialOutFlow.h" #include "FPGA.h" #include "InternalADC.h" #include "ModeTreatment.h" #include "NVDataMgmt.h" #include "OperationModes.h" #include "PersistentAlarm.h" #include "PIControllers.h" #include "SafetyShutdown.h" #include "SystemCommMessages.h" #include "TaskGeneral.h" #include "TaskPriority.h" #include "Timers.h" /** * @addtogroup DialysateOutlet * @{ */ // ********** private definitions ********** /// Interval (ms/task time) at which the dialysate outlet pump data is published on the CAN bus. #define DIAL_OUT_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) /// Interval (ms/task time) at which the dialysate outlet flow is filtered. #define DIAL_OUT_FILTER_INTERVAL ( 100 / TASK_PRIORITY_INTERVAL ) /// Interval (ms/task time) at which the ultrafiltration flow rate is calculated. #define DIAL_OUT_UF_CALC_INTERVAL ( 1000 / TASK_PRIORITY_INTERVAL ) #define MAX_DIAL_OUT_FLOW_RATE 700 ///< 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 MAX_DIAL_OUT_PUMP_PWM_STEP_UP_CHANGE 0.01064F ///< Maximum duty cycle change when ramping up. #define MAX_DIAL_OUT_PUMP_PWM_STEP_DN_CHANGE 0.016F ///< Maximum duty cycle change when ramping down. #define MAX_DIAL_OUT_PUMP_PWM_DUTY_CYCLE 0.90F ///< Controller will error if PWM duty cycle > 90%, so set max to 89%. #define MIN_DIAL_OUT_PUMP_PWM_DUTY_CYCLE 0.10F ///< Controller will error if PWM duty cycle < 10%, so set min to 10%. #define MAX_DIAL_OUT_PUMP_PWM_OFFSET_CONTROL 0.4F ///< Maximum PWM offset (added to DPi PWM duty cycle). #define MIN_DIAL_OUT_PUMP_PWM_OFFSET_CONTROL -0.4F ///< Minimum PWM offset (added to DPi PWM duty cycle). #define P_VOL 0.0008F ///< P term for volume error feedback into dialysate outlet pump control. #define P_UF 0.0008F ///< P term for UF rate error feedback into dialysate outlet pump control. #define P_CORR 0.0666F ///< P term for volume error feedback into dialysate outlet pump flow estimate correction offset. #define RPM_2_ML_MIN_CONVERSION 0.215964F ///< Conversion factor for estimating flow rate from pump motor RPM. #define OFFSET_2_PWM_OFFSET 0.135F ///< Conversion factor for estimating PWM duty cycle offset for a given rate offset. #define SIZE_OF_ROLLING_AVG 40 ///< Number of samples in DPo flow estimation moving average. #define DOP_HOME_SPEED 400 ///< Target pump speed (in RPM) for homing. #define DOP_HOME_TIMEOUT_MS 10000 ///< Maximum time allowed for homing to complete (in ms). /// Interval (ms/task time) at which the blood pump speed is calculated (every 40 ms). #define DOP_SPEED_CALC_INTERVAL ( 40 / TASK_PRIORITY_INTERVAL ) /// Number of hall sensor counts kept in buffer to hold last 1 second of count data. #define DOP_SPEED_CALC_BUFFER__LEN ( 1000 / DOP_SPEED_CALC_INTERVAL / TASK_PRIORITY_INTERVAL ) #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.0F ///< Maximum motor speed (RPM) while motor is commanded off. #define DOP_MAX_ROTOR_VS_MOTOR_DIFF_RPM 5.0F ///< Maximum difference in speed between motor and rotor (in rotor RPM). #define DOP_MAX_MOTOR_SPEED_ERROR_RPM 300.0F ///< Maximum difference in speed between measured and commanded RPM. #define DOP_MAX_MOTOR_SPEED_VS_TRGT_DIFF_PCT 0.15F ///< Maximum motor speed vs target difference in percent. /// Persist time (task intervals) for motor off error condition. static const U32 DOP_OFF_ERROR_PERSIST = ((5 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL); /// Persist time (task intervals) motor speed error condition. static const U32 DOP_MOTOR_SPEED_ERROR_PERSIST = ((5 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL); /// Persist time (task intervals) pump direction error condition. static const U32 DOP_DIRECTION_ERROR_PERSIST = (250 / TASK_PRIORITY_INTERVAL); #define DOP_MAX_CURR_WHEN_STOPPED_MA 150.0F ///< Motor controller current should not exceed this when pump should be stopped. #define DOP_MAX_CURR_WHEN_RUNNING_MA 2000.0F ///< Motor controller current should not exceed this when pump should be running. #define DOP_MAX_CURR_ERROR_DURATION_MS 5000 ///< Motor controller current errors persisting beyond this duration will trigger an alarm. #define DOP_ADC_FULL_SCALE_V 3.0F ///< 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. #define DOP_SPEED_ADC_TO_RPM_FACTOR 2.152152F ///< Conversion factor from ADC counts to RPM for dialysate outlet pump motor (4300 RPM/1998 counts). #define DOP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.0002F ///< ~50 BP motor RPM = 1% PWM duty cycle #define DOP_CURRENT_ADC_TO_MA_FACTOR 3.002F ///< Conversion factor from ADC counts to mA for dialysate outlet pump motor. #define DOP_REV_PER_LITER 146.84F ///< 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.0F ///< Pump motor to pump gear ratio. #define DOP_PWM_ZERO_OFFSET 0.1F ///< 10% PWM duty cycle = zero speed. #define DOP_100_PCT_PWM_RPM_RANGE 5000.0F ///< 10-90% PWM range yields 0-4,000 RPM range. Full 100% PWM range would yield 5,000 RPM range. #define DOP_RATE_CORRECTION_SCALAR 0.194F ///< Scalar for estimating DPo rate correction offset based on target Qd. #define DOP_RATE_CORRECTION_OFFSET -19.40F ///< Offset for estimating DPo rate correction offset based on target Qd. /// Macro converts a flow rate to an estimated PWM duty cycle %. #define DOP_PWM_FROM_ML_PER_MIN(rate) ( ( ( rate ) * 0.00072F ) + 0.0972F + DOP_PWM_ZERO_OFFSET ) /// Conversion from PWM duty cycle % to commanded pump motor speed. #define DOP_PWM_TO_MOTOR_SPEED_RPM(pwm,dir) ( ( ( ( pwm ) - DOP_PWM_ZERO_OFFSET) * DOP_100_PCT_PWM_RPM_RANGE ) * ( dir == MOTOR_DIR_FORWARD ? 1.0F : -1.0F ) ) /// Conversion from RPM to PWM duty cycle %. #define DOP_MOTOR_SPEED_RPM_TO_PWM(rpm) ( ( (F32)(rpm) / DOP_100_PCT_PWM_RPM_RANGE ) + DOP_PWM_ZERO_OFFSET ) /// Macro converts a PWM to an estimated flow rate. #define DOP_ML_PER_MIN_FROM_PWM(pwm) ( ( ( pwm - DOP_PWM_ZERO_OFFSET ) - 0.0972F ) / 0.00072F ) /// Macro converts a PWM to an estimated flow rate (basic version). #define DOP_ML_PER_MIN_FROM_PWM_BASIC(pwm) ( ( ( ( pwm ) - DOP_PWM_ZERO_OFFSET ) * DOP_100_PCT_PWM_RPM_RANGE ) * 0.2 ) #define PUMP_DIR_ERROR_COUNT_MASK 0x3F ///< Bit mask for pump direction error counter. #define DOP_MIN_DIR_CHECK_SPEED_RPM 10.0F ///< Minimum motor speed before we check pump direction. #define DOP_COMMUTATION_ERROR_MAX_CNT 3 ///< Maximum number of commutation errors within time window before alarm triggered. #define DOP_COMMUTATION_ERROR_TIME_WIN_MS (15 * MS_PER_SECOND) ///< Time window for DPo commutation error. #define DATA_PUBLISH_COUNTER_START_COUNT 40 ///< Data publish counter start count. /// 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 pump self-test states. } DIAL_OUT_PUMP_SELF_TEST_STATE_T; // Pin assignments for pump stop and direction outputs and DPo rotor hall sensor input #define STOP_DO_PUMP_MIBSPI1_PORT_MASK 0x00000400 ///< MIBSPI1 SIMO[0] - re-purposed as GPIOoutput for pump controller run/stop pin. #define STOP_DO_PUMP_GIO_PORT_PIN 6U ///< GIO port A pin used for pump controller direction pin. #define DOP_ROTOR_HALL_SENSOR_NHET_ID 0x0000000E ///< NHET pin number associated with DPo rotor hall sensor input // Dialysate outlet pump stop and direction macros #define SET_DOP_STOP() {mibspiREG1->PC3 &= ~STOP_DO_PUMP_MIBSPI1_PORT_MASK;} ///< Macro sets pump controller disable signal low (active low). #define CLR_DOP_STOP() {mibspiREG1->PC3 |= STOP_DO_PUMP_MIBSPI1_PORT_MASK;} ///< Macro sets pump controller disable signal high (active low). #define SET_DOP_DIR() gioSetBit( gioPORTA, STOP_DO_PUMP_GIO_PORT_PIN, PIN_SIGNAL_HIGH ) ///< Macro sets pump controller direction to high (forward direction). #define CLR_DOP_DIR() gioSetBit( gioPORTA, STOP_DO_PUMP_GIO_PORT_PIN, PIN_SIGNAL_LOW ) ///< Macro sets pump controller direction to low (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; ///< 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_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 F32 offsetPWMDutyCyclePct = 0.0; ///< PWM duty cycle percentage offset to add to inlet pump duty cycle for UF control. static F32 dopMeasuredRate = 0.0; ///< Estimated flow rate for dialysate outlet pump. static F32 ufMeasuredRate = 0.0; ///< Calculated UF flow rate from measured dialysate flow rate subtracted from estimated dialysate outlet flow rate. static F32 dopRateCorrectionOffset = 0.0; ///< Correction offset for estimated flow rate for dialysate outlet pump. static U32 ufCalcTimerCtr = 0; ///< Timer counter for determining when to calculate the ultrafiltration rate. static U32 flowFilterTimerCtr = 0; ///< Timer counter for determining when to add a new sample to the moving average. static F64 flowReadings[ SIZE_OF_ROLLING_AVG ]; ///< Holds flow samples for a rolling average. static U32 flowReadingsIdx = 0; ///< Index for next sample in rolling average array. static F64 flowReadingsTotal = 0.0; ///< Rolling total - used to calc average. static U32 flowReadingsCount = 0; ///< Number of samples in flow rolling average buffer. static BOOL dopControlSignal = FALSE; ///< Control signal for syncing UF control to follow control interval of dialysate inlet 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 dopLastMotorHallSensorCounts[ DOP_SPEED_CALC_BUFFER__LEN ]; ///< Last hall sensor count for the dialysate outlet pump motor static U32 dopMotorSpeedCalcIdx = 0; ///< Index into 1 second buffer of motor speed hall sensor counts 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 U32 errorDialOutPumpDirectionPersistTimerCtr = 0; ///< Persistence timer counter for pump direction error condition. static U08 lastDialOutPumpDirectionCount = 0; ///< Previous pump direction error count reported by FPGA. static HD_PUMPS_CAL_RECORD_T dialOutPumpCalRecord; ///< Dialysate outlet calibration record. // ********** 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 void resetDialOutFlowMovingAverage( void ); static void filterDialOutFlowReadings( F64 flow ); static BOOL setDialOutPumpToFixedPWM( F32 pwm ); /*********************************************************************//** * @brief * The initDialOutFlow function initializes the DialOutFlow module. * @details Inputs: none * @details Outputs: dialOutFlowDataPublicationTimerCounter * @return none *************************************************************************/ void initDialOutFlow( void ) { U32 i; dialOutFlowDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; signalDialOutPumpHardStop(); setDialOutPumpDirection( MOTOR_DIR_FORWARD ); // Zero motor hall sensors counts buffer dopMotorSpeedCalcIdx = 0; for ( i = 0; i < DOP_SPEED_CALC_BUFFER__LEN; i++ ) { dopLastMotorHallSensorCounts[ i ] = getFPGADialOutPumpHallSensorCount(); } dopMeasuredRate = 0.0; ufMeasuredRate = 0.0; dopRateCorrectionOffset = 0.0; offsetPWMDutyCyclePct = 0.0; resetDialOutFlowMovingAverage(); initTimeWindowedCount( TIME_WINDOWED_COUNT_DOP_COMMUTATION_ERROR, DOP_COMMUTATION_ERROR_MAX_CNT, DOP_COMMUTATION_ERROR_TIME_WIN_MS ); } /*********************************************************************//** * @brief * The setDialOutPumpTargetRate function sets a new target flow rate, pump * direction, and control mode. * @details Inputs: isDialOutPumpOn, dialOutPumpDirectionSet, dopRateCorrectionOffset, * dialOutPumpState * @details Outputs: targetDialOutFlowRate, dialOutPumpdirection, dialOutPumpPWMDutyCyclePct, * dopRateCorrectionOffset, offsetPWMDutyCyclePct, dopControlSignal, dialOutPumpDirection, * dialOutPumpControlMode * @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 ) ) { F32 pwmDC = getDialInPumpPWMDutyCyclePct( TRUE ); // start initial DPo PWM duty cycle % estimate at whatever DPi PWM duty cycle % was set to. if ( mode != PUMP_CONTROL_MODE_CLOSED_LOOP ) { pwmDC = ( 0 == flowRate ? DOP_PWM_ZERO_OFFSET : DOP_PWM_FROM_ML_PER_MIN( (F32)flowRate ) ); } else { // set initial estimate for rate correction offset dopRateCorrectionOffset = ( (F32)getTargetDialInFlowRate() + getCurrentUFSetRate() ) * DOP_RATE_CORRECTION_SCALAR + DOP_RATE_CORRECTION_OFFSET; // adjust initial pwm duty cycle % estimate per set UF rate and rate correction offset offsetPWMDutyCyclePct = ( ( ( getCurrentUFSetRate() - dopRateCorrectionOffset ) / OFFSET_2_PWM_OFFSET ) / DOP_100_PCT_PWM_RPM_RANGE ); pwmDC += offsetPWMDutyCyclePct; } // Don't interrupt pump control unless rate or mode is changing if ( ( fabs( pwmDC - dialOutPumpPWMDutyCyclePct ) > NEARLY_ZERO ) || ( mode != dialOutPumpControlMode ) ) { #ifndef _RELEASE_ BOOL byPassFlowLimit = FALSE; if ( SW_CONFIG_ENABLE_VALUE == getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMPS_FLOW_LIMITS ) ) { byPassFlowLimit = TRUE; } #endif // Verify flow rate #ifdef _RELEASE_ if ( flowRate <= MAX_DIAL_OUT_FLOW_RATE ) #else if ( ( flowRate <= MAX_DIAL_OUT_FLOW_RATE ) || ( TRUE == byPassFlowLimit ) ) #endif { resetDialOutFlowMovingAverage(); dopControlSignal = FALSE; 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 will control to flow when ramp completed dialOutPumpPWMDutyCyclePct = pwmDC; dialOutPumpPWMDutyCyclePct = RANGE( dialOutPumpPWMDutyCyclePct, MIN_DIAL_OUT_PUMP_PWM_DUTY_CYCLE, MAX_DIAL_OUT_PUMP_PWM_DUTY_CYCLE ); 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 { #ifndef _RELEASE_ if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMPS_FLOW_LIMITS ) != SW_CONFIG_ENABLE_VALUE ) #endif { 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 setDialOutPumpTargetRPM function sets a new target pump speed and pump * direction. Pump is set to open loop control. * @details Inputs: none * @details Outputs: none * @param rpm new target dialysate outlet pump speed (in RPM) * @param dir new dialysate outlet flow direction * @return TRUE if new flow rate & direction are set, FALSE if not *************************************************************************/ BOOL setDialOutPumpTargetRPM( U32 rpm, MOTOR_DIR_T dir ) { BOOL result = FALSE; F32 pwm = DOP_MOTOR_SPEED_RPM_TO_PWM( rpm ); F32 targetRate = DOP_ML_PER_MIN_FROM_PWM( pwm ); result = setDialOutPumpTargetRate( (U32)targetRate, dir, PUMP_CONTROL_MODE_OPEN_LOOP ); return result; } /*********************************************************************//** * @brief * The resetDialOutRateOffset function resets the dialysate outlet pump * rate correction offset. Call this function before starting a new treatment. * @details Inputs: none * @details Outputs: dopRateCorrectionOffset * @return none *************************************************************************/ void resetDialOutRateOffset( void ) { dopRateCorrectionOffset = 0.0F; } /*********************************************************************//** * @brief * The setDialOutUFVolumes function sets the ultrafiltration reference and * measured total volumes (in mL). * @details Inputs: none * @details 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 * @details 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 = DOP_PWM_ZERO_OFFSET; resetDialOutFlowMovingAverage(); } /*********************************************************************//** * @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 * @details Outputs: dopRotorRevStartTime, dialOutPumpRotorSpeedRPM * @return none *************************************************************************/ void signalDialOutPumpRotorHallSensor( void ) { U32 rotTime = getMSTimerCount(); U32 deltaTime = calcTimeBetween( dopRotorRevStartTime, rotTime ); // Calculate rotor speed (in RPM) dialOutPumpRotorSpeedRPM.data = ( 1.0F / (F32)deltaTime ) * (F32)MS_PER_SECOND * (F32)SEC_PER_MIN; dopRotorRevStartTime = rotTime; // If we are supposed to stop pump at home position, stop pump now. if ( TRUE == dopStopAtHomePosition ) { signalDialOutPumpHardStop(); dopStopAtHomePosition = FALSE; } } /*********************************************************************//** * @brief * The signalDialOutControl function signals the dialysate outlet pump to * perform a control. Call this function when dialysate inlet pump control * has completed. * @details Inputs: none * @details Outputs: dopControlSignal * @return none *************************************************************************/ void signalDialOutControl( void ) { dopControlSignal = TRUE; } /*********************************************************************//** * @brief * The resetDialOutFlowMovingAverage function resets the properties of the * dialysate outlet flow moving average sample buffer. * @details Inputs: none * @details Outputs: flowReadingsTotal, flowReadingsIdx, flowReadingsCount all set to zero. * @return none *************************************************************************/ static void resetDialOutFlowMovingAverage( void ) { flowReadingsIdx = 0; flowReadingsCount = 0; flowReadingsTotal = 0.0; } /*********************************************************************//** * @brief * The filterDialOutFlowReadings function adds a new flow sample to the filter. * @details Inputs: none * @details Outputs: flowReadings[], flowReadingsIdx, flowReadingsCount, flowReadingsTotal * @return none *************************************************************************/ static void filterDialOutFlowReadings( F64 flow ) { if ( flowReadingsCount >= SIZE_OF_ROLLING_AVG ) { flowReadingsTotal -= flowReadings[ flowReadingsIdx ]; } flowReadings[ flowReadingsIdx ] = flow; flowReadingsTotal += flow; flowReadingsIdx = INC_WRAP( flowReadingsIdx, 0, SIZE_OF_ROLLING_AVG - 1 ); flowReadingsCount = INC_CAP( flowReadingsCount, SIZE_OF_ROLLING_AVG ); dopMeasuredRate = (F32)( flowReadingsTotal / (F64)flowReadingsCount ) + dopRateCorrectionOffset; } /*********************************************************************//** * @brief * The homeDialOutPump function initiates a dialysate outlet pump home operation. * @details Inputs: dialOutPumpState * @details 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 = setDialOutPumpTargetRPM( DOP_HOME_SPEED, MOTOR_DIR_FORWARD ); } return result; } /*********************************************************************//** * @brief * The isDialOutPumpRunning function returns whether the dialysate outlet * pump is currently running or not. * @details Inputs: isDialOutPumpOn * @details Outputs: none * @return isDialOutPumpOn *************************************************************************/ BOOL isDialOutPumpRunning( void ) { return isDialOutPumpOn; } /*********************************************************************//** * @brief * The isDialOutPumpRampComplete function returns whether the dialysate outlet pump has * completed its ramp up and entered control state (closed or open loop). * @details Inputs: dialOutPumpState * @details Outputs: none * @return TRUE if pump is in control state, FALSE if not *************************************************************************/ BOOL isDialOutPumpRampComplete( void ) { BOOL result = ( DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE == dialOutPumpState ? TRUE : FALSE ); return result; } /*********************************************************************//** * @brief * The execDialOutFlowTest function executes the state machine for the * DialOutFlow self-test. * @details Inputs: none * @details Outputs: dialOutPumpCalRecord * @return the current state of the DialOutFlow self-test. *************************************************************************/ SELF_TEST_STATUS_T execDialOutFlowTest( void ) { SELF_TEST_STATUS_T result = SELF_TEST_STATUS_PASSED; BOOL calStatus = getNVRecord2Driver( GET_CAL_PUMPS, (U08*)&dialOutPumpCalRecord, sizeof( HD_PUMPS_CAL_RECORD_T ), NUM_OF_CAL_DATA_HD_PUMPS, ALARM_ID_NO_ALARM ); if ( TRUE == calStatus ) { result = SELF_TEST_STATUS_PASSED; } else { result = SELF_TEST_STATUS_FAILED; } 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, flowFilterTimerCtr, ufCalcTimerCtr * @details Outputs: dialOutPumpMCSpeedRPM, dialOutPumpMCCurrentmA, ufMeasuredRate, * dopMeasuredRate, flowFilterTimerCtr, ufCalcTimerCtr * @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(); // Filter estimated dialysate out flow rate and estimate UF rate if running pump in closed loop mode if ( ( TRUE == isDialOutPumpOn ) && ( PUMP_CONTROL_MODE_CLOSED_LOOP == dialOutPumpControlMode ) ) { if ( DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE == dialOutPumpState ) { // pump is controlling if ( ++flowFilterTimerCtr >= DIAL_OUT_FILTER_INTERVAL ) { // Calculate DPo flow in mL/min flowFilterTimerCtr = 0; filterDialOutFlowReadings( getMeasuredDialOutPumpMCSpeed() * RPM_2_ML_MIN_CONVERSION ); } if ( ++ufCalcTimerCtr >= DIAL_OUT_UF_CALC_INTERVAL ) { // Calculate UF flow in mL/min ufCalcTimerCtr = 0; ufMeasuredRate = dopMeasuredRate - getMeasuredDialInFlowRate(); } } else { // pump is ramping flowFilterTimerCtr = 0; ufCalcTimerCtr = 0; ufMeasuredRate = getCurrentUFSetRate(); // UF calculation won't work while ramping so just set to set UF rate dopMeasuredRate = getMeasuredDialInFlowRate() + ufMeasuredRate; // and set flow rate to in flow + set UF rate } } else // pump is off or in open loop mode { if ( DIAL_OUT_PUMP_OFF_STATE == dialOutPumpState ) { dopMeasuredRate = 0.0F; } else { dopMeasuredRate = lastGivenRate; } ufMeasuredRate = 0.0F; flowFilterTimerCtr = 0; ufCalcTimerCtr = 0; } // Do not 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(); } else { lastDialOutPumpDirectionCount = getFPGADialOutPumpHallSensorStatus() & PUMP_DIR_ERROR_COUNT_MASK; } publishDialOutFlowData(); } /*********************************************************************//** * @brief * The execDialOutFlowController function executes the dialysate outlet pump * ultrafiltration controller state machine. * @details Inputs: dialOutPumpState * @details 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 * @details 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 have 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 { isDialOutPumpOn = FALSE; dialOutPumpPWMDutyCyclePct = 0.0; } return result; } /*********************************************************************//** * @brief * The handleDialOutPumpRampingUpState function handles the ramp up state * of the dialOut pump controller state machine. * @details Inputs: dialOutPumpPWMDutyCyclePctSet * @details 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 < (MAX_DIAL_OUT_PUMP_PWM_STEP_DN_CHANGE + DOP_PWM_ZERO_OFFSET) ) { // 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 ) { dialOutPumpPWMDutyCyclePctSet = dialOutPumpPWMDutyCyclePct; dialOutPumpControlModeSet = dialOutPumpControlMode; resetDialOutFlowMovingAverage(); setDialOutPumpControlSignalPWM( dialOutPumpPWMDutyCyclePctSet ); 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 * @details 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_DN_CHANGE + DOP_PWM_ZERO_OFFSET) ) { stopDialOutPump(); result = DIAL_OUT_PUMP_OFF_STATE; } // Have we reached end of ramp down? else if ( dialOutPumpPWMDutyCyclePctSet <= dialOutPumpPWMDutyCyclePct ) { dialOutPumpPWMDutyCyclePctSet = dialOutPumpPWMDutyCyclePct; dialOutPumpControlModeSet = dialOutPumpControlMode; resetDialOutFlowMovingAverage(); setDialOutPumpControlSignalPWM( dialOutPumpPWMDutyCyclePctSet ); 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. * @details 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 when signalled (sync'd with dialysate inlet pump control - we want this control to follow inlet pump) if ( TRUE == dopControlSignal ) { if ( dialOutPumpControlModeSet == PUMP_CONTROL_MODE_CLOSED_LOOP ) { F32 refVol = getTotalTargetDialOutUFVolumeInMl(); F32 totVol = getTotalMeasuredUFVolumeInMl(); F32 volErr = refVol - totVol; F32 ufrErr = getCurrentUFSetRate() - ufMeasuredRate; dopControlSignal = FALSE; dopRateCorrectionOffset += ( ( totVol - refVol ) * P_CORR ); offsetPWMDutyCyclePct += volErr * P_VOL; offsetPWMDutyCyclePct += ( ufrErr * P_UF ); // Add PWM offset to DPi PWM mirror for our new DPo PWM dialOutPumpPWMDutyCyclePctSet = getDialInPumpPWMDutyCyclePct( FALSE ) + offsetPWMDutyCyclePct; // Limit PWM range dialOutPumpPWMDutyCyclePctSet = MIN( dialOutPumpPWMDutyCyclePctSet, MAX_DIAL_OUT_PUMP_PWM_DUTY_CYCLE ); dialOutPumpPWMDutyCyclePctSet = MAX( dialOutPumpPWMDutyCyclePctSet, MIN_DIAL_OUT_PUMP_PWM_DUTY_CYCLE ); // Apply new PWM to DPo pump setDialOutPumpControlSignalPWM( dialOutPumpPWMDutyCyclePctSet ); } } return result; } /*********************************************************************//** * @brief * The setDialOutPumpControlSignalPWM function set the PWM duty cycle of * the dialysate outlet pump to a given %. * @details Inputs: none * @details 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 * @details Outputs: dialOut pump stop signal activated, PWM duty cycle zeroed * @return none *************************************************************************/ static void stopDialOutPump( void ) { dialOutPumpPWMDutyCyclePctSet = DOP_PWM_ZERO_OFFSET; setDialOutPumpControlSignalPWM( 0 ); SET_DOP_STOP(); isDialOutPumpOn = FALSE; } /*********************************************************************//** * @brief * The releaseDialOutPumpStop function clears the dialysate outlet pump stop signal. * @details Inputs: none * @details 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 * @details 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 * @details 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 >= getU32OverrideValue( &dialOutDataPublishInterval ) ) { DIAL_OUT_FLOW_DATA_T dialOutBroadCastVariables; U32 hallSensor = gioGetBit( hetPORT1, DOP_ROTOR_HALL_SENSOR_NHET_ID ); 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; dialOutBroadCastVariables.dopCorrOffset = dopRateCorrectionOffset; dialOutBroadCastVariables.dopCalcRate = dopMeasuredRate; dialOutBroadCastVariables.ufCalcRate = ufMeasuredRate; dialOutBroadCastVariables.rotorHall = ( hallSensor > 0 ? 0 : 1 ); // 1=home, 0=not home dialOutBroadCastVariables.currentSetUFRate = getCurrentUFSetRate(); broadcastData( MSG_ID_DIALYSATE_OUT_FLOW_DATA, COMM_BUFFER_OUT_CAN_HD_BROADCAST, (U08*)&dialOutBroadCastVariables, sizeof( DIAL_OUT_FLOW_DATA_T ) ); 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 * @details Outputs: dopMotorDirectionFromHallSensors, dialOutPumpSpeedRPM * @return none *************************************************************************/ static void updateDialOutPumpSpeedAndDirectionFromHallSensors( void ) { if ( ++dopMotorSpeedCalcTimerCtr >= DOP_SPEED_CALC_INTERVAL ) { U16 dopMotorHallSensorCount = getFPGADialOutPumpHallSensorCount(); U32 nextIdx = INC_WRAP( dopMotorSpeedCalcIdx, 0, DOP_SPEED_CALC_BUFFER__LEN - 1 ); U16 incDelta = ( dopMotorHallSensorCount >= dopLastMotorHallSensorCounts[ nextIdx ] ? \ dopMotorHallSensorCount - dopLastMotorHallSensorCounts[ nextIdx ] : \ ( HEX_64_K - dopLastMotorHallSensorCounts[ nextIdx ] ) + dopMotorHallSensorCount ); U16 decDelta = ( 0 == incDelta ? 0xFFFF : HEX_64_K - incDelta ); U16 delta; // Determine dialysate outlet pump speed/direction from delta hall sensor count since last interval if ( incDelta < decDelta ) { delta = incDelta; dialOutPumpSpeedRPM.data = ( (F32)delta / (F32)DOP_HALL_EDGE_COUNTS_PER_REV ) * (F32)SEC_PER_MIN; } else { 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 dopLastMotorHallSensorCounts[ nextIdx ] = dopMotorHallSensorCount; dopMotorSpeedCalcIdx = nextIdx; 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 * @details 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 * @details Outputs: none * @return none *************************************************************************/ static void checkDialOutPumpDirection( void ) { if ( DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE == dialOutPumpState ) { MOTOR_DIR_T dopMCDir, dopDir; U08 dirErrorCnt = getFPGADialOutPumpHallSensorStatus() & PUMP_DIR_ERROR_COUNT_MASK; F32 measMCSpeed = getMeasuredDialOutPumpMCSpeed(); BOOL minDirSpeed = ( fabs( measMCSpeed ) >= DOP_MIN_DIR_CHECK_SPEED_RPM ? TRUE : FALSE ); BOOL isHallSensorFailed = ( TRUE == minDirSpeed && lastDialOutPumpDirectionCount != dirErrorCnt ? TRUE : FALSE ); // Check pump direction error count if ( ( TRUE == isHallSensorFailed ) && ( TRUE == incTimeWindowedCount( TIME_WINDOWED_COUNT_DOP_COMMUTATION_ERROR ) ) ) { #ifndef _RELEASE_ if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMP_DIRECTION_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) #endif { SET_ALARM_WITH_1_U32_DATA( ALARM_ID_HD_PUMP_DIRECTION_STATUS_ERROR, (U32)HD_PUMP_DIALYSATE_OUTLET_PUMP ) } } lastDialOutPumpDirectionCount = dirErrorCnt; dopMCDir = ( getMeasuredDialOutPumpMCSpeed() >= 0.0 ? MOTOR_DIR_FORWARD : MOTOR_DIR_REVERSE ); dopDir = ( getMeasuredDialOutPumpSpeed() >= 0.0 ? MOTOR_DIR_FORWARD : MOTOR_DIR_REVERSE ); // Check set direction vs. direction from hall sensors if ( ( dialOutPumpDirectionSet != dopDir ) && ( TRUE == minDirSpeed ) ) { if ( ++errorDialOutPumpDirectionPersistTimerCtr >= DOP_DIRECTION_ERROR_PERSIST ) { #ifndef _RELEASE_ if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMP_DIRECTION_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) #endif { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_DIAL_OUT_PUMP_MC_DIRECTION_CHECK, (U32)dialOutPumpDirectionSet, (U32)dopDir ) } } } // Check set direction vs. direction from sign of motor controller speed else if ( ( dialOutPumpDirectionSet != dopMCDir ) && ( TRUE == minDirSpeed ) ) { if ( ++errorDialOutPumpDirectionPersistTimerCtr >= DOP_DIRECTION_ERROR_PERSIST ) { #ifndef _RELEASE_ if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMP_DIRECTION_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) #endif { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_DIAL_OUT_PUMP_MC_DIRECTION_CHECK, (U32)dialOutPumpDirectionSet, (U32)dopMCDir ) } } } else { errorDialOutPumpDirectionPersistTimerCtr = 0; } } else { errorDialOutPumpDirectionPersistTimerCtr = 0; } } /*********************************************************************//** * @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: dialOutPumpPWMDutyCyclePctSet, dialOutPumpState, dialOutPumpPWMDutyCyclePctSet, dialOutPumpDirectionSet * @details Outputs: errorDialOutMotorOffPersistTimerCtr, alarm(s) may be triggered * @return none *************************************************************************/ static void checkDialOutPumpSpeeds( void ) { F32 measMotorSpeed = getMeasuredDialOutPumpSpeed(); F32 measMCMotorSpeed = getMeasuredDialOutPumpMCSpeed(); // Check for pump running while commanded off if ( dialOutPumpPWMDutyCyclePctSet <= DOP_PWM_ZERO_OFFSET ) { if ( measMotorSpeed > DOP_MAX_MOTOR_SPEED_WHILE_OFF_RPM ) { if ( ++errorDialOutMotorOffPersistTimerCtr >= DOP_OFF_ERROR_PERSIST ) { #ifndef _RELEASE_ if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMP_SPEED_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) #endif { SET_ALARM_WITH_1_F32_DATA( ALARM_ID_HD_DIAL_OUT_PUMP_OFF_CHECK, measMotorSpeed ); activateSafetyShutdown(); } } } else { errorDialOutMotorOffPersistTimerCtr = 0; } } else { errorDialOutMotorOffPersistTimerCtr = 0; } if ( DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE == dialOutPumpState ) { F32 cmdMotorSpeed = DOP_PWM_TO_MOTOR_SPEED_RPM( dialOutPumpPWMDutyCyclePctSet, dialOutPumpDirectionSet ); F32 deltaMotorSpeed = fabs( measMotorSpeed - cmdMotorSpeed ); F32 deltaMCMotorSpeed = fabs( measMCMotorSpeed - cmdMotorSpeed ); F32 measRotorSpeed = fabs( getMeasuredDialOutPumpRotorSpeed() ); F32 measMotorSpeedInRotorRPM = fabs( measMotorSpeed / DOP_GEAR_RATIO ); F32 deltaRotorSpeed = fabs( measRotorSpeed - measMotorSpeedInRotorRPM ); F32 measMotorSpeedDeltaPct = fabs( deltaRotorSpeed / measMotorSpeedInRotorRPM ); // Check measured motor speed vs. commanded motor speed while controlling to target if ( ( deltaMotorSpeed > DOP_MAX_MOTOR_SPEED_ERROR_RPM ) || ( deltaMCMotorSpeed > DOP_MAX_MOTOR_SPEED_ERROR_RPM ) ) { if ( ++errorDialOutMotorSpeedPersistTimerCtr >= DOP_MOTOR_SPEED_ERROR_PERSIST ) { #ifndef _RELEASE_ if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMP_SPEED_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) #endif { SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_DIAL_OUT_PUMP_MOTOR_SPEED_CHECK, cmdMotorSpeed, measMotorSpeed ); } } } else { errorDialOutMotorSpeedPersistTimerCtr = 0; } // Check measured rotor speed vs. measured motor speed while controlling to target if ( ( deltaRotorSpeed > DOP_MAX_ROTOR_VS_MOTOR_DIFF_RPM ) && ( measMotorSpeedDeltaPct > DOP_MAX_MOTOR_SPEED_VS_TRGT_DIFF_PCT ) ) { if ( ++errorDialOutRotorSpeedPersistTimerCtr >= ( getPumpRotorErrorPersistTime( measMotorSpeed, DOP_GEAR_RATIO ) / TASK_PRIORITY_INTERVAL ) ) { #ifndef _RELEASE_ if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMP_SPEED_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) #endif { SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_DIAL_OUT_PUMP_ROTOR_SPEED_CHECK, measRotorSpeed, measMotorSpeed ); } } } 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 * @details Outputs: none * @return none *************************************************************************/ static void checkDialOutPumpMCCurrent( void ) { F32 dopCurr; // only check current when we have A/C power if ( getCPLDACPowerLossDetected() != TRUE ) { // 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 _RELEASE_ if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_MOTOR_CURRNT_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) #endif { SET_ALARM_WITH_1_F32_DATA( ALARM_ID_HD_DIAL_OUT_PUMP_MC_CURRENT_CHECK, getMeasuredDialOutPumpMCCurrent() ); } } } else { dopCurrErrorDurationCtr = 0; } } // DialOut pump should be running else { dopCurr = fabs( getMeasuredDialOutPumpMCCurrent() ); if ( dopCurr > DOP_MAX_CURR_WHEN_RUNNING_MA ) { dopCurrErrorDurationCtr += TASK_PRIORITY_INTERVAL; if ( dopCurrErrorDurationCtr > DOP_MAX_CURR_ERROR_DURATION_MS ) { #ifndef _RELEASE_ if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_MOTOR_CURRNT_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) #endif { SET_ALARM_WITH_1_F32_DATA( ALARM_ID_HD_DIAL_OUT_PUMP_MC_CURRENT_CHECK, getMeasuredDialOutPumpMCCurrent() ); } } } else { dopCurrErrorDurationCtr = 0; } } } } /************************************************************************* * GET SUPPORT FUNCTIONS *************************************************************************/ /*********************************************************************//** * @brief * The setDialOutPumpToFixedPWM function sets a new pwm value and pump direction. * @details Inputs: isDialOutPumpOn, dialOutPumpPWMDutyCyclePct, dialOutPumpDirectionSet, * dialOutPumpState * @details Outputs: dialOutPumpControlMode, dialOutPumpDirection, dialOutPumpPWMDutyCyclePct * @param pwm the new pwm value * @return TRUE if new flow rate & dir are set, FALSE if not **************************************************************************/ static BOOL setDialOutPumpToFixedPWM( F32 pwm ) { MOTOR_DIR_T dir = ( pwm < 0.0F ? MOTOR_DIR_REVERSE : MOTOR_DIR_FORWARD ); BOOL result = FALSE; F32 pwmFabs = fabs(pwm); // Direction change while pump is running is not allowed unless we are turning off if ( ( FALSE == isDialOutPumpOn ) && ( dir != dialOutPumpDirectionSet ) ) { dialOutPumpDirection = dir; } // Direction change while pump is running is not allowed if ( ( ( FALSE == isDialOutPumpOn ) || ( dir == dialOutPumpDirectionSet ) ) || ( pwmFabs <= MIN_DIAL_OUT_PUMP_PWM_DUTY_CYCLE ) ) { // Don't interrupt pump control unless rate is changing if ( ( pwmFabs != dialOutPumpPWMDutyCyclePct ) ) { resetDialOutFlowMovingAverage(); dialOutPumpControlMode = PUMP_CONTROL_MODE_OPEN_LOOP; dialOutPumpPWMDutyCyclePct = RANGE( pwmFabs, MIN_DIAL_OUT_PUMP_PWM_DUTY_CYCLE, MAX_DIAL_OUT_PUMP_PWM_DUTY_CYCLE ); lastGivenRate = DOP_ML_PER_MIN_FROM_PWM_BASIC( pwmFabs ); 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; } } return result; } /*********************************************************************//** * @brief * The getTotalTargetDialOutUFVolumeInMl function gets the target UF volume. * @details Inputs: referenceUFVolumeInMl * @details 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 * @details 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 * @details 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 * @details 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 * @details 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 * @details 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 * @details 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 * @details 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 * @details 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 ) { if ( 0 == value ) { signalDialOutPumpHardStop(); result = TRUE; } else { result = setDialOutPumpTargetRate( abs(value), dir, (PUMP_CONTROL_MODE_T)ctrlMode ); } } } return result; } /*********************************************************************//** * @brief * The testSetDialOutUFRefVolumeOverride function overrides the target * dialout vol rate. * @details Inputs: referenceUFVolumeInMl * @details 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 * @details 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 * @details 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 * @details 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 * @details 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 * @details 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 * @details 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 * @details 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 * @details 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 * @details 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 * @details 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 * @details 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 testSetBloodPumpTargetDutyCycle function sets the duty cycle of the * blood pump by calling setDialOutPumpTargetRate. * @details Inputs: none * @details Outputs: none * @param value duty cycle of the dialysate outlet pump (as a percentage). * @return TRUE if reset successful, FALSE if not *************************************************************************/ BOOL testSetDialOutPumpTargetDutyCycle( F32 value ) { BOOL result = FALSE; F32 absolutePWM = fabs( value ); // check for max of pump pwm for acceptance. *** Function used in dialyzer re-prime, so no Dialin login required *** if ( absolutePWM < MAX_DIAL_OUT_PUMP_PWM_DUTY_CYCLE ) { result = setDialOutPumpToFixedPWM( value ); } return result; } /**@}*/