Index: firmware/App/Controllers/DialOutFlow.c =================================================================== diff -u -r6d19d844800eaaa05d03561c7f6e2d882de2c1ff -rcd5be724d5a3ba7457e761191d82f278654d7f5c --- firmware/App/Controllers/DialOutFlow.c (.../DialOutFlow.c) (revision 6d19d844800eaaa05d03561c7f6e2d882de2c1ff) +++ firmware/App/Controllers/DialOutFlow.c (.../DialOutFlow.c) (revision cd5be724d5a3ba7457e761191d82f278654d7f5c) @@ -1,14 +1,14 @@ /************************************************************************** * -* Copyright (c) 2020-2023 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) Sean Nash -* @date (last) 06-Jun-2023 +* @author (last) Darren Cox +* @date (last) 04-Oct-2023 * * @author (original) Sean * @date (original) 24-Jan-2020 @@ -22,7 +22,7 @@ #include "mibspi.h" #include "reg_het.h" -#include "Battery.h" +#include "CPLD.h" #include "DialOutFlow.h" #include "FPGA.h" #include "InternalADC.h" @@ -48,23 +48,26 @@ #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. -#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_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_SPEED 400 ///< Target pump speed (in RPM) for homing. #define DOP_HOME_TIMEOUT_MS 10000 ///< Maximum time allowed for homing to complete (in ms). @@ -77,7 +80,7 @@ #define DOP_HALL_EDGE_COUNTS_PER_REV 48 ///< Number of hall sensor edge counts per motor revolution. #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 2.0F ///< Maximum difference in speed between motor and rotor (in rotor RPM). +#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. @@ -96,26 +99,26 @@ #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 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_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.0009F ) + 0.0972F + 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,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. -#define DOP_ML_PER_MIN_FROM_PWM(pwm) ( ( ( pwm - DOP_PWM_ZERO_OFFSET ) - 0.0972F ) / 0.0009F ) +/// 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 ) @@ -149,10 +152,10 @@ #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 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 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 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 ********** @@ -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 ) { @@ -378,10 +388,11 @@ { BOOL result = FALSE; F32 pwm = DOP_MOTOR_SPEED_RPM_TO_PWM( rpm ); - F32 targetRate = DOP_ML_PER_MIN_FROM_PWM( pwm ); - result = setDialOutPumpTargetRate( (U32)targetRate, dir, PUMP_CONTROL_MODE_OPEN_LOOP ); + pwm = ( MOTOR_DIR_REVERSE == dir ? pwm * -1.0F : pwm ); + result = setDialOutPumpToFixedPWM( pwm ); + return result; } @@ -484,7 +495,6 @@ flowReadingsIdx = 0; flowReadingsCount = 0; flowReadingsTotal = 0.0; - offsetPWMDutyCyclePct = 0.0; } /*********************************************************************//** @@ -588,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 ) @@ -603,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 @@ -905,17 +945,18 @@ 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.rotorHall = ( hallSensor > 0 ? 0 : 1 ); // 1=home, 0=not home + 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; @@ -1003,7 +1044,7 @@ MOTOR_DIR_T dopMCDir, dopDir; U08 dirErrorCnt = getFPGADialOutPumpHallSensorStatus() & PUMP_DIR_ERROR_COUNT_MASK; F32 measMCSpeed = getMeasuredDialOutPumpMCSpeed(); - BOOL minDirSpeed = ( measMCSpeed >= DOP_MIN_DIR_CHECK_SPEED_RPM ? TRUE : FALSE ); + 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 @@ -1167,7 +1208,7 @@ F32 dopCurr; // only check current when we have A/C power - if ( FALSE == isACPowerLost() ) + if ( getCPLDACPowerLossDetected() != TRUE ) { // DialOut pump should be off if ( DIAL_OUT_PUMP_OFF_STATE == dialOutPumpState )