Index: firmware/App/Controllers/DialOutFlow.c =================================================================== diff -u -r97e1c0abbbf667f834d284eb6fc14999f073b020 -r2564e234d1191e4a5aad384cb12e634571d202f3 --- firmware/App/Controllers/DialOutFlow.c (.../DialOutFlow.c) (revision 97e1c0abbbf667f834d284eb6fc14999f073b020) +++ firmware/App/Controllers/DialOutFlow.c (.../DialOutFlow.c) (revision 2564e234d1191e4a5aad384cb12e634571d202f3) @@ -48,6 +48,8 @@ #define DIAL_OUT_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) /// Interval (ms/task time) at which the dialysate outlet flow is filtered. #define DIAL_OUT_FILTER_INTERVAL ( 100 / TASK_PRIORITY_INTERVAL ) +/// Interval (ms/task time) at which the ultrafiltration flow rate is calculated. +#define DIAL_OUT_UF_CALC_INTERVAL ( 1000 / TASK_PRIORITY_INTERVAL ) #define MAX_DIAL_OUT_FLOW_RATE 700 ///< Maximum dialysate outlet pump flow rate in mL/min. #define MIN_DIAL_OUT_FLOW_RATE 100 ///< Minimum dialysate outlet pump flow rate in mL/min. @@ -64,6 +66,7 @@ #define P_CORR 0.1666F ///< P term for volume error feedback into dialysate outlet pump flow estimate correction offset. #define RPM_2_ML_MIN_CONVERSION 0.215964F ///< Conversion factor for estimating flow rate from pump motor RPM. +#define OFFSET_2_PWM_OFFSET 0.135F ///< Conversion factor for estimating PWM duty cycle offset for a given rate offset. #define SIZE_OF_ROLLING_AVG 100 ///< Number of samples in DPo flow estimation moving average. #define DOP_HOME_SPEED 400 ///< Target pump speed (in RPM) for homing. @@ -105,8 +108,8 @@ #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 4000.0F ///< 10-90% PWM range yields 0-3,200 RPM range. Full 100% PWM range would yield 4,000 RPM range. -#define DOP_RATE_CORRECTION_SCALAR 0.08F ///< Scalar for estimating DPo rate correction offset based on target Qd. -#define DOP_RATE_CORRECTION_OFFSET 25.0F ///< Offset for estimating DPo rate correction offset based on target Qd. +#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.0009F ) + 0.0972F + DOP_PWM_ZERO_OFFSET ) @@ -180,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. @@ -256,6 +260,7 @@ 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 ); @@ -265,8 +270,11 @@ * @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 @@ -279,18 +287,19 @@ // Direction change while pump is running is not allowed if ( ( FALSE == isDialOutPumpOn ) || ( 0 == flowRate ) || ( dir == dialOutPumpDirectionSet ) ) { - F32 pwmDC = getDialInPumpPWMDutyCyclePct( TRUE ); + 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 - { // if rate correction offset not yet set, set it based on target dialysate flow rate - if ( fabs( dopRateCorrectionOffset ) < NEARLY_ZERO ) - { - dopRateCorrectionOffset = (F32)getTargetDialInFlowRate() * DOP_RATE_CORRECTION_SCALAR + DOP_RATE_CORRECTION_OFFSET; - } + { + // 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 ) ) @@ -318,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 ) { @@ -484,7 +494,6 @@ flowReadingsIdx = 0; flowReadingsCount = 0; flowReadingsTotal = 0.0; - offsetPWMDutyCyclePct = 0.0; } /*********************************************************************//** @@ -588,8 +597,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 ) @@ -603,14 +613,36 @@ // 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 + dopMeasuredRate = 0.0F; + 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