Index: firmware/App/Controllers/DialOutFlow.c =================================================================== diff -u -ra6d1ff9f7223af8efaa54f47c217d44391740405 -r85b43b31e65b2820237d864bd5e67feca03b98ee --- firmware/App/Controllers/DialOutFlow.c (.../DialOutFlow.c) (revision a6d1ff9f7223af8efaa54f47c217d44391740405) +++ firmware/App/Controllers/DialOutFlow.c (.../DialOutFlow.c) (revision 85b43b31e65b2820237d864bd5e67feca03b98ee) @@ -8,7 +8,7 @@ * @file DialOutFlow.c * * @author (last) Sean Nash -* @date (last) 23-Mar-2023 +* @date (last) 17-Jul-2023 * * @author (original) Sean * @date (original) 24-Jan-2020 @@ -20,6 +20,7 @@ #include "etpwm.h" #include "gio.h" #include "mibspi.h" +#include "reg_het.h" #include "Battery.h" #include "DialOutFlow.h" @@ -47,8 +48,10 @@ #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. @@ -60,10 +63,11 @@ #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_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). @@ -76,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. @@ -104,6 +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.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 ) @@ -141,11 +147,13 @@ 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 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 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. @@ -175,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. @@ -251,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 ); @@ -260,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 @@ -274,12 +287,20 @@ // 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 + { + // 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 ) ) { @@ -306,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 ) { @@ -375,6 +397,19 @@ /*********************************************************************//** * @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 @@ -399,11 +434,14 @@ *************************************************************************/ void signalDialOutPumpHardStop( void ) { - lastGivenRate = 0; - stopDialOutPump(); - dialOutPumpState = DIAL_OUT_PUMP_OFF_STATE; + dialOutPumpControlMode = PUMP_CONTROL_MODE_OPEN_LOOP; + dialOutPumpDirection = MOTOR_DIR_FORWARD; dialOutPumpPWMDutyCyclePct = DOP_PWM_ZERO_OFFSET; + dialOutPumpState = DIAL_OUT_PUMP_OFF_STATE; + dopControlSignal = FALSE; + lastGivenRate = 0; resetDialOutFlowMovingAverage(); + stopDialOutPump(); } /*********************************************************************//** @@ -459,7 +497,6 @@ flowReadingsIdx = 0; flowReadingsCount = 0; flowReadingsTotal = 0.0; - offsetPWMDutyCyclePct = 0.0; } /*********************************************************************//** @@ -563,8 +600,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 ) @@ -578,14 +616,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 @@ -878,6 +938,7 @@ 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(); @@ -889,6 +950,7 @@ dialOutBroadCastVariables.dopCorrOffset = dopRateCorrectionOffset; dialOutBroadCastVariables.dopCalcRate = dopMeasuredRate; dialOutBroadCastVariables.ufCalcRate = ufMeasuredRate; + dialOutBroadCastVariables.rotorHall = ( hallSensor > 0 ? 0 : 1 ); // 1=home, 0=not home broadcastData( MSG_ID_DIALYSATE_OUT_FLOW_DATA, COMM_BUFFER_OUT_CAN_HD_BROADCAST, (U08*)&dialOutBroadCastVariables, sizeof( DIAL_OUT_FLOW_DATA_T ) ); dialOutFlowDataPublicationTimerCounter = 0; @@ -976,7 +1038,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 @@ -1003,7 +1065,7 @@ 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 ) } } } @@ -1016,7 +1078,7 @@ 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 ) } } } @@ -1059,7 +1121,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(); } } @@ -1093,7 +1155,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 ); } } } @@ -1105,13 +1167,13 @@ // Check measured rotor speed vs. measured motor speed while controlling to target if ( ( deltaRotorSpeed > DOP_MAX_ROTOR_VS_MOTOR_DIFF_RPM ) && ( measMotorSpeedDeltaPct > DOP_MAX_MOTOR_SPEED_VS_TRGT_DIFF_PCT ) ) { - if ( ++errorDialOutRotorSpeedPersistTimerCtr >= ( getPumpRotorErrorPersistTime( measMotorSpeed, DOP_GEAR_RATIO ) / TASK_PRIORITY_INTERVAL ) ) + 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 ); } } } @@ -1155,7 +1217,7 @@ 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() ); } } } @@ -1177,7 +1239,7 @@ 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() ); } } }