/************************************************************************** * * 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 DialInFlow.c * * @author (last) Sean Nash * @date (last) 13-Oct-2020 * * @author (original) Sean * @date (original) 16-Dec-2019 * ***************************************************************************/ #include #include "etpwm.h" #include "gio.h" #include "mibspi.h" #include "DialInFlow.h" #include "FPGA.h" #include "InternalADC.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 DialysateInletFlow * @{ */ // ********** private definitions ********** /// Interval (ms/task time) at which the dialIn flow data is published on the CAN bus. #define DIAL_IN_FLOW_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) #define MAX_DIAL_IN_PUMP_PWM_STEP_UP_CHANGE 0.0133 ///< Max duty cycle change when ramping up ~ 200 mL/min/s. #define MAX_DIAL_IN_PUMP_PWM_STEP_DN_CHANGE 0.02 ///< Max duty cycle change when ramping down ~ 300 mL/min/s. #define MAX_DIAL_IN_PUMP_PWM_DUTY_CYCLE 0.89 ///< Controller will error if PWM duty cycle > 90%, so set max to 89%. #define MIN_DIAL_IN_PUMP_PWM_DUTY_CYCLE 0.10 ///< Controller will error if PWM duty cycle < 10%, so set min to 10%. #define DIP_CONTROL_INTERVAL_SEC 10 ///< Dialysate inlet pump control interval (in seconds). /// Interval (ms/task time) at which the dialIn pump is controlled. static const U32 DIP_CONTROL_INTERVAL = ( DIP_CONTROL_INTERVAL_SEC * MS_PER_SECOND / TASK_GENERAL_INTERVAL ); #define DIP_P_COEFFICIENT 0.0001 ///< P term for dialIn pump control. #define DIP_I_COEFFICIENT 0.00075 ///< I term for dialIn pump control. #define DIP_HOME_RATE 100 ///< Target pump speed (in estimate mL/min) for homing. #define DIP_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 DIP_SPEED_CALC_INTERVAL ( 40 / TASK_PRIORITY_INTERVAL ) /// Number of hall sensor counts kept in buffer to hold last 1 second of count data. #define DIP_SPEED_CALC_BUFFER_LEN ( 1000 / DIP_SPEED_CALC_INTERVAL / TASK_PRIORITY_INTERVAL ) #define DIP_HALL_EDGE_COUNTS_PER_REV 48 ///< Number of hall sensor edge counts per motor revolution. #define DIP_MAX_FLOW_RATE 1320.0 ///< Maximum measured BP flow rate allowed. #define DIP_MIN_FLOW_RATE -1320.0 ///< Minimum measured BP flow rate allowed. #ifdef USE_FMD_FLOW_SENSOR #define DIP_MAX_FLOW_VS_SPEED_DIFF_RPM 200.0 ///< Maximum difference between measured motor speed and speed implied by measured flow. #endif #define DIP_MAX_MOTOR_SPEED_WHILE_OFF_RPM 100.0 ///< Maximum motor speed (RPM) while motor is commanded off. #define DIP_MAX_ROTOR_VS_MOTOR_DIFF_RPM 5.0 ///< Maximum difference in speed between motor and rotor (in rotor RPM). #define DIP_MAX_MOTOR_SPEED_ERROR_RPM 300.0 ///< Maximum difference in speed between measured and commanded RPM. /// Persist time (task intervals) for flow vs. motor speed error condition. static const U32 DIP_FLOW_VS_SPEED_PERSIST = ((5 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL); /// Persist time (task intervals) for motor off error condition. static const U32 DIP_OFF_ERROR_PERSIST = ((5 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL); /// Persist time (task intervals) motor speed error condition. static const U32 DIP_MOTOR_SPEED_ERROR_PERSIST = ((5 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL); /// Persist time (task intervals) rotor speed error condition. static const U32 DIP_ROTOR_SPEED_ERROR_PERSIST = ((12 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL); /// Persist time (task intervals) pump direction error condition. static const U32 DIP_DIRECTION_ERROR_PERSIST = (250 / TASK_PRIORITY_INTERVAL); /// Persist time (task intervals) dialysate flow rate out of range error condition. static const U32 DIP_MAX_FLOW_RATE_OUT_OF_RANGE_PERSIST = ((1 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL); #define DIP_MAX_CURR_WHEN_STOPPED_MA 150.0 ///< Motor controller current should not exceed this when pump should be stopped. #define DIP_MAX_CURR_WHEN_RUNNING_MA 2000.0 ///< Motor controller current should not exceed this when pump should be running. #define DIP_MAX_CURR_ERROR_DURATION_MS 2000 ///< Motor controller current errors persisting beyond this duration will trigger an alarm. #define DIP_SPEED_ADC_TO_RPM_FACTOR 1.751752 ///< Conversion factor from ADC counts to RPM for dialIn pump motor. #define DIP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.000193 ///< ~52 BP motor RPM = 1% PWM duty cycle #define DIP_CURRENT_ADC_TO_MA_FACTOR 3.002 ///< Conversion factor from ADC counts to mA for dialIn pump motor. #define DIP_REV_PER_LITER 144.7 ///< Rotor revolutions per liter. /// Macro converts flow rate to motor RPM. #define DIP_ML_PER_MIN_TO_PUMP_RPM_FACTOR ( DIP_REV_PER_LITER / ML_PER_LITER ) #define DIP_GEAR_RATIO 32.0 ///< DialIn pump motor to dialIn pump gear ratio. #define DIP_PWM_ZERO_OFFSET 0.1 ///< 10% PWM duty cycle = zero speed. /// Macro converts flow rate to estimate PWM needed to achieve it. #define DIP_PWM_FROM_ML_PER_MIN(rate) ( (rate) * DIP_ML_PER_MIN_TO_PUMP_RPM_FACTOR * DIP_GEAR_RATIO * DIP_MOTOR_RPM_TO_PWM_DC_FACTOR + DIP_PWM_ZERO_OFFSET ) /// Conversion from PWM duty cycle % to commanded pump motor speed. #define DIP_PWM_TO_MOTOR_SPEED_RPM(pwm) ( ((pwm) - DIP_PWM_ZERO_OFFSET) * 4000.0 ) #define DIAL_IN_PUMP_ADC_FULL_SCALE_V 3.0 ///< BP analog signals are 0-3V (while int. ADC ref V may be different). #define DIAL_IN_PUMP_ADC_ZERO 1998 ///< Mid-point (zero) for ADC readings. ///< Macro converts a 12-bit ADC reading to a signed 16-bit value. #define SIGN_FROM_12_BIT_VALUE(v) ( (S16)(v) - (S16)DIAL_IN_PUMP_ADC_ZERO ) /// Measured dialIn flow is filtered w/ moving average. #define SIZE_OF_ROLLING_AVG ( ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) * DIP_CONTROL_INTERVAL_SEC ) #ifdef USE_FMD_FLOW_SENSOR /// Dialysate flow sensor signal strength low alarm persistence. #define FLOW_SIG_STRGTH_ALARM_PERSIST ( 5 * MS_PER_SECOND ) #define MIN_FLOW_SIG_STRENGTH 0.9 ///< Minimum flow sensor signal strength (90%). /// Dialysate flow fast read timeout alarm persistence. #define DIALYSATE_FLOW_FAST_READ_TO_PERSIST 100 /// Dialysate flow slow read timeout alarm persistence. #define DIALYSATE_FLOW_SLOW_READ_TO_PERSIST ( MS_PER_SECOND * 3 ) /// Blood flow comm error persistence. #define DIALYSATE_FLOW_COMM_ERROR_PERSIST MS_PER_SECOND #define DFM_SENSOR_CONNECTED_STATUS 0x00 ///< Dialysate flow meter connected status. #define DFM_SENSOR_PARAM_CORRUPT_STATUS 0x07 ///< Dialysate flow meter NVM parameter corrupt status. #endif #define PUMP_DIR_ERROR_COUNT_MASK 0x3F ///< Bit mask for pump direction error counter. /// Enumeration of dialysate inlet pump states. typedef enum DialInPump_States { DIAL_IN_PUMP_OFF_STATE = 0, ///< Off state for the dialysate inlet pump. DIAL_IN_PUMP_RAMPING_UP_STATE, ///< Ramping up state for the dialysate inlet pump. DIAL_IN_PUMP_RAMPING_DOWN_STATE, ///< Ramping down state for the dialysate inlet pump. DIAL_IN_PUMP_CONTROL_TO_TARGET_STATE, ///< Control to target state for the dialysate inlet pump. NUM_OF_DIAL_IN_PUMP_STATES ///< Number of dialysate inlet pump states. } DIAL_IN_PUMP_STATE_T; /// Enumeration of dialysate inlet self-test states. typedef enum DialInFlow_Self_Test_States { DIAL_IN_FLOW_SELF_TEST_STATE_START = 0, ///< Start state for the dialysate inlet pump self-test. DIAL_IN_FLOW_TEST_STATE_IN_PROGRESS, ///< Test in progress state for the dialysate inlet pump self-test. DIAL_IN_FLOW_TEST_STATE_COMPLETE, ///< Test completed state for the dialysate inlet pump self-test. NUM_OF_DIAL_IN_FLOW_SELF_TEST_STATES ///< Number of dialysate inlet pump self-test states. } DIAL_IN_FLOW_SELF_TEST_STATE_T; // Pin assignments for pump stop and direction outputs #define STOP_DI_PUMP_GIO_PORT_PIN 2U ///< Pin # on GIO A for stopping the dialysate inlet pump. #define DIR_DI_PUMP_SPI5_PORT_MASK 0x00000100 ///< Pin on unused SPI5 peripheral (ENA) - re-purposed as output GPIO to set dialysate inlet pump direction. // DialIn pump stop and direction macros #define SET_DIP_DIR() {mibspiREG5->PC3 |= DIR_DI_PUMP_SPI5_PORT_MASK;} ///< Macro for setting the dialysate inlet pump direction pin high. #define CLR_DIP_DIR() {mibspiREG5->PC3 &= ~DIR_DI_PUMP_SPI5_PORT_MASK;} ///< Macro for setting the dialysate inlet pump direction pin low. #define SET_DIP_STOP() gioSetBit( gioPORTA, STOP_DI_PUMP_GIO_PORT_PIN, PIN_SIGNAL_LOW ) ///< Macro for setting the dialysate inlet pump stop pin low. #define CLR_DIP_STOP() gioSetBit( gioPORTA, STOP_DI_PUMP_GIO_PORT_PIN, PIN_SIGNAL_HIGH ) ///< Macro for setting the dialysate inlet pump stop pin high. // ********** private data ********** static DIAL_IN_PUMP_STATE_T dialInPumpState = DIAL_IN_PUMP_OFF_STATE; ///< Current state of dialIn flow controller state machine static U32 dialInFlowDataPublicationTimerCounter = 5; ///< Used to schedule dialIn flow data publication to CAN bus static BOOL isDialInPumpOn = FALSE; ///< DialIn pump is currently running static F32 dialInPumpPWMDutyCyclePct = 0.0; ///< Initial dialIn pump PWM duty cycle static F32 dialInPumpPWMDutyCyclePctSet = 0.0; ///< Currently set dialIn pump PWM duty cycle static MOTOR_DIR_T dialInPumpDirection = MOTOR_DIR_FORWARD; ///< Requested dialysate flow direction static MOTOR_DIR_T dialInPumpDirectionSet = MOTOR_DIR_FORWARD; ///< Currently set dialysate flow direction static PUMP_CONTROL_MODE_T dialInPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; ///< Requested dialIn pump control mode. static PUMP_CONTROL_MODE_T dialInPumpControlModeSet = PUMP_CONTROL_MODE_CLOSED_LOOP;///< Currently set dialIn pump control mode. /// Interval (in ms) at which to publish dialIn flow data to CAN bus static OVERRIDE_U32_T dialInFlowDataPublishInterval = { DIAL_IN_FLOW_DATA_PUB_INTERVAL, DIAL_IN_FLOW_DATA_PUB_INTERVAL, DIAL_IN_FLOW_DATA_PUB_INTERVAL, 0 }; static S32 targetDialInFlowRate = 0; ///< Requested dialIn flow rate static OVERRIDE_F32_T measuredDialInFlowRate = { 0.0, 0.0, 0.0, 0 }; ///< Measured dialysate inlet flow rate static OVERRIDE_F32_T dialInPumpRotorSpeedRPM = { 0.0, 0.0, 0.0, 0 }; ///< Measured dialysate inlet pump rotor speed static OVERRIDE_F32_T dialInPumpSpeedRPM = { 0.0, 0.0, 0.0, 0 }; ///< Measured dialysate inlet pump motor speed static OVERRIDE_F32_T adcDialInPumpMCSpeedRPM = { 0.0, 0.0, 0.0, 0 }; ///< Measured dialysate inlet pump motor controller speed static OVERRIDE_F32_T adcDialInPumpMCCurrentmA = { 0.0, 0.0, 0.0, 0 }; ///< Measured dialysate inlet pump motor controller current #ifdef USE_FMD_FLOW_SENSOR static OVERRIDE_F32_T dialInFlowSignalStrength = { 0.0, 0.0, 0.0, 0 }; ///< Measured dialysate flow signal strength (%) static U08 lastDialysateFlowFastPacketReadCtr = 0; ///< Previous read counter for the dialysate flow fast packets. static U08 lastDialysateFlowSlowPacketReadCtr = 0; ///< Previous read counter for the dialysate flow slow packets. #ifndef DISABLE_PUMP_FLOW_CHECKS static U08 lastDialysateFlowCommErrorCount = 0; ///< Previous DPi flow sensor comm error count. #endif static HD_FLOW_SENSORS_CAL_RECORD_T dialysateFlowCalRecord; ///< Dialysate flow sensor calibration record. #endif static U08 lastDialInPumpDirectionCount = 0; ///< Previous pump direction error count reported by FPGA. static U32 dipControlTimerCounter = 0; ///< Determines when to perform control on dialIn flow static U32 dipRotorRevStartTime = 0; ///< Dialysate inlet pump rotor rotation start time (in ms) static BOOL dipStopAtHomePosition = FALSE; ///< Stop dialysate inlet pump at next home position static U32 dipHomeStartTime = 0; ///< When did dialysate inlet pump home command begin? (in ms) static U16 dipLastMotorHallSensorCounts[ DIP_SPEED_CALC_BUFFER_LEN ]; ///< Last hall sensor count for the dialysate inlet pump motor static U32 dipMotorSpeedCalcIdx = 0; ///< Index into 1 second buffer of motor speed hall sensor counts static U32 dipMotorSpeedCalcTimerCtr = 0; ///< Counter determines interval for calculating dialysate inlet pump motor speed from hall sensor count. static U32 errorDialInFlowVsMotorSpeedPersistTimerCtr = 0; ///< Persistence timer counter for flow vs. motor speed error condition. static U32 errorDialInMotorOffPersistTimerCtr = 0; ///< Persistence timer counter for motor off check error condition. static U32 errorDialInMotorSpeedPersistTimerCtr = 0; ///< Persistence timer counter for motor speed error condition. static U32 errorDialInRotorSpeedPersistTimerCtr = 0; ///< Persistence timer counter for rotor speed error condition. static U32 errorDialInPumpDirectionPersistTimerCtr = 0; ///< Persistence timer counter for pump direction error condition. static F32 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 F32 flowReadingsTotal = 0.0; ///< Rolling total - used to calc average. static U32 flowReadingsCount = 0; ///< Number of samples in flow rolling average buffer. static U32 dipCurrErrorDurationCtr = 0; ///< Used for tracking persistence of dip current errors. // ********** private function prototypes ********** static DIAL_IN_PUMP_STATE_T handleDialInPumpOffState( void ); static DIAL_IN_PUMP_STATE_T handleDialInPumpRampingUpState( void ); static DIAL_IN_PUMP_STATE_T handleDialInPumpRampingDownState( void ); static DIAL_IN_PUMP_STATE_T handleDialInPumpControlToTargetState( void ); static void setDialInPumpControlSignalPWM( F32 newPWM ); static void stopDialInPump( void ); static void releaseDialInPumpStop( void ); static void setDialInPumpDirection( MOTOR_DIR_T dir ); static void publishDialInFlowData( void ); static void resetDialInFlowMovingAverage( void ); static void filterDialInFlowReadings( F32 flow ); static void updateDialInPumpSpeedAndDirectionFromHallSensors( void ); static void checkDialInPumpRotor( void ); static void checkDialInPumpDirection( void ); static void checkDialInPumpSpeeds( void ); static void checkDialInPumpMCCurrent( void ); static void checkDialInPumpFlowRate( void ); #ifdef USE_FMD_FLOW_SENSOR static void checkDialInFlowSensorSignalStrength( void ); static BOOL processCalibrationData( void ); #endif /*********************************************************************//** * @brief * The initDialInFlow function initializes the DialInFlow module. * @details Inputs: none * @details Outputs: DialInFlow module initialized. * @return none *************************************************************************/ void initDialInFlow( void ) { U32 i; stopDialInPump(); setDialInPumpDirection( MOTOR_DIR_FORWARD ); // Zero rolling flow average buffer resetDialInFlowMovingAverage(); // Zero motor hall sensors counts buffer dipMotorSpeedCalcIdx = 0; for ( i = 0; i < DIP_SPEED_CALC_BUFFER_LEN; i++ ) { dipLastMotorHallSensorCounts[ i ] = 0; } // Initialize dialysate inlet flow PI controller initializePIController( PI_CONTROLLER_ID_DIALYSATE_FLOW, MIN_DIAL_IN_PUMP_PWM_DUTY_CYCLE, DIP_P_COEFFICIENT, DIP_I_COEFFICIENT, MIN_DIAL_IN_PUMP_PWM_DUTY_CYCLE, MAX_DIAL_IN_PUMP_PWM_DUTY_CYCLE ); // Initialize persistent alarm for flow sensor signal strength too low #ifdef USE_FMD_FLOW_SENSOR initPersistentAlarm( ALARM_ID_DIALYSATE_FLOW_SIGNAL_STRENGTH_TOO_LOW, FLOW_SIG_STRGTH_ALARM_PERSIST, FLOW_SIG_STRGTH_ALARM_PERSIST ); initPersistentAlarm( ALARM_ID_HD_DP_FLOW_READ_TIMEOUT_ERROR, 0, DIALYSATE_FLOW_FAST_READ_TO_PERSIST ); initPersistentAlarm( ALARM_ID_HD_DP_FLOW_SLOW_READ_TIMEOUT_ERROR, 0, DIALYSATE_FLOW_SLOW_READ_TO_PERSIST ); initPersistentAlarm( ALARM_ID_HD_DP_FLOW_SENSOR_ERROR, 0, DIALYSATE_FLOW_COMM_ERROR_PERSIST ); #endif initPersistentAlarm( ALARM_ID_HD_DIAL_IN_FLOW_OUT_OF_RANGE, 0, DIP_MAX_FLOW_RATE_OUT_OF_RANGE_PERSIST ); } /*********************************************************************//** * @brief * The setDialInPumpTargetFlowRate function sets a new target flow rate and * pump direction. * @details Inputs: isDialInPumpOn, dialInPumpDirectionSet * @details Outputs: targetDialInFlowRate, dialInPumpdirection, dialInPumpPWMDutyCyclePct * @param flowRate new target dialIn flow rate * @param dir new dialIn flow direction * @param mode new control mode * @return TRUE if new flow rate & dir are set, FALSE if not *************************************************************************/ BOOL setDialInPumpTargetFlowRate( 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 == isDialInPumpOn ) || ( 0 == flowRate ) || ( dir == dialInPumpDirectionSet ) ) { #ifndef NO_PUMP_FLOW_LIMITS // Verify flow rate if ( flowRate <= MAX_DIAL_IN_FLOW_RATE ) #endif { resetDialInFlowMovingAverage(); targetDialInFlowRate = ( dir == MOTOR_DIR_FORWARD ? (S32)flowRate : (S32)flowRate * -1 ); dialInPumpDirection = dir; dialInPumpControlMode = 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 dialInPumpPWMDutyCyclePct = DIP_PWM_FROM_ML_PER_MIN((F32)flowRate); // ~ 8% per 100 mL/min with a 10% zero offset added in (e.g. 100 mL/min = 8+10 = 18%) switch ( dialInPumpState ) { case DIAL_IN_PUMP_RAMPING_UP_STATE: // See if we need to reverse direction of ramp if ( dialInPumpPWMDutyCyclePct < dialInPumpPWMDutyCyclePctSet ) { dialInPumpState = DIAL_IN_PUMP_RAMPING_DOWN_STATE; } break; case DIAL_IN_PUMP_RAMPING_DOWN_STATE: // See if we need to reverse direction of ramp if ( dialInPumpPWMDutyCyclePct > dialInPumpPWMDutyCyclePctSet ) { dialInPumpState = DIAL_IN_PUMP_RAMPING_UP_STATE; } break; case DIAL_IN_PUMP_CONTROL_TO_TARGET_STATE: // Start ramp to new target in appropriate direction if ( dialInPumpPWMDutyCyclePctSet > dialInPumpPWMDutyCyclePct ) { dialInPumpState = DIAL_IN_PUMP_RAMPING_DOWN_STATE; } else { dialInPumpState = DIAL_IN_PUMP_RAMPING_UP_STATE; } break; default: // Ok - not all states need to be handled here break; } result = TRUE; } #ifndef NO_PUMP_FLOW_LIMITS else // Requested flow rate too high { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_DIAL_IN_FLOW_SET_TOO_HIGH, flowRate ) } #endif } return result; } /*********************************************************************//** * @brief * The signalDialInPumpHardStop function stops the dialIn pump immediately. * @details Inputs: none * @details Outputs: DialIn pump stopped, set point reset, state changed to off * @return none *************************************************************************/ void signalDialInPumpHardStop( void ) { targetDialInFlowRate = 0; stopDialInPump(); dialInPumpState = DIAL_IN_PUMP_OFF_STATE; dialInPumpPWMDutyCyclePct = 0.0; dipControlTimerCounter = 0; resetPIController( PI_CONTROLLER_ID_DIALYSATE_FLOW, MIN_DIAL_IN_PUMP_PWM_DUTY_CYCLE ); } /*********************************************************************//** * @brief * The signalDialInPumpRotorHallSensor function handles the dialysate inlet pump rotor * hall sensor detection. Calculates rotor speed (in RPM). Stops pump if * there is a pending request to home the pump. * @details Inputs: dipRotorRevStartTime, dipStopAtHomePosition * @details Outputs: dipRotorRevStartTime, dialInPumpRotorSpeedRPM * @return none *************************************************************************/ void signalDialInPumpRotorHallSensor( void ) { U32 rotTime = getMSTimerCount(); U32 deltaTime = calcTimeBetween( dipRotorRevStartTime, rotTime ); // Calculate rotor speed (in RPM) dialInPumpRotorSpeedRPM.data = ( 1.0 / (F32)deltaTime ) * (F32)MS_PER_SECOND * (F32)SEC_PER_MIN; dipRotorRevStartTime = rotTime; // If we are supposed to stop pump at home position, stop pump now. if ( TRUE == dipStopAtHomePosition ) { signalDialInPumpHardStop(); dipStopAtHomePosition = FALSE; } } /*********************************************************************//** * @brief * The homeDialInPump function initiates a dialysate inlet pump home operation. * @details Inputs: dialInPumpState * @details Outputs: dipStopAtHomePosition, dipHomeStartTime, dialysate inlet pump started (slow) * @return none *************************************************************************/ BOOL homeDialInPump( void ) { BOOL result = FALSE; if ( DIAL_IN_PUMP_OFF_STATE == dialInPumpState ) { dipStopAtHomePosition = TRUE; dipHomeStartTime = getMSTimerCount(); result = setDialInPumpTargetFlowRate( DIP_HOME_RATE, MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_OPEN_LOOP ); } return result; } /*********************************************************************//** * @brief * The isDialInPumpRunning function returns whether the dialysate inlet pump * is currently running or not. * @details Inputs: isDialInPumpOn * @details Outputs: none * @return isDialInPumpOn *************************************************************************/ BOOL isDialInPumpRunning( void ) { return isDialInPumpOn; } /*********************************************************************//** * @brief * The execDialInFlowMonitor function executes the dialIn flow monitor. * @details Inputs: none * @details Outputs: measuredDialInFlowRate, adcDialInPumpMCSpeedRPM, adcDialInPumpMCCurrentmA * @return none *************************************************************************/ void execDialInFlowMonitor( void ) { HD_OP_MODE_T opMode = getCurrentOperationMode(); U16 dipRPM = getIntADCReading( INT_ADC_DIAL_IN_PUMP_SPEED ); U16 dipmA = getIntADCReading( INT_ADC_DIAL_IN_PUMP_MOTOR_CURRENT ); U08 fpReadCtr = getFPGADialysateFlowFastPacketReadCounter(); U08 spReadCtr = getFPGADialysateFlowSlowPacketReadCounter(); U08 flowErrorCtr = getFPGADialysateFlowErrorCounter(); U08 flowStatus = getFPGADialysateFlowMeterStatus(); F32 dipFlow; #ifdef USE_FMD_FLOW_SENSOR F32 dpFlow = getFPGADialysateFlow(); // Check if a new calibration is available if ( TRUE == isNewCalibrationRecordAvailable() ) { // Get the new calibration data and check its validity processCalibrationData(); } dipFlow = pow(dpFlow, 4) * dialysateFlowCalRecord.hdFlowSensors[ CAL_DATA_HD_DIALYSATE_FLOW_SENSOR ].fourthOrderCoeff + pow(dpFlow, 3) * dialysateFlowCalRecord.hdFlowSensors[ CAL_DATA_HD_DIALYSATE_FLOW_SENSOR ].thirdOrderCoeff + pow(dpFlow, 2) * dialysateFlowCalRecord.hdFlowSensors[ CAL_DATA_HD_DIALYSATE_FLOW_SENSOR ].secondOrderCoeff + dpFlow * dialysateFlowCalRecord.hdFlowSensors[ CAL_DATA_HD_DIALYSATE_FLOW_SENSOR ].gain + dialysateFlowCalRecord.hdFlowSensors[ CAL_DATA_HD_DIALYSATE_FLOW_SENSOR ].offset; #ifndef DISABLE_PUMP_FLOW_CHECKS if ( TRUE == isPersistentAlarmTriggered( ALARM_ID_HD_DP_FLOW_SENSOR_ERROR, ( flowErrorCtr != lastDialysateFlowCommErrorCount ) ) ) { activateAlarmNoData( ALARM_ID_HD_DP_FLOW_SENSOR_ERROR ); } if ( flowStatus != DFM_SENSOR_CONNECTED_STATUS ) { SET_ALARM_WITH_1_U32_DATA( ALARM_ID_HD_DIALYSATE_FLOW_STATUS_SELF_TEST_FAILURE, (U32)flowStatus ); } lastDialysateFlowCommErrorCount = flowErrorCtr; #endif #ifndef DISABLE_FPGA_COUNTER_CHECKS // Check for stale flow reading if ( TRUE == isPersistentAlarmTriggered( ALARM_ID_HD_DP_FLOW_READ_TIMEOUT_ERROR, ( fpReadCtr == lastDialysateFlowFastPacketReadCtr ) ) ) { activateAlarmNoData( ALARM_ID_HD_DP_FLOW_READ_TIMEOUT_ERROR ); } if ( TRUE == isPersistentAlarmTriggered( ALARM_ID_HD_DP_FLOW_SLOW_READ_TIMEOUT_ERROR, ( spReadCtr == lastDialysateFlowSlowPacketReadCtr ) ) ) { activateAlarmNoData( ALARM_ID_HD_DP_FLOW_SLOW_READ_TIMEOUT_ERROR ); } #endif // Record flow read counters for next time around lastDialysateFlowFastPacketReadCtr = fpReadCtr; lastDialysateFlowSlowPacketReadCtr = spReadCtr; dialInFlowSignalStrength.data = getFPGADialysateFlowSignalStrength(); #else dipFlow = 0.0; // TODO - get dialysate flow from DG #endif adcDialInPumpMCSpeedRPM.data = (F32)(SIGN_FROM_12_BIT_VALUE(dipRPM)) * DIP_SPEED_ADC_TO_RPM_FACTOR; adcDialInPumpMCCurrentmA.data = (F32)(SIGN_FROM_12_BIT_VALUE(dipmA)) * DIP_CURRENT_ADC_TO_MA_FACTOR; filterDialInFlowReadings( dipFlow ); // Calculate dialysate inlet pump motor speed/direction from hall sensor count updateDialInPumpSpeedAndDirectionFromHallSensors(); // Do not start enforcing checks until out of init/POST mode if ( opMode != MODE_INIT ) { // Check pump direction checkDialInPumpDirection(); // Check pump controller current checkDialInPumpMCCurrent(); // Check pump speeds and flow checkDialInPumpSpeeds(); checkDialInPumpFlowRate(); #ifdef USE_FMD_FLOW_SENSOR checkDialInFlowSensorSignalStrength(); #endif // Check for home position, zero/low speed checkDialInPumpRotor(); } // Publish dialIn flow data on interval publishDialInFlowData(); } /*********************************************************************//** * @brief * The execDialInFlowController function executes the dialIn flow controller. * @details Inputs: dialInPumpState * @details Outputs: dialInPumpState * @return none *************************************************************************/ void execDialInFlowController( void ) { switch ( dialInPumpState ) { case DIAL_IN_PUMP_OFF_STATE: dialInPumpState = handleDialInPumpOffState(); break; case DIAL_IN_PUMP_RAMPING_UP_STATE: dialInPumpState = handleDialInPumpRampingUpState(); break; case DIAL_IN_PUMP_RAMPING_DOWN_STATE: dialInPumpState = handleDialInPumpRampingDownState(); break; case DIAL_IN_PUMP_CONTROL_TO_TARGET_STATE: dialInPumpState = handleDialInPumpControlToTargetState(); break; default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_DIAL_IN_FLOW_INVALID_DIAL_IN_PUMP_STATE, dialInPumpState ) break; } } /*********************************************************************//** * @brief * The handleDialInPumpOffState function handles the dialIn pump off state * of the dialIn pump controller state machine. * @details Inputs: targetDialInFlowRate, dialInPumpDirection * @details Outputs: dialInPumpPWMDutyCyclePctSet, dialInPumpDirectionSet, isDialInPumpOn * @return next state *************************************************************************/ static DIAL_IN_PUMP_STATE_T handleDialInPumpOffState( void ) { DIAL_IN_PUMP_STATE_T result = DIAL_IN_PUMP_OFF_STATE; // If we have been given a flow rate, setup ramp up and transition to ramp up state if ( targetDialInFlowRate != 0 ) { // Set initial PWM duty cycle dialInPumpPWMDutyCyclePctSet = DIP_PWM_ZERO_OFFSET + MAX_DIAL_IN_PUMP_PWM_STEP_UP_CHANGE; setDialInPumpControlSignalPWM( dialInPumpPWMDutyCyclePctSet ); // Allow dialIn pump to run in requested direction setDialInPumpDirection( dialInPumpDirection ); releaseDialInPumpStop(); isDialInPumpOn = TRUE; result = DIAL_IN_PUMP_RAMPING_UP_STATE; } return result; } /*********************************************************************//** * @brief * The handleDialInPumpRampingUpState function handles the ramp up state * of the dialIn pump controller state machine. * @details Inputs: dialInPumpPWMDutyCyclePctSet * @details Outputs: dialInPumpPWMDutyCyclePctSet * @return next state *************************************************************************/ static DIAL_IN_PUMP_STATE_T handleDialInPumpRampingUpState( void ) { DIAL_IN_PUMP_STATE_T result = DIAL_IN_PUMP_RAMPING_UP_STATE; // Have we been asked to stop the dialIn pump? if ( 0 == targetDialInFlowRate ) { // Start ramp down to stop dialInPumpPWMDutyCyclePctSet -= MAX_DIAL_IN_PUMP_PWM_STEP_DN_CHANGE; setDialInPumpControlSignalPWM( dialInPumpPWMDutyCyclePctSet ); result = DIAL_IN_PUMP_RAMPING_DOWN_STATE; } // Have we reached end of ramp up? else if ( dialInPumpPWMDutyCyclePctSet >= dialInPumpPWMDutyCyclePct ) { resetDialInFlowMovingAverage(); dialInPumpPWMDutyCyclePctSet = dialInPumpPWMDutyCyclePct; resetPIController( PI_CONTROLLER_ID_DIALYSATE_FLOW, dialInPumpPWMDutyCyclePctSet ); dialInPumpControlModeSet = dialInPumpControlMode; setDialInPumpControlSignalPWM( dialInPumpPWMDutyCyclePctSet ); dipControlTimerCounter = 0; result = DIAL_IN_PUMP_CONTROL_TO_TARGET_STATE; } // Continue ramp up else { dialInPumpPWMDutyCyclePctSet += MAX_DIAL_IN_PUMP_PWM_STEP_UP_CHANGE; setDialInPumpControlSignalPWM( dialInPumpPWMDutyCyclePctSet ); } return result; } /*********************************************************************//** * @brief * The handleDialInPumpRampingDownState function handles the ramp down state * of the dialIn pump controller state machine. * @details Inputs: dialInPumpPWMDutyCyclePctSet * @details Outputs: dialInPumpPWMDutyCyclePctSet * @return next state *************************************************************************/ static DIAL_IN_PUMP_STATE_T handleDialInPumpRampingDownState( void ) { DIAL_IN_PUMP_STATE_T result = DIAL_IN_PUMP_RAMPING_DOWN_STATE; // Have we essentially reached zero speed if ( dialInPumpPWMDutyCyclePctSet < (MAX_DIAL_IN_PUMP_PWM_STEP_DN_CHANGE + DIP_PWM_ZERO_OFFSET) ) { stopDialInPump(); result = DIAL_IN_PUMP_OFF_STATE; } // Have we reached end of ramp down? else if ( dialInPumpPWMDutyCyclePctSet <= dialInPumpPWMDutyCyclePct ) { resetDialInFlowMovingAverage(); dialInPumpPWMDutyCyclePctSet = dialInPumpPWMDutyCyclePct; resetPIController( PI_CONTROLLER_ID_DIALYSATE_FLOW, dialInPumpPWMDutyCyclePctSet ); dialInPumpControlModeSet = dialInPumpControlMode; setDialInPumpControlSignalPWM( dialInPumpPWMDutyCyclePctSet ); dipControlTimerCounter = 0; result = DIAL_IN_PUMP_CONTROL_TO_TARGET_STATE; } // Continue ramp down else { dialInPumpPWMDutyCyclePctSet -= MAX_DIAL_IN_PUMP_PWM_STEP_DN_CHANGE; setDialInPumpControlSignalPWM( dialInPumpPWMDutyCyclePctSet ); } return result; } /*********************************************************************//** * @brief * The handleDialInPumpControlToTargetState function handles the "control to * target" state of the dialIn pump controller state machine. * @details Inputs: none * @details Outputs: dialInPumpState * @return next state *************************************************************************/ static DIAL_IN_PUMP_STATE_T handleDialInPumpControlToTargetState( void ) { DIAL_IN_PUMP_STATE_T result = DIAL_IN_PUMP_CONTROL_TO_TARGET_STATE; // Control at set interval if ( ++dipControlTimerCounter >= DIP_CONTROL_INTERVAL ) { if ( dialInPumpControlModeSet == PUMP_CONTROL_MODE_CLOSED_LOOP ) { F32 tgtFlow = (F32)targetDialInFlowRate; F32 actFlow = getMeasuredDialInFlowRate(); F32 newPWM; newPWM = runPIController( PI_CONTROLLER_ID_DIALYSATE_FLOW, tgtFlow, actFlow ); dialInPumpPWMDutyCyclePctSet = newPWM; setDialInPumpControlSignalPWM( newPWM ); } dipControlTimerCounter = 0; } return result; } /*********************************************************************//** * @brief * The setDialInPumpControlSignalPWM function sets the PWM duty cycle for * the dialysate inlet pump to a given %. * @details Inputs: none * @details Outputs: dialIn pump stop signal activated, PWM duty cycle zeroed * @param newPWM new duty cycle % to apply to PWM * @return none *************************************************************************/ static void setDialInPumpControlSignalPWM( F32 newPWM ) { etpwmSetCmpA( etpwmREG2, (U32)( (S32)( ( newPWM * (F32)(etpwmREG2->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) ); } /*********************************************************************//** * @brief * The stopDialInPump function sets the dialIn pump stop signal. * @details Inputs: none * @details Outputs: dialIn pump stop signal activated, PWM duty cycle zeroed * @return none *************************************************************************/ static void stopDialInPump( void ) { isDialInPumpOn = FALSE; dialInPumpPWMDutyCyclePctSet = DIP_PWM_ZERO_OFFSET; etpwmSetCmpA( etpwmREG2, 0 ); SET_DIP_STOP(); } /*********************************************************************//** * @brief * The releaseDialInPumpStop function clears the dialIn pump stop signal. * @details Inputs: none * @details Outputs: dialIn pump stop signal * @return none *************************************************************************/ static void releaseDialInPumpStop( void ) { CLR_DIP_STOP(); } /*********************************************************************//** * @brief * The setDialInPumpDirection function sets the set dialIn pump direction to * the given direction. * @details Inputs: dialInPumpState * @details Outputs: dialInPumpState * @param dir dialIn pump direction to set * @return none *************************************************************************/ static void setDialInPumpDirection( MOTOR_DIR_T dir ) { switch ( dir ) { case MOTOR_DIR_FORWARD: dialInPumpDirectionSet = dir; SET_DIP_DIR(); break; case MOTOR_DIR_REVERSE: dialInPumpDirectionSet = dir; CLR_DIP_DIR(); break; default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_DIAL_IN_FLOW_INVALID_DIAL_IN_PUMP_DIRECTION, dir ) break; } } #ifdef USE_FMD_FLOW_SENSOR /*********************************************************************//** * @brief * The processCalibrationData function gets the calibration data and makes * sure it is valid by checking the calibration date. The calibration date * should not be 0. * @details Inputs: none * @details Outputs: dialysateFlowCalRecord * @return TRUE if the calibration record is valid, otherwise FALSE *************************************************************************/ static BOOL processCalibrationData( void ) { BOOL status = TRUE; // Get the calibration record from NVDataMgmt HD_FLOW_SENSORS_CAL_RECORD_T calData = getHDFlowSensorsCalibrationRecord(); // Check if the calibration data that was received from NVDataMgmt is legitimate // The calibration date item should not be zero. If the calibration date is 0, // then the dialysate flow sensors data is not stored in the NV memory or it was corrupted. if ( 0 == calData.hdFlowSensors[ CAL_DATA_HD_DIALYSATE_FLOW_SENSOR ].calibrationTime ) { #ifndef SKIP_CAL_CHECK activateAlarmNoData( ALARM_ID_HD_DIALYSATE_FLOW_INVALID_CAL_RECORD ); status = FALSE; #endif } // The calibration data was valid, update the local copy dialysateFlowCalRecord.hdFlowSensors[ CAL_DATA_HD_DIALYSATE_FLOW_SENSOR ].fourthOrderCoeff = calData.hdFlowSensors[ CAL_DATA_HD_DIALYSATE_FLOW_SENSOR ].fourthOrderCoeff; dialysateFlowCalRecord.hdFlowSensors[ CAL_DATA_HD_DIALYSATE_FLOW_SENSOR ].thirdOrderCoeff = calData.hdFlowSensors[ CAL_DATA_HD_DIALYSATE_FLOW_SENSOR ].thirdOrderCoeff; dialysateFlowCalRecord.hdFlowSensors[ CAL_DATA_HD_DIALYSATE_FLOW_SENSOR ].secondOrderCoeff = calData.hdFlowSensors[ CAL_DATA_HD_DIALYSATE_FLOW_SENSOR ].secondOrderCoeff; dialysateFlowCalRecord.hdFlowSensors[ CAL_DATA_HD_DIALYSATE_FLOW_SENSOR ].gain = calData.hdFlowSensors[ CAL_DATA_HD_DIALYSATE_FLOW_SENSOR ].gain; dialysateFlowCalRecord.hdFlowSensors[ CAL_DATA_HD_DIALYSATE_FLOW_SENSOR ].offset = calData.hdFlowSensors[ CAL_DATA_HD_DIALYSATE_FLOW_SENSOR ].offset; return status; } #endif /*********************************************************************//** * @brief * The getTargetDialInFlowRate function gets the current target dialIn flow * rate. * @details Inputs: targetDialInFlowRate * @details Outputs: none * @return the current target dialIn flow rate (in mL/min). *************************************************************************/ S32 getTargetDialInFlowRate( void ) { return targetDialInFlowRate; } /*********************************************************************//** * @brief * The getMeasuredDialInFlowRate function gets the measured dialIn flow * rate. * @details Inputs: measuredDialInFlowRate * @details Outputs: none * @return the current dialIn flow rate (in mL/min). *************************************************************************/ F32 getMeasuredDialInFlowRate( void ) { F32 result = measuredDialInFlowRate.data; if ( OVERRIDE_KEY == measuredDialInFlowRate.override ) { result = measuredDialInFlowRate.ovData; } return result; } #ifdef USE_FMD_FLOW_SENSOR /*********************************************************************//** * @brief * The getMeasuredDialInFlowSignalStrength function gets the measured dialIn flow * signal strength. * @details Inputs: dialInFlowSignalStrength * @details Outputs: none * @return the current dialIn flow signal strength (in %). *************************************************************************/ F32 getMeasuredDialInFlowSignalStrength( void ) { F32 result = dialInFlowSignalStrength.data; if ( OVERRIDE_KEY == dialInFlowSignalStrength.override ) { result = dialInFlowSignalStrength.ovData; } return result; } #endif /*********************************************************************//** * @brief * The getMeasuredDialInPumpRotorSpeed function gets the measured dialIn flow * rate. * @details Inputs: dialInPumpRotorSpeedRPM * @details Outputs: none * @return the current dialIn flow rate (in mL/min). *************************************************************************/ F32 getMeasuredDialInPumpRotorSpeed( void ) { F32 result = dialInPumpRotorSpeedRPM.data; if ( OVERRIDE_KEY == dialInPumpRotorSpeedRPM.override ) { result = dialInPumpRotorSpeedRPM.ovData; } return result; } /*********************************************************************//** * @brief * The getMeasuredDialInPumpSpeed function gets the measured dialIn flow * rate. * @details Inputs: dialInPumpSpeedRPM * @details Outputs: none * @return the current dialIn flow rate (in mL/min). *************************************************************************/ F32 getMeasuredDialInPumpSpeed( void ) { F32 result = dialInPumpSpeedRPM.data; if ( OVERRIDE_KEY == dialInPumpSpeedRPM.override ) { result = dialInPumpSpeedRPM.ovData; } return result; } /*********************************************************************//** * @brief * The getMeasuredDialInPumpMCSpeed function gets the measured dialIn pump * speed. * @details Inputs: adcDialInPumpMCSpeedRPM * @details Outputs: none * @return the current dialIn pump speed (in RPM). *************************************************************************/ F32 getMeasuredDialInPumpMCSpeed( void ) { F32 result = adcDialInPumpMCSpeedRPM.data; if ( OVERRIDE_KEY == adcDialInPumpMCSpeedRPM.override ) { result = adcDialInPumpMCSpeedRPM.ovData; } return result; } /*********************************************************************//** * @brief * The getMeasuredDialInPumpMCCurrent function gets the measured dialIn pump * current. * @details Inputs: adcDialInPumpMCCurrentmA * @details Outputs: none * @return the current dialIn pump current (in mA). *************************************************************************/ F32 getMeasuredDialInPumpMCCurrent( void ) { F32 result = adcDialInPumpMCCurrentmA.data; if ( OVERRIDE_KEY == adcDialInPumpMCCurrentmA.override ) { result = adcDialInPumpMCCurrentmA.ovData; } return result; } /*********************************************************************//** * @brief * The getDialInPumpPWMDutyCyclePct function gets the current dialIn pump * PWM duty cycle percentage. * @details Inputs: dialInPumpPWMDutyCyclePctSet * @details Outputs: none * @param init Flag indicates whether or not initial PWM duty cycle is wanted * @return the current dialIn pump PWM duty cycle percentage (0..1). *************************************************************************/ F32 getDialInPumpPWMDutyCyclePct( BOOL init ) { F32 result = dialInPumpPWMDutyCyclePctSet; if ( TRUE == init ) { result = dialInPumpPWMDutyCyclePct; } return result; } /*********************************************************************//** * @brief * The publishDialInFlowData function publishes dialIn flow data at the set * interval. * @details Inputs: target flow rate, measured flow rate, measured MC speed, * measured MC current * @details Outputs: DialIn flow data is published to CAN bus. * @return none *************************************************************************/ static void publishDialInFlowData( void ) { // Publish dialIn flow data on interval if ( ++dialInFlowDataPublicationTimerCounter >= getU32OverrideValue( &dialInFlowDataPublishInterval ) ) { DIALIN_PUMP_STATUS_PAYLOAD_T payload; payload.setPoint = targetDialInFlowRate; payload.measFlow = getMeasuredDialInFlowRate(); payload.measRotorSpd = getMeasuredDialInPumpRotorSpeed(); payload.measPumpSpd = getMeasuredDialInPumpSpeed(); payload.measMCSpd = getMeasuredDialInPumpMCSpeed(); payload.measMCCurr = getMeasuredDialInPumpMCCurrent(); payload.pwmDC = dialInPumpPWMDutyCyclePctSet * FRACTION_TO_PERCENT_FACTOR; #ifdef USE_FMD_FLOW_SENSOR payload.flowSigStrength = getMeasuredDialInFlowSignalStrength() * FRACTION_TO_PERCENT_FACTOR; #else payload.flowSigStrength = 0.0; #endif broadcastData( MSG_ID_DIALYSATE_FLOW_DATA, COMM_BUFFER_OUT_CAN_HD_BROADCAST, (U08*)&payload, sizeof( DIALIN_PUMP_STATUS_PAYLOAD_T ) ); dialInFlowDataPublicationTimerCounter = 0; } } /*********************************************************************//** * @brief * The resetDialInFlowMovingAverage function resets the properties of the * dialIn flow moving average sample buffer. * @details Inputs: none * @details Outputs: flowReadingsTotal, flowReadingsIdx, flowReadingsCount all set to zero. * @return none *************************************************************************/ static void resetDialInFlowMovingAverage( void ) { flowReadingsIdx = 0; flowReadingsCount = 0; flowReadingsTotal = 0.0; dipControlTimerCounter = 0; } /*********************************************************************//** * @brief * The filterDialInFlowReadings function adds a new flow sample to the filter. * @details Inputs: none * @details Outputs: flowReadings[], flowReadingsIdx, flowReadingsCount, flowReadingsTotal * @return none *************************************************************************/ static void filterDialInFlowReadings( F32 flow ) { #ifndef RAW_FLOW_SENSOR_DATA 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 ); measuredDialInFlowRate.data = flowReadingsTotal / (F32)flowReadingsCount; #else measuredDialInFlowRate.data = flow; #endif } /*********************************************************************//** * @brief * The updateDialInPumpSpeedAndDirectionFromHallSensors function calculates * the dialysate inlet pump motor speed and direction from hall sensor counter on * a 1 second interval. * @details Inputs: dipLastMotorHallSensorCount, dipMotorSpeedCalcTimerCtr, current count from FPGA * @details Outputs: dipMotorDirectionFromHallSensors, dialInPumpSpeedRPM * @return none *************************************************************************/ static void updateDialInPumpSpeedAndDirectionFromHallSensors( void ) { if ( ++dipMotorSpeedCalcTimerCtr >= DIP_SPEED_CALC_INTERVAL ) { U16 dipMotorHallSensorCount = getFPGADialInPumpHallSensorCount(); U32 nextIdx = INC_WRAP( dipMotorSpeedCalcIdx, 0, DIP_SPEED_CALC_BUFFER_LEN - 1 ); U16 incDelta = ( dipMotorHallSensorCount >= dipLastMotorHallSensorCounts[ nextIdx ] ? \ dipMotorHallSensorCount - dipLastMotorHallSensorCounts[ nextIdx ] : \ ( HEX_64_K - dipLastMotorHallSensorCounts[ nextIdx ] ) + dipMotorHallSensorCount ); U16 decDelta = HEX_64_K - incDelta; U16 delta; // Determine dialysate inlet pump speed/direction from delta hall sensor count since last interval if ( incDelta < decDelta ) { delta = incDelta; dialInPumpSpeedRPM.data = ( (F32)delta / (F32)DIP_HALL_EDGE_COUNTS_PER_REV ) * (F32)SEC_PER_MIN; } else { delta = decDelta; dialInPumpSpeedRPM.data = ( (F32)delta / (F32)DIP_HALL_EDGE_COUNTS_PER_REV ) * (F32)SEC_PER_MIN * -1.0; } // Update last count for next time dipLastMotorHallSensorCounts[ nextIdx ] = dipMotorHallSensorCount; dipMotorSpeedCalcIdx = nextIdx; dipMotorSpeedCalcTimerCtr = 0; } } /*********************************************************************//** * @brief * The checkDialInPumpRotor function checks the rotor for the dialysate inlet * 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: dipStopAtHomePosition, dipHomeStartTime, dipRotorRevStartTime * @details Outputs: pump may be stopped if homing, dialInPumpRotorSpeedRPM may be set to zero. * @return none *************************************************************************/ static void checkDialInPumpRotor( void ) { // If homing, check timeout if ( ( TRUE == dipStopAtHomePosition ) && ( TRUE == didTimeout( dipHomeStartTime, DIP_HOME_TIMEOUT_MS ) ) ) { signalDialInPumpHardStop(); dipStopAtHomePosition = FALSE; // TODO - alarm??? } // If pump is stopped or running very slowly, set rotor speed to zero if ( TRUE == didTimeout( dipRotorRevStartTime, DIP_HOME_TIMEOUT_MS ) ) { dialInPumpRotorSpeedRPM.data = 0.0; } } /*********************************************************************//** * @brief * The checkDialInPumpDirection function checks the set direction vs. * the direction implied by the sign of the measured MC speed. * @details Inputs: adcDialInPumpMCSpeedRPM, dialInPumpDirectionSet, dialInPumpState * @details Outputs: none * @return none *************************************************************************/ static void checkDialInPumpDirection( void ) { if ( DIAL_IN_PUMP_CONTROL_TO_TARGET_STATE == dialInPumpState ) { MOTOR_DIR_T dipMCDir, dipDir; U08 dirErrorCnt = getFPGADialInPumpHallSensorStatus() & PUMP_DIR_ERROR_COUNT_MASK; // Check pump direction error count if ( lastDialInPumpDirectionCount != dirErrorCnt ) { lastDialInPumpDirectionCount = dirErrorCnt; #ifndef DISABLE_PUMP_DIRECTION_CHECKS SET_ALARM_WITH_1_U32_DATA( ALARM_ID_HD_PUMP_DIRECTION_STATUS_ERROR, (U32)HD_PUMP_DIALYSATE_INLET_PUMP ) #endif } dipMCDir = ( getMeasuredDialInPumpMCSpeed() >= 0.0 ? MOTOR_DIR_FORWARD : MOTOR_DIR_REVERSE ); dipDir = ( getMeasuredDialInPumpSpeed() >= 0.0 ? MOTOR_DIR_FORWARD : MOTOR_DIR_REVERSE ); // Check set direction vs. direction from hall sensors if ( dialInPumpDirectionSet != dipDir ) { if ( ++errorDialInPumpDirectionPersistTimerCtr >= DIP_DIRECTION_ERROR_PERSIST ) { #ifndef DISABLE_PUMP_DIRECTION_CHECKS SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DIAL_IN_PUMP_MC_DIRECTION_CHECK, (U32)dialInPumpDirectionSet, (U32)dipDir ) #endif } } // Check set direction vs. direction from sign of motor controller speed else if ( dialInPumpDirectionSet != dipMCDir ) { if ( ++errorDialInPumpDirectionPersistTimerCtr >= DIP_DIRECTION_ERROR_PERSIST ) { #ifndef DISABLE_PUMP_DIRECTION_CHECKS SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DIAL_IN_PUMP_MC_DIRECTION_CHECK, (U32)dialInPumpDirectionSet, (U32)dipMCDir ) #endif } } else { errorDialInPumpDirectionPersistTimerCtr = 0; } } else { errorDialInPumpDirectionPersistTimerCtr = 0; } } /*********************************************************************//** * @brief * The checkDialInPumpSpeeds function checks several aspects of the dialysate * inlet 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 commanded 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: targetDialInFlowRate, dialInPumpSpeedRPM, dialInPumpRotorSpeedRPM * @details Outputs: alarm(s) may be triggered * @return none *************************************************************************/ static void checkDialInPumpSpeeds( void ) { F32 measMotorSpeed = getMeasuredDialInPumpSpeed(); F32 measMCMotorSpeed = fabs( getMeasuredDialInPumpMCSpeed() ); // Check for pump running while commanded off if ( 0 == targetDialInFlowRate ) { if ( measMotorSpeed > DIP_MAX_MOTOR_SPEED_WHILE_OFF_RPM ) { if ( ++errorDialInMotorOffPersistTimerCtr >= DIP_OFF_ERROR_PERSIST ) { #ifndef DISABLE_PUMP_SPEED_CHECKS SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DIAL_IN_PUMP_OFF_CHECK, measMotorSpeed ); activateSafetyShutdown(); #endif } } else { errorDialInMotorOffPersistTimerCtr = 0; } } else { errorDialInMotorOffPersistTimerCtr = 0; } // Checks that only occur when pump is running (and beyond ramp). if ( DIAL_IN_PUMP_CONTROL_TO_TARGET_STATE == dialInPumpState ) { F32 cmdMotorSpeed = DIP_PWM_TO_MOTOR_SPEED_RPM( dialInPumpPWMDutyCyclePctSet ); F32 deltaMotorSpeed = fabs( measMotorSpeed - cmdMotorSpeed ); F32 deltaMCMotorSpeed = fabs( measMCMotorSpeed - cmdMotorSpeed ); F32 measRotorSpeed = getMeasuredDialInPumpRotorSpeed(); F32 measMotorSpeedInRotorRPM = measMotorSpeed / DIP_GEAR_RATIO; F32 deltaRotorSpeed = fabs( measRotorSpeed - measMotorSpeedInRotorRPM ); // Check measured motor speed vs. commanded motor speed while controlling to target if ( ( deltaMotorSpeed > DIP_MAX_MOTOR_SPEED_ERROR_RPM ) || ( deltaMCMotorSpeed > DIP_MAX_MOTOR_SPEED_ERROR_RPM ) ) { if ( ++errorDialInMotorSpeedPersistTimerCtr >= DIP_MOTOR_SPEED_ERROR_PERSIST ) { #ifndef DISABLE_PUMP_SPEED_CHECKS SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DIAL_IN_PUMP_MOTOR_SPEED_CHECK, cmdMotorSpeed, measMotorSpeed ); #endif } } else { errorDialInMotorSpeedPersistTimerCtr = 0; } // Check measured rotor speed vs. measured motor speed while controlling to target if ( deltaRotorSpeed > DIP_MAX_ROTOR_VS_MOTOR_DIFF_RPM ) { if ( ++errorDialInRotorSpeedPersistTimerCtr >= DIP_ROTOR_SPEED_ERROR_PERSIST ) { #ifndef DISABLE_PUMP_SPEED_CHECKS SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DIAL_IN_PUMP_ROTOR_SPEED_CHECK, measRotorSpeed, measMotorSpeed ); #endif } } else { errorDialInRotorSpeedPersistTimerCtr = 0; } } else { errorDialInMotorSpeedPersistTimerCtr = 0; errorDialInRotorSpeedPersistTimerCtr = 0; } } /*********************************************************************//** * @brief * The checkDialInPumpFlowRate function checks the measured dialysate flow * rate is in range. * @details Inputs: measuredDialInFlowRate * @details Outputs: alarm may be triggered * @return none *************************************************************************/ static void checkDialInPumpFlowRate( void ) { F32 flow = getMeasuredDialInFlowRate(); // Range check on measure DPi flow rate. #ifndef DISABLE_PUMP_FLOW_CHECKS if ( TRUE == isPersistentAlarmTriggered( ALARM_ID_HD_DIAL_IN_FLOW_OUT_OF_RANGE, ( flow > DIP_MAX_FLOW_RATE ) || ( flow < DIP_MIN_FLOW_RATE ) ) ) { SET_ALARM_WITH_1_F32_DATA( ALARM_ID_HD_DIAL_IN_FLOW_OUT_OF_RANGE, flow ); } #endif #ifdef USE_FMD_FLOW_SENSOR // Check only performed while in treatment mode and while we are in control to target state if ( ( MODE_TREA == getCurrentOperationMode() ) && ( DIAL_IN_PUMP_CONTROL_TO_TARGET_STATE == dialInPumpState ) ) { F32 flow = getMeasuredDialInFlowRate(); F32 speed = getMeasuredDialInPumpSpeed(); F32 impliedSpeed = ( flow / (F32)ML_PER_LITER ) * DIP_REV_PER_LITER * DIP_GEAR_RATIO; F32 delta = fabs( speed - impliedSpeed ); if ( delta > DIP_MAX_FLOW_VS_SPEED_DIFF_RPM ) { if ( ++errorDialInFlowVsMotorSpeedPersistTimerCtr >= DIP_FLOW_VS_SPEED_PERSIST ) { #ifndef DISABLE_PUMP_FLOW_CHECKS SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DIAL_IN_PUMP_FLOW_VS_MOTOR_SPEED_CHECK, flow, speed ); #endif } } else { errorDialInFlowVsMotorSpeedPersistTimerCtr = 0; } } else { errorDialInFlowVsMotorSpeedPersistTimerCtr = 0; } #endif } /*********************************************************************//** * @brief * The checkDialInPumpMCCurrent function checks the measured MC current vs. * the set state of the dialIn pump (stopped or running). * @details Inputs: dialInPumpState, dipCurrErrorDurationCtr, adcDialInPumpMCCurrentmA * @details Outputs: none * @return none *************************************************************************/ static void checkDialInPumpMCCurrent( void ) { F32 dipCurr; // DialIn pump should be off if ( DIAL_IN_PUMP_OFF_STATE == dialInPumpState ) { dipCurr = fabs( getMeasuredDialInPumpMCCurrent() ); if ( dipCurr > DIP_MAX_CURR_WHEN_STOPPED_MA ) { dipCurrErrorDurationCtr += TASK_PRIORITY_INTERVAL; if ( dipCurrErrorDurationCtr > DIP_MAX_CURR_ERROR_DURATION_MS ) { #ifndef DISABLE_MOTOR_CURRENT_CHECKS SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DIAL_IN_PUMP_MC_CURRENT_CHECK, getMeasuredDialInPumpMCCurrent() ); #endif } } else { dipCurrErrorDurationCtr = 0; } } // DialIn pump should be running else { dipCurr = fabs( getMeasuredDialInPumpMCCurrent() ); if ( dipCurr > DIP_MAX_CURR_WHEN_RUNNING_MA ) { dipCurrErrorDurationCtr += TASK_PRIORITY_INTERVAL; if ( dipCurrErrorDurationCtr > DIP_MAX_CURR_ERROR_DURATION_MS ) { #ifndef DISABLE_MOTOR_CURRENT_CHECKS SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DIAL_IN_PUMP_MC_CURRENT_CHECK, getMeasuredDialInPumpMCCurrent() ); #endif } } else { dipCurrErrorDurationCtr = 0; } } } #ifdef USE_FMD_FLOW_SENSOR /*********************************************************************//** * @brief * The checkDialInFlowSensorSignalStrength function checks the measured * dialysate flow sensor signal strength is sufficient for accurate flow sensing. * @details Inputs: * @details Outputs: * @return none *************************************************************************/ static void checkDialInFlowSensorSignalStrength( void ) { #ifndef DISABLE_PUMP_FLOW_CHECKS HD_OP_MODE_T opMode = getCurrentOperationMode(); // Check flow sensor signal strength when appropriate TODO - in pre-treatment, must be far enough along for fluid to be in tubing if ( MODE_TREA == opMode || ( MODE_PRET == opMode && FALSE ) ) { F32 sigStrength = getMeasuredDialInFlowSignalStrength(); BOOL outOfRange = ( sigStrength < MIN_FLOW_SIG_STRENGTH ? TRUE : FALSE ); if ( TRUE == isPersistentAlarmTriggered( ALARM_ID_DIALYSATE_FLOW_SIGNAL_STRENGTH_TOO_LOW, outOfRange ) ) { SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DIALYSATE_FLOW_SIGNAL_STRENGTH_TOO_LOW, sigStrength, MIN_FLOW_SIG_STRENGTH ); } } #endif } #endif /*********************************************************************//** * @brief * The execDialInFlowTest function executes the state machine for the * DialInFlow self-test. * @details Inputs: none * @details Outputs: none * @return the current state of the DialInFlow self-test. *************************************************************************/ SELF_TEST_STATUS_T execDialInFlowTest( void ) { #ifdef USE_FMD_FLOW_SENSOR SELF_TEST_STATUS_T result = SELF_TEST_STATUS_IN_PROGRESS; U08 const dfmStatus = getFPGADialysateFlowMeterStatus(); if ( DFM_SENSOR_PARAM_CORRUPT_STATUS != dfmStatus ) { BOOL calStatus = processCalibrationData(); if ( TRUE == calStatus ) { result = SELF_TEST_STATUS_PASSED; } else { result = SELF_TEST_STATUS_FAILED; } } else { SET_ALARM_WITH_1_U32_DATA( ALARM_ID_HD_DIALYSATE_FLOW_STATUS_SELF_TEST_FAILURE, (U32)dfmStatus ); } #else SELF_TEST_STATUS_T result = SELF_TEST_STATUS_PASSED; #endif return result; } /************************************************************************* * TEST SUPPORT FUNCTIONS *************************************************************************/ /*********************************************************************//** * @brief * The testSetDialInFlowDataPublishIntervalOverride function overrides the * dialIn flow data publish interval. * @details Inputs: none * @details Outputs: dialInFlowDataPublishInterval * @param value override dialIn flow data publish interval with (in ms) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetDialInFlowDataPublishIntervalOverride( U32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { U32 intvl = value / TASK_PRIORITY_INTERVAL; result = TRUE; dialInFlowDataPublishInterval.ovData = intvl; dialInFlowDataPublishInterval.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetDialInFlowDataPublishIntervalOverride function resets the override * of the dialIn flow data publish interval. * @details Inputs: none * @details Outputs: dialInFlowDataPublishInterval * @return TRUE if override reset successful, FALSE if not *************************************************************************/ BOOL testResetDialInFlowDataPublishIntervalOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; dialInFlowDataPublishInterval.override = OVERRIDE_RESET; dialInFlowDataPublishInterval.ovData = dialInFlowDataPublishInterval.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetTargetDialInFlowRateOverride function overrides the target * dialysate inlet flow rate.n * @details Inputs: none * @details Outputs: targetDialInFlowRate * @param value override target dialysate inlet 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 testSetTargetDialInFlowRateOverride( 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 ) { targetDialInFlowRate = value; result = setDialInPumpTargetFlowRate( abs(value), dir, (PUMP_CONTROL_MODE_T)ctrlMode ); } } return result; } /*********************************************************************//** * @brief * The testResetMeasuredDialInFlowRateOverride function overrides the measured * dialIn flow rate. * @details Inputs: none * @details Outputs: measuredDialInFlowRate * @param value override measured dialIn flow rate (in mL/min) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetMeasuredDialInFlowRateOverride( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; measuredDialInFlowRate.ovData = value; measuredDialInFlowRate.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetMeasuredDialInFlowRateOverride function resets the override of the * measured dialIn flow rate. * @details Inputs: none * @details Outputs: measuredDialInFlowRate * @return TRUE if reset successful, FALSE if not *************************************************************************/ BOOL testResetMeasuredDialInFlowRateOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; measuredDialInFlowRate.override = OVERRIDE_RESET; measuredDialInFlowRate.ovData = measuredDialInFlowRate.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetMeasuredDialInPumpRotorSpeedOverride function overrides the measured * dialIn pump rotor speed. * @details Inputs: none * @details Outputs: dialInPumpRotorSpeedRPM * @param value override measured dialIn pump rotor speed (in RPM) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetMeasuredDialInPumpRotorSpeedOverride( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; dialInPumpRotorSpeedRPM.ovData = value; dialInPumpRotorSpeedRPM.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetMeasuredDialInPumpRotorSpeedOverride function resets the override of the * measured dialIn pump rotor speed. * @details Inputs: none * @details Outputs: dialInPumpRotorSpeedRPM * @return TRUE if reset successful, FALSE if not *************************************************************************/ BOOL testResetMeasuredDialInPumpRotorSpeedOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; dialInPumpRotorSpeedRPM.override = OVERRIDE_RESET; dialInPumpRotorSpeedRPM.ovData = dialInPumpRotorSpeedRPM.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetMeasuredDialInPumpSpeedOverride function overrides the measured * dialIn pump motor speed. * @details Inputs: none * @details Outputs: dialInPumpSpeedRPM * @param value override measured dialIn pump motor speed (in RPM) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetMeasuredDialInPumpSpeedOverride( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; dialInPumpSpeedRPM.ovData = value; dialInPumpSpeedRPM.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetMeasuredDialInPumpSpeedOverride function resets the override of the * measured dialIn pump motor speed. * @details Inputs: none * @details Outputs: dialInPumpSpeedRPM * @return TRUE if reset successful, FALSE if not *************************************************************************/ BOOL testResetMeasuredDialInPumpSpeedOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; dialInPumpSpeedRPM.override = OVERRIDE_RESET; dialInPumpSpeedRPM.ovData = dialInPumpSpeedRPM.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetMeasuredDialInPumpMCSpeedOverride function overrides the measured * dialIn pump motor speed. * @details Inputs: none * @details Outputs: adcDialInPumpMCSpeedRPM * @param value override measured dialIn pump speed (in RPM) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetMeasuredDialInPumpMCSpeedOverride( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; adcDialInPumpMCSpeedRPM.ovData = value; adcDialInPumpMCSpeedRPM.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetMeasuredDialInPumpMCSpeedOverride function resets the override of the * measured dialIn pump motor speed. * @details Inputs: none * @details Outputs: adcDialInPumpMCSpeedRPM * @return TRUE if reset successful, FALSE if not *************************************************************************/ BOOL testResetMeasuredDialInPumpMCSpeedOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; adcDialInPumpMCSpeedRPM.override = OVERRIDE_RESET; adcDialInPumpMCSpeedRPM.ovData = adcDialInPumpMCSpeedRPM.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetMeasuredDialInPumpMCCurrentOverride function overrides the measured * dialIn pump motor current. * @details Inputs: none * @details Outputs: adcDialInPumpMCCurrentmA * @param value override measured dialIn pump current (in mA) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetMeasuredDialInPumpMCCurrentOverride( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; adcDialInPumpMCCurrentmA.ovData = value; adcDialInPumpMCCurrentmA.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetMeasuredDialInPumpMCCurrentOverride function resets the override of the * measured dialIn pump motor current. * @details Inputs: none * @details Outputs: adcDialInPumpMCCurrentmA * @return TRUE if reset successful, FALSE if not *************************************************************************/ BOOL testResetMeasuredDialInPumpMCCurrentOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; adcDialInPumpMCCurrentmA.override = OVERRIDE_RESET; adcDialInPumpMCCurrentmA.ovData = adcDialInPumpMCCurrentmA.ovInitData; } return result; } #ifdef USE_FMD_FLOW_SENSOR /*********************************************************************//** * @brief * The testSetMeasuredDialInFlowSignalStrengthOverride function overrides the measured * dialysate flow signal strength. * @details Inputs: none * @details Outputs: dialInFlowSignalStrength * @param value override measured dialysate flow signal strength (in %) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetMeasuredDialInFlowSignalStrengthOverride( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; dialInFlowSignalStrength.ovData = value / 100.0; dialInFlowSignalStrength.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetMeasuredDialInFlowSignalStrengthOverride function resets the override * of the measured dialysate flow signal strength. * @details Inputs: none * @details Outputs: dialInFlowSignalStrength * @return TRUE if reset successful, FALSE if not *************************************************************************/ BOOL testResetMeasuredDialInFlowSignalStrengthOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; dialInFlowSignalStrength.override = OVERRIDE_RESET; dialInFlowSignalStrength.ovData = dialInFlowSignalStrength.ovInitData; } return result; } #endif /**@}*/