Index: firmware/App/Controllers/DialInFlow.c =================================================================== diff -u -re2ca8378636da437905103e59033cf036b5304a9 -rd23f94b810d65dd54809b0bfeb32f0f763051d4d --- firmware/App/Controllers/DialInFlow.c (.../DialInFlow.c) (revision e2ca8378636da437905103e59033cf036b5304a9) +++ firmware/App/Controllers/DialInFlow.c (.../DialInFlow.c) (revision d23f94b810d65dd54809b0bfeb32f0f763051d4d) @@ -7,8 +7,8 @@ * * @file DialInFlow.c * -* @author (last) Michael Garthwaite -* @date (last) 12-Oct-2022 +* @author (last) Dara Navaei +* @date (last) 18-Oct-2022 * * @author (original) Sean * @date (original) 16-Dec-2019 @@ -50,7 +50,7 @@ #define MAX_DIAL_IN_PUMP_PWM_DUTY_CYCLE 0.89F ///< Controller will error if PWM duty cycle > 90%, so set max to 89%. #define MIN_DIAL_IN_PUMP_PWM_DUTY_CYCLE 0.10F ///< 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). +#define DIP_CONTROL_INTERVAL_SEC 4 ///< 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.0001F ///< P term for dialIn pump control. @@ -106,16 +106,18 @@ #define DIP_GEAR_RATIO 32.0F ///< DialIn pump motor to dialIn pump gear ratio. #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 ) +/// 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. 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 ) +/// Conversion from pump motor speed to PWM duty cycle. +#define DIP_MOTOR_SPEED_RPM_TO_PWM(rpm) ( ( rpm / 4000.0F ) + 0.1 ) // Macro converts PWM to estimate flow rate needed to achieve it. #define DIP_ML_PER_MIN_FROM_PWM(pwm) ( (( pwm - DIP_PWM_ZERO_OFFSET) * 684.73F ) + 49.121F ) /// Measured dialIn flow is filtered w/ moving average. -#define SIZE_OF_ROLLING_AVG 10 +#define SIZE_OF_ROLLING_AVG 4 #define PUMP_DIR_ERROR_COUNT_MASK 0x3F ///< Bit mask for pump direction error counter. #define DIP_MIN_DIR_CHECK_SPEED_RPM 10.0F ///< Minimum motor speed before we check pump direction. @@ -124,6 +126,19 @@ #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 (m'). +#define DIAL_IN_FLOW_WEAR_B_TERM 0.002244F ///< B term used for wear portion of alpha flow coefficient (m0). +#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). +#define DIAL_IN_STROKE_VOLUME 3.405 ///< Stroke volume (SV) used for Flow Estimation ALgorithm +#define DIAL_IN_GEAR_RATIO 32 ///< Gear ratio used for Flow Estimation ALgorithm + /// Enumeration of dialysate inlet pump states. typedef enum DialInPump_States { @@ -198,8 +213,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 +237,8 @@ static void checkDialInPumpSpeeds( void ); static void checkDialInPumpMCCurrent( void ); static void checkDialInPumpFlowRate( void ); +static F32 calcDialInFlow( void ); +static F32 dialysateInPumpPWMFromTargetFlowRate( F32 QdTarget ); /*********************************************************************//** * @brief @@ -237,7 +257,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 +318,7 @@ 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 : dialysateInPumpPWMFromTargetFlowRate( (F32)flowRate ) ); switch ( dialInPumpState ) { @@ -371,7 +393,7 @@ * 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 + * @details Outputs: dipRotorRevStartTime, dialInPumpRotorSpeedRPM, dialysateInPumpRotorCounter, dipStopAtHomePosition * @return none *************************************************************************/ void signalDialInPumpRotorHallSensor( void ) @@ -382,6 +404,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 +482,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 +772,115 @@ 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, targetDialInFlowRate, dialInPumpRotorSpeedRPM + * @details Outputs: measuredDialInFlowRate (ml/min) + * @return none + *************************************************************************/ +static F32 calcDialInFlow( void ) +{ + F32 estimatedFlow; + F32 QdTarget = getTargetDialInFlowRate(); + F32 deltaFlow; + + 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 motorRPM = getMeasuredDialInPumpMCSpeed(); + U32 r = getDialInPumpRotorCount(); + U32 rotCount = CAP( r, DIAL_IN_MAX_ROTOR_COUNT_FOR_WEAR ); + F32 wearFactor = DIAL_IN_FLOW_WEAR_A_TERM * (F32)rotCount + 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 alphaTerm = wearFactor * Pest + DIAL_IN_FLOW_A_ZERO; + + F32 calculatedFlow = ( motorRPM * 2 * DIAL_IN_STROKE_VOLUME/DIAL_IN_GEAR_RATIO ) * alphaTerm; + + 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; + } + } + + // Check the measured flow against estimated flow - / + deltaFlow = 0.5 * estimatedFlow; // 50% estimated flow + //if ( deltaFlow < fabs( estimatedFlow - filteredDialInFlowMeterReading ) ) + //{ + // SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_DIAL_IN_FLOW_CHECK_FAILURE, filteredDialInFlowMeterReading, estimatedFlow ); + //} + if ( TRUE == isDialInPumpRunning() ) + { + if ( ( TRUE == isPersistentAlarmTriggered( ALARM_ID_HD_DIAL_IN_FLOW_CHECK_FAILURE, deltaFlow < fabs( estimatedFlow - filteredDialInFlowMeterReading ) ) ) ) // DN-31OCT2022 + { + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_DIAL_IN_FLOW_CHECK_FAILURE, filteredDialInFlowMeterReading, estimatedFlow ); + } + } + else + { + isPersistentAlarmTriggered( ALARM_ID_HD_DIAL_IN_FLOW_CHECK_FAILURE, FALSE ); + } + + return estimatedFlow; +} + + +/*********************************************************************//** + * @brief + * The dialysateInPumpPWMFromTargetFlowRate function calculates a motor PWM setting + * from the estimator based on target flow rate and tubing wear. + * @details Inputs: dialInPumpRotorCounter + * @details Outputs: none + * @param QdTarget target dialysate flow rate + * @return Motor PWM value for given target flow rate + *************************************************************************/ +static F32 dialysateInPumpPWMFromTargetFlowRate( F32 QdTarget ) +{ + U32 r = getDialInPumpRotorCount(); + U32 rotCount = CAP( r, DIAL_IN_MAX_ROTOR_COUNT_FOR_WEAR ); + F32 wearFactor = DIAL_IN_FLOW_WEAR_A_TERM * (F32)rotCount + 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 alphaTerm = wearFactor * Pest + DIAL_IN_FLOW_A_ZERO; + F32 rpmTgt = QdTarget * DIAL_IN_GEAR_RATIO / ( 2 * DIAL_IN_STROKE_VOLUME * alphaTerm ); + F32 pwmDC = DIP_MOTOR_SPEED_RPM_TO_PWM( rpmTgt ); + + return pwmDC; +} + +/*********************************************************************//** + * @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 is reset to 0 + * @return none + *************************************************************************/ +void resetDialInPumpRotorCount( void ) +{ +#ifndef _RELEASE_ + if ( SW_CONFIG_ENABLE_VALUE == getSoftwareConfigStatus( SW_CONFIG_ENABLE_WORN_OUT_CARTRIDGE ) ) + { + dialysateInPumpRotorCounter.data = DIAL_IN_MAX_ROTOR_COUNT_FOR_WEAR; + } + else +#endif + { + dialysateInPumpRotorCounter.data = 0; + } +} /*********************************************************************//** * @brief @@ -882,6 +1015,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 @@ -895,6 +1048,7 @@ U32 getPumpRotorErrorPersistTime( F32 mtr_rpm, F32 gear_ratio ) { U32 err_persist_time = HEX_32_BIT_FULL_SCALE; // 49 days + if ( mtr_rpm > 0 ) { /// Calculate persist time for rotor speed error condition. @@ -927,7 +1081,7 @@ payload.measMCSpd = getMeasuredDialInPumpMCSpeed(); payload.measMCCurr = getMeasuredDialInPumpMCCurrent(); payload.pwmDC = dialInPumpPWMDutyCyclePctSet * FRACTION_TO_PERCENT_FACTOR; - payload.flowSigStrength = 0.0; + payload.rotorCount = getDialInPumpRotorCount(); broadcastData( MSG_ID_DIALYSATE_FLOW_DATA, COMM_BUFFER_OUT_CAN_HD_BROADCAST, (U08*)&payload, sizeof( DIALIN_PUMP_STATUS_PAYLOAD_T ) ); dialInFlowDataPublicationTimerCounter = 0; } @@ -965,8 +1119,8 @@ 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 = (F32)( flowReadingsTotal / (F64)flowReadingsCount ); + flowReadingsCount = INC_CAP( flowReadingsCount, SIZE_OF_ROLLING_AVG ); + filteredDialInFlowMeterReading = (F32)( flowReadingsTotal / (F64)flowReadingsCount ); } /*********************************************************************//** @@ -1686,4 +1840,49 @@ return result; } +/*********************************************************************//** + * @brief + * The testSetDialysateInPumpRotorCountOverride 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 testSetDialysateInPumpRotorCountOverride( 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; +} + /**@}*/