Index: firmware/App/Controllers/DialOutFlow.c =================================================================== diff -u -r19a8bf98a7154e24c35da25225d4b55bf70ddd09 -rcd5be724d5a3ba7457e761191d82f278654d7f5c --- firmware/App/Controllers/DialOutFlow.c (.../DialOutFlow.c) (revision 19a8bf98a7154e24c35da25225d4b55bf70ddd09) +++ firmware/App/Controllers/DialOutFlow.c (.../DialOutFlow.c) (revision cd5be724d5a3ba7457e761191d82f278654d7f5c) @@ -1,14 +1,14 @@ /************************************************************************** * -* Copyright (c) 2020-2022 Diality Inc. - All Rights Reserved. +* Copyright (c) 2020-2024 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) Dara Navaei -* @date (last) 24-May-2022 +* @author (last) Darren Cox +* @date (last) 04-Oct-2023 * * @author (original) Sean * @date (original) 24-Jan-2020 @@ -20,13 +20,16 @@ #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" @@ -45,25 +48,28 @@ #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 650 ///< Maximum dialysate outlet pump flow rate in mL/min. +#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.0133F ///< Maximum duty cycle change when ramping up ~ 200 mL/min/s. -#define MAX_DIAL_OUT_PUMP_PWM_STEP_DN_CHANGE 0.02F ///< Maximum duty cycle change when ramping down ~ 300 mL/min/s. -#define MAX_DIAL_OUT_PUMP_PWM_DUTY_CYCLE 0.89F ///< Controller will error if PWM duty cycle > 90%, so set max to 89%. +#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.001F ///< P term for volume error feedback into dialysate outlet pump control. -#define P_UF 0.001F ///< P term for UF rate error feedback into dialysate outlet pump control. -#define P_CORR 0.1666F ///< P term for volume error feedback into dialysate outlet pump flow estimate correction offset. +#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 SIZE_OF_ROLLING_AVG 100 ///< Number of samples in DPo flow estimation moving average. +#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_RATE 100 ///< Target pump speed (in estimate mL/min) for homing. +#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). @@ -76,39 +82,50 @@ #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) rotor speed error condition. -static const U32 DOP_ROTOR_SPEED_ERROR_PERSIST = ((12 * 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 2000 ///< Motor controller current errors persisting beyond this duration will trigger an alarm. +#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 1.751752 ///< Conversion factor from ADC counts to RPM for dialysate outlet pump motor (3500 RPM/1998 counts). -#define DOP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.000238F ///< ~42 BP motor RPM = 1% PWM duty cycle +#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.0009 ) + 0.0972 + DOP_PWM_ZERO_OFFSET ) +#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) ( ((pwm) - DOP_PWM_ZERO_OFFSET) * 4000.0F ) +#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. COMMENTED BUT SAVED FOR FUTURE USE. +//#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. @@ -130,13 +147,15 @@ 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 and macros for pump stop and direction outputs +// 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 SET_DOP_STOP() {mibspiREG1->PC3 &= ~STOP_DO_PUMP_MIBSPI1_PORT_MASK;} ///< Macro sets pump controller run/stop signal to stop. -#define CLR_DOP_STOP() {mibspiREG1->PC3 |= STOP_DO_PUMP_MIBSPI1_PORT_MASK;} ///< Macro sets pump controller run/stop signal to run. -#define STOP_DO_PUMP_GIO_PORT_PIN 6U ///< GIO port A pin used for pump controller direction pin. -#define SET_DOP_DIR() gioSetBit( gioPORTA, STOP_DO_PUMP_GIO_PORT_PIN, PIN_SIGNAL_HIGH ) ///< Macro sets pump controller direction to forward direction. -#define CLR_DOP_DIR() gioSetBit( gioPORTA, STOP_DO_PUMP_GIO_PORT_PIN, PIN_SIGNAL_LOW ) ///< Macro sets pump controller direction to reverse direction. +#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 ********** @@ -164,6 +183,7 @@ 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. @@ -212,6 +232,7 @@ static void resetDialOutFlowMovingAverage( void ); static void filterDialOutFlowReadings( F64 flow ); +static BOOL setDialOutPumpToFixedPWM( F32 pwm ); /*********************************************************************//** * @brief @@ -239,15 +260,21 @@ 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 - * @details Outputs: targetDialOutFlowRate, dialOutPumpdirection, dialOutPumpPWMDutyCyclePct + * @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 @@ -260,8 +287,20 @@ // Direction change while pump is running is not allowed if ( ( FALSE == isDialOutPumpOn ) || ( 0 == flowRate ) || ( dir == dialOutPumpDirectionSet ) ) { - F32 pwmDC = ( 0 == flowRate ? DOP_PWM_ZERO_OFFSET : DOP_PWM_FROM_ML_PER_MIN( (F32)flowRate ) ); + 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 ) ) { @@ -288,6 +327,7 @@ 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 ) { @@ -336,6 +376,41 @@ /*********************************************************************//** * @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 ); + + pwm = ( MOTOR_DIR_REVERSE == dir ? pwm * -1.0F : pwm ); + + result = setDialOutPumpToFixedPWM( pwm ); + + 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 @@ -382,7 +457,7 @@ U32 deltaTime = calcTimeBetween( dopRotorRevStartTime, rotTime ); // Calculate rotor speed (in RPM) - dialOutPumpRotorSpeedRPM.data = ( 1.0 / (F32)deltaTime ) * (F32)MS_PER_SECOND * (F32)SEC_PER_MIN; + 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. @@ -420,7 +495,6 @@ flowReadingsIdx = 0; flowReadingsCount = 0; flowReadingsTotal = 0.0; - offsetPWMDutyCyclePct = 0.0; } /*********************************************************************//** @@ -458,7 +532,7 @@ { dopStopAtHomePosition = TRUE; dopHomeStartTime = getMSTimerCount(); - result = setDialOutPumpTargetRate( DOP_HOME_RATE, MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_OPEN_LOOP ); + result = setDialOutPumpTargetRPM( DOP_HOME_SPEED, MOTOR_DIR_FORWARD ); } return result; @@ -524,8 +598,9 @@ * 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 - * @details Outputs: dialOutPumpMCSpeedRPM, dialOutPumpMCCurrentmA + * @details Inputs: latest sensor data, flowFilterTimerCtr, ufCalcTimerCtr + * @details Outputs: dialOutPumpMCSpeedRPM, dialOutPumpMCCurrentmA, ufMeasuredRate, + * dopMeasuredRate, flowFilterTimerCtr, ufCalcTimerCtr * @return none *************************************************************************/ void execDialOutFlowMonitor( void ) @@ -539,14 +614,43 @@ // Calculate dialysate outlet pump motor speed/direction from hall sensor count updateDialOutPumpSpeedAndDirectionFromHallSensors(); - // Filter estimated dialysate out flow rate and calculate UF rate - if ( ++flowFilterTimerCtr >= DIAL_OUT_FILTER_INTERVAL ) + // 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; - // Calculate DPo flow in mL/min - filterDialOutFlowReadings( getMeasuredDialOutPumpMCSpeed() * RPM_2_ML_MIN_CONVERSION ); - // Calculate UF flow in mL/min - ufMeasuredRate = dopMeasuredRate - getMeasuredDialInFlowRate(); + ufCalcTimerCtr = 0; } // Do not start enforcing checks until out of init/POST mode @@ -561,6 +665,10 @@ // Check for home position, zero/low speed checkDialOutPumpRotor(); } + else + { + lastDialOutPumpDirectionCount = getFPGADialOutPumpHallSensorStatus() & PUMP_DIR_ERROR_COUNT_MASK; + } publishDialOutFlowData(); } @@ -625,6 +733,7 @@ } else { + isDialOutPumpOn = FALSE; dialOutPumpPWMDutyCyclePct = 0.0; } @@ -834,17 +943,20 @@ 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.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; @@ -931,40 +1043,48 @@ { 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 ( lastDialOutPumpDirectionCount != dirErrorCnt ) + if ( ( TRUE == isHallSensorFailed ) && ( TRUE == incTimeWindowedCount( TIME_WINDOWED_COUNT_DOP_COMMUTATION_ERROR ) ) ) { - lastDialOutPumpDirectionCount = dirErrorCnt; - SET_ALARM_WITH_1_U32_DATA( ALARM_ID_HD_PUMP_DIRECTION_STATUS_ERROR, (U32)HD_PUMP_DIALYSATE_OUTLET_PUMP ) +#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 ) + 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_DIAL_OUT_PUMP_MC_DIRECTION_CHECK, (U32)dialOutPumpDirectionSet, (U32)dopDir ) + 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 ) + 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_DIAL_OUT_PUMP_MC_DIRECTION_CHECK, (U32)dialOutPumpDirectionSet, (U32)dopMCDir ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_DIAL_OUT_PUMP_MC_DIRECTION_CHECK, (U32)dialOutPumpDirectionSet, (U32)dopMCDir ) } } } @@ -987,14 +1107,14 @@ * 2. while pump is controlling, measured motor speed should be within allowed range of measured motor controller speed. * 3. measured motor speed should be within allowed range of measured rotor speed. * All 3 checks have a persistence time that must be met before an alarm is triggered. - * @details Inputs: targetDialOutFlowRate, dialOutPumpSpeedRPM, dialOutPumpRotorSpeedRPM - * @details Outputs: alarm(s) may be 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 = fabs( getMeasuredDialOutPumpMCSpeed() ); + F32 measMCMotorSpeed = getMeasuredDialOutPumpMCSpeed(); // Check for pump running while commanded off if ( dialOutPumpPWMDutyCyclePctSet <= DOP_PWM_ZERO_OFFSET ) @@ -1007,7 +1127,7 @@ if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMP_SPEED_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) #endif { - SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DIAL_OUT_PUMP_OFF_CHECK, measMotorSpeed ); + SET_ALARM_WITH_1_F32_DATA( ALARM_ID_HD_DIAL_OUT_PUMP_OFF_CHECK, measMotorSpeed ); activateSafetyShutdown(); } } @@ -1024,12 +1144,13 @@ if ( DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE == dialOutPumpState ) { - F32 cmdMotorSpeed = DOP_PWM_TO_MOTOR_SPEED_RPM( dialOutPumpPWMDutyCyclePctSet ); + F32 cmdMotorSpeed = DOP_PWM_TO_MOTOR_SPEED_RPM( dialOutPumpPWMDutyCyclePctSet, dialOutPumpDirectionSet ); F32 deltaMotorSpeed = fabs( measMotorSpeed - cmdMotorSpeed ); F32 deltaMCMotorSpeed = fabs( measMCMotorSpeed - cmdMotorSpeed ); - F32 measRotorSpeed = getMeasuredDialOutPumpRotorSpeed(); - F32 measMotorSpeedInRotorRPM = measMotorSpeed / DOP_GEAR_RATIO; + 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 ) ) @@ -1040,7 +1161,7 @@ if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMP_SPEED_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) #endif { - SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DIAL_OUT_PUMP_MOTOR_SPEED_CHECK, cmdMotorSpeed, measMotorSpeed ); + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_DIAL_OUT_PUMP_MOTOR_SPEED_CHECK, cmdMotorSpeed, measMotorSpeed ); } } } @@ -1050,15 +1171,15 @@ } // Check measured rotor speed vs. measured motor speed while controlling to target - if ( deltaRotorSpeed > DOP_MAX_ROTOR_VS_MOTOR_DIFF_RPM ) + if ( ( deltaRotorSpeed > DOP_MAX_ROTOR_VS_MOTOR_DIFF_RPM ) && ( measMotorSpeedDeltaPct > DOP_MAX_MOTOR_SPEED_VS_TRGT_DIFF_PCT ) ) { - if ( ++errorDialOutRotorSpeedPersistTimerCtr >= DOP_ROTOR_SPEED_ERROR_PERSIST ) + 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_DIAL_OUT_PUMP_ROTOR_SPEED_CHECK, measRotorSpeed, measMotorSpeed ); + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_DIAL_OUT_PUMP_ROTOR_SPEED_CHECK, measRotorSpeed, measMotorSpeed ); } } } @@ -1086,49 +1207,53 @@ { F32 dopCurr; - // DialOut pump should be off - if ( DIAL_OUT_PUMP_OFF_STATE == dialOutPumpState ) + // only check current when we have A/C power + if ( getCPLDACPowerLossDetected() != TRUE ) { - dopCurr = fabs( getMeasuredDialOutPumpMCCurrent() ); - if ( dopCurr > DOP_MAX_CURR_WHEN_STOPPED_MA ) + // DialOut pump should be off + if ( DIAL_OUT_PUMP_OFF_STATE == dialOutPumpState ) { - dopCurrErrorDurationCtr += TASK_PRIORITY_INTERVAL; - if ( dopCurrErrorDurationCtr > DOP_MAX_CURR_ERROR_DURATION_MS ) + 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 ) + if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_MOTOR_CURRNT_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) #endif - { - SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DIAL_OUT_PUMP_MC_CURRENT_CHECK, getMeasuredDialOutPumpMCCurrent() ); + { + 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 { - 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 ) + dopCurr = fabs( getMeasuredDialOutPumpMCCurrent() ); + if ( dopCurr > DOP_MAX_CURR_WHEN_RUNNING_MA ) { -#ifndef _RELEASE_ - if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_MOTOR_CURRNT_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) -#endif + dopCurrErrorDurationCtr += TASK_PRIORITY_INTERVAL; + if ( dopCurrErrorDurationCtr > DOP_MAX_CURR_ERROR_DURATION_MS ) { - SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DIAL_OUT_PUMP_MC_CURRENT_CHECK, getMeasuredDialOutPumpMCCurrent() ); + #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; + } } - else - { - dopCurrErrorDurationCtr = 0; - } } } @@ -1140,6 +1265,73 @@ /*********************************************************************//** * @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 @@ -1340,6 +1532,7 @@ if ( 0 == value ) { signalDialOutPumpHardStop(); + result = TRUE; } else { @@ -1621,4 +1814,27 @@ 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; +} + /**@}*/