Index: firmware/App/Controllers/DialInFlow.c =================================================================== diff -u -ra4e044315bbfaaeff11cb9391485897458a48a5f -red39a4c00ef51318dfced8372eabcbee3ce03c52 --- firmware/App/Controllers/DialInFlow.c (.../DialInFlow.c) (revision a4e044315bbfaaeff11cb9391485897458a48a5f) +++ firmware/App/Controllers/DialInFlow.c (.../DialInFlow.c) (revision ed39a4c00ef51318dfced8372eabcbee3ce03c52) @@ -107,7 +107,8 @@ #define DIP_PWM_ZERO_OFFSET 0.1F ///< 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) - 49.121F ) / 684.73F ) + DIP_PWM_ZERO_OFFSET ) +//#define DIP_PWM_FROM_ML_PER_MIN(rate) ( ( ( (rate) - 49.121F ) / 684.73F ) + DIP_PWM_ZERO_OFFSET ) +#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. PWM range is 10% to 90%. RPM range is 0 to 3200. 3200 / 0.8 = 4000. #define DIP_PWM_TO_MOTOR_SPEED_RPM(pwm) ( ((pwm) - DIP_PWM_ZERO_OFFSET) * 4000.0F ) // Macro converts PWM to estimate flow rate needed to achieve it. @@ -124,6 +125,17 @@ #define DATA_PUBLISH_COUNTER_START_COUNT 30 ///< Data publish counter start count. +//Hybrid flow rate algorithm parameters +#define DIAL_IN_FLOW_A_ZERO 1.267F ///< Y intercept used for alpha flow coefficient calculation. +#define DIAL_IN_FLOW_WEAR_A_TERM 0.000000003551F ///< A term used for wear portion of alpha flow coefficient. +#define DIAL_IN_FLOW_WEAR_B_TERM 0.002244F ///< B term used for wear portion of alpha flow coefficient. +#define DIAL_IN_FLOW_QHIGHTRANSITION 400.0F /// High flow rate transition for blended algorithm +#define DIAL_IN_FLOW_QLOWTRANSITION 300.0F /// Low flow rate transition for blended algorithm +#define DIAL_IN_FLOW_PEST_A_TERM -0.000491F /// a (2nd order) term in polynomial fit for pressure estimation +#define DIAL_IN_FLOW_PEST_B_TERM -0.04672F /// b (first order) term in polynomial fit for pressure estimation +#define DIAL_IN_FLOW_PEST_C_TERM 18.648F /// c (zero order) term in polynomial fit for pressure estimation +#define DIAL_IN_MAX_ROTOR_COUNT_FOR_WEAR 25000 ///< Maximum rotor count for determining wear of the cartridge (negligible affect beyond this threshold). + /// Enumeration of dialysate inlet pump states. typedef enum DialInPump_States { @@ -198,8 +210,11 @@ static U32 flowReadingsCount = 0; ///< Number of samples in flow rolling average buffer. static U32 dipCurrErrorDurationCtr = 0; ///< Used for tracking persistence of dip current errors. -static HD_PUMPS_CAL_RECORD_T dialInPumpCalRecord; ///< Dialysate inlet calibration record. +static HD_PUMPS_CAL_RECORD_T dialInPumpCalRecord; ///< Dialysate inlet calibration record. +static OVERRIDE_U32_T dialysateInPumpRotorCounter = { 0, 0, 0, 0 }; ///< Running counter for dialin pump rotor revolutions +static F32 filteredDialInFlowMeterReading = 0.0; ///< Storage for current filtered flow meter reading + // ********** private function prototypes ********** static DIAL_IN_PUMP_STATE_T handleDialInPumpOffState( void ); @@ -219,6 +234,9 @@ static void checkDialInPumpSpeeds( void ); static void checkDialInPumpMCCurrent( void ); static void checkDialInPumpFlowRate( void ); +static F32 calcDialInFlow( void ); +static F32 dialysateInPumpPWMFromTargetFlowRate( F32 QdTarget); +static void resetDialInPumpRotorCount( void ); /*********************************************************************//** * @brief @@ -237,7 +255,9 @@ setDialInPumpDirection( MOTOR_DIR_FORWARD ); // Zero rolling flow average buffer - resetDialInFlowMovingAverage(); + resetDialInFlowMovingAverage(); + // Zero pump rotor count + resetDialInPumpRotorCount(); // Zero motor hall sensors counts buffer dipMotorSpeedCalcIdx = 0; @@ -296,7 +316,8 @@ 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 = ( 0 == flowRate ? DIP_PWM_ZERO_OFFSET : DIP_PWM_FROM_ML_PER_MIN( (F32)flowRate ) ); + //dialInPumpPWMDutyCyclePct = ( 0 == flowRate ? DIP_PWM_ZERO_OFFSET : DIP_PWM_FROM_ML_PER_MIN( (F32)flowRate ) ); + dialInPumpPWMDutyCyclePct = ( 0 == flowRate ? DIP_PWM_ZERO_OFFSET : dialysateInPumpPWMFromTargetFlowRate( (F32)flowRate ) ); switch ( dialInPumpState ) { @@ -382,6 +403,7 @@ // Calculate rotor speed (in RPM) dialInPumpRotorSpeedRPM.data = ( 1.0 / (F32)deltaTime ) * (F32)MS_PER_SECOND * (F32)SEC_PER_MIN; dipRotorRevStartTime = rotTime; + dialysateInPumpRotorCounter.data++; // If we are supposed to stop pump at home position, stop pump now. if ( TRUE == dipStopAtHomePosition ) @@ -459,6 +481,7 @@ { dipFlow = getDGDialysateFlowRateLMin() * (F64)ML_PER_LITER; // convert rate to mL/min filterDialInFlowReadings( dipFlow ); // process the fresh dialysate flow data + measuredDialInFlowRate.data = calcDialInFlow(); // calculate the measured flow rate using blended algorithm dialysateFlowDataFreshStatusCounter = 0; } else @@ -748,6 +771,93 @@ break; } } + +/*********************************************************************//** + * @brief + * The calcDialInFlow function calculates an estimated dialysate flow rate from + * a blended algorithm based on flow meter data and dialin pump speed and tubing wear + * (measured from count of rotor revolutions since cartridge install). + * @details Inputs: DialInPumpRotorCounter, (target flow rate) + * @details Outputs: measuredDialInFlowRate (ml/min) + * @return none + *************************************************************************/ +static F32 calcDialInFlow( void ) +{ + + F32 estimatedFlow; + F32 QdTarget = getTargetDialInFlowRate(); + + if (QdTarget >= DIAL_IN_FLOW_QHIGHTRANSITION) + { + //At higher flow rates, use the flow meter value. Assume target flow rate has already checked for out of bounds on high end. + estimatedFlow = filteredDialInFlowMeterReading; + } + else + { //Use blended or calculated flow rate + F32 pumpRPM = getMeasuredDialInPumpRotorSpeed(); + U32 r = getDialInPumpRotorCount(); + U32 rotCnt = CAP( r, DIAL_IN_MAX_ROTOR_COUNT_FOR_WEAR ); + F32 wear = DIAL_IN_FLOW_WEAR_A_TERM * (F32)rotCnt + DIAL_IN_FLOW_WEAR_B_TERM; + F32 Pest = DIAL_IN_FLOW_PEST_A_TERM * (QdTarget * QdTarget) + DIAL_IN_FLOW_PEST_B_TERM * QdTarget + DIAL_IN_FLOW_PEST_C_TERM; + F32 alpha = wear * Pest + DIAL_IN_FLOW_A_ZERO; + F32 calculatedFlow = (alpha * pumpRPM * ML_PER_LITER) / DIP_REV_PER_LITER; + + printf("pumpRPM=%f,rotCnt=%d,wear=%f,Pest=%f,alpha=%f,calculatedFlow=%f\n", pumpRPM,rotCnt,wear,Pest,alpha,calculatedFlow); + if (QdTarget < DIAL_IN_FLOW_QHIGHTRANSITION && QdTarget >= DIAL_IN_FLOW_QLOWTRANSITION) + { // use blended flow rate calculation + estimatedFlow = (QdTarget - DIAL_IN_FLOW_QLOWTRANSITION)/(DIAL_IN_FLOW_QHIGHTRANSITION-DIAL_IN_FLOW_QLOWTRANSITION) * calculatedFlow + + (DIAL_IN_FLOW_QHIGHTRANSITION - QdTarget)/(DIAL_IN_FLOW_QHIGHTRANSITION-DIAL_IN_FLOW_QLOWTRANSITION) * filteredDialInFlowMeterReading; + } + else + { // use calculated flow rate. Assume target flow rate has already checked for out of bounds on low end. + estimatedFlow = calculatedFlow; + } + } + return estimatedFlow; +} + + +/*********************************************************************//** + * @brief + * The dialysateInPumpPWMFromTargetFlowRate function calculates a motor PWM setting + * from the estimator based on target flow rate and tubing wear. + * @details Inputs: QdTarget, dialInPumpRotorCounter + * @details Outputs: none + * @return Motor PWM value for target rate + *************************************************************************/ +static F32 dialysateInPumpPWMFromTargetFlowRate( F32 QdTarget) +{ + F32 uncorrectedPWM = DIP_PWM_FROM_ML_PER_MIN(QdTarget); + U32 r = getDialInPumpRotorCount(); + U32 rotCnt = CAP( r, DIAL_IN_MAX_ROTOR_COUNT_FOR_WEAR ); + F32 wear = DIAL_IN_FLOW_WEAR_A_TERM * (F32)rotCnt + DIAL_IN_FLOW_WEAR_B_TERM; + F32 Pest = DIAL_IN_FLOW_PEST_A_TERM * (QdTarget * QdTarget) + DIAL_IN_FLOW_PEST_B_TERM * QdTarget + DIAL_IN_FLOW_PEST_C_TERM; + F32 alpha = wear * Pest + DIAL_IN_FLOW_A_ZERO; + return uncorrectedPWM / alpha; +} + +/*********************************************************************//** + * @brief + * The resetDialInPumpRotorCount function resets the dialin pump rotor counter + * that is a proxy for cartridge wear. Call this function after a new cartridge + * has been installed. + * @details Inputs: none + * @details Outputs: dialInPumpRotorCounter + * @return none + *************************************************************************/ +void resetDialInPumpRotorCount( void ) +{ +#ifndef _RELEASE_ + if ( getSoftwareConfigStatus( SW_CONFIG_ENABLE_WORN_OUT_CARTRIDGE ) != SW_CONFIG_ENABLE_VALUE ) + { + dialysateInPumpRotorCounter.data = 0; + } + else +#endif + { + dialysateInPumpRotorCounter.data = DIAL_IN_MAX_ROTOR_COUNT_FOR_WEAR; + } +} /*********************************************************************//** * @brief @@ -882,6 +992,26 @@ return result; } + +/*********************************************************************//** + * @brief + * The getDialInPumpRotorCount function returns the current count for the + * dialin pump rotor revolution counter. + * @details Inputs: dialInPumpRotorCounter + * @details Outputs: none + * @return dialInPumpRotorCounter + *************************************************************************/ +U32 getDialInPumpRotorCount( void ) +{ + U32 result = dialysateInPumpRotorCounter.data; + + if ( OVERRIDE_KEY == dialysateInPumpRotorCounter.override ) + { + result = dialysateInPumpRotorCounter.ovData; + } + + return result; +} /*********************************************************************//** * @brief @@ -945,7 +1075,8 @@ flowReadingsTotal += flow; flowReadingsIdx = INC_WRAP( flowReadingsIdx, 0, SIZE_OF_ROLLING_AVG - 1 ); flowReadingsCount = INC_CAP( flowReadingsCount, SIZE_OF_ROLLING_AVG ); - measuredDialInFlowRate.data = (F32)( flowReadingsTotal / (F64)flowReadingsCount ); + //measuredDialInFlowRate.data = (F32)( flowReadingsTotal / (F64)flowReadingsCount ); + filteredDialInFlowMeterReading = (F32)( flowReadingsTotal / (F64)flowReadingsCount ); } /*********************************************************************//** @@ -1665,4 +1796,48 @@ return result; } +/*********************************************************************//** + * @brief + * The testSetDialInPumpRotorCountOverride function overrides the dialin pump + * rotor counter value. + * @details Inputs: none + * @details Outputs: dialInPumpRotorCounter + * @param value override dialin pump rotor counter value + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +BOOL testSetDialInPumpRotorCountOverride( U32 value ) +{ + BOOL result = FALSE; + + if ( TRUE == isTestingActivated() ) + { + result = TRUE; + dialysateInPumpRotorCounter.ovData = value; + dialysateInPumpRotorCounter.override = OVERRIDE_KEY; + } + + return result; +} + +/*********************************************************************//** + * @brief + * The testResetDialInPumpRotorCountOverride function resets the override + * of the dialin pump rotor counter. + * @details Inputs: none + * @details Outputs: dialysateInPumpRotorCounter + * @return TRUE if reset successful, FALSE if not + *************************************************************************/ +BOOL testResetDialysateInPumpRotorCountOverride( void ) +{ + BOOL result = FALSE; + + if ( TRUE == isTestingActivated() ) + { + result = TRUE; + dialysateInPumpRotorCounter.override = OVERRIDE_RESET; + dialysateInPumpRotorCounter.ovData = dialysateInPumpRotorCounter.ovInitData; + } + + return result; +} /**@}*/