Index: firmware/App/Controllers/DialOutFlow.c =================================================================== diff -u -r5e85dc21e918ecdae841f8398bca20ff8823c52b -r8466e63f95f65a3ffb18c3af85ac99328e41167b --- firmware/App/Controllers/DialOutFlow.c (.../DialOutFlow.c) (revision 5e85dc21e918ecdae841f8398bca20ff8823c52b) +++ firmware/App/Controllers/DialOutFlow.c (.../DialOutFlow.c) (revision 8466e63f95f65a3ffb18c3af85ac99328e41167b) @@ -44,16 +44,18 @@ #define MAX_DIAL_OUT_FLOW_RATE 650 ///< 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 DPO_FLOW_ADJ_DUE_TO_HIGHER_INLET_PRES 1.200 ///< Adjustment factor to account for higher pump inlet pressure (than DPi pump inlet). #define MAX_DIAL_OUT_PUMP_PWM_STEP_UP_CHANGE 0.0133 ///< Maximum duty cycle change when ramping up ~ 200 mL/min/s. #define MAX_DIAL_OUT_PUMP_PWM_STEP_DN_CHANGE 0.02 ///< Maximum duty cycle change when ramping down ~ 300 mL/min/s. #define MAX_DIAL_OUT_PUMP_PWM_DUTY_CYCLE 0.89 ///< Controller will error if PWM duty cycle > 90%, so set max to 89%. #define MIN_DIAL_OUT_PUMP_PWM_DUTY_CYCLE 0.10 ///< Controller will error if PWM duty cycle < 10%, so set min to 10%. +#define MAX_DIAL_OUT_PUMP_PWM_OFFSET_CONTROL 0.4 ///< Maximum PWM offset (added to DPi PWM duty cycle). +#define MIN_DIAL_OUT_PUMP_PWM_OFFSET_CONTROL -0.4 ///< Minimum PWM offset (added to DPi PWM duty cycle). +#define DOP_CONTROL_INTERVAL_SEC 5 ///< Dialysate outlet pump control interval (in seconds). /// Interval (ms/task time) at which the dialysate outlet pump is controlled. -static const U32 DOP_CONTROL_INTERVAL = ( 2000 / TASK_GENERAL_INTERVAL ); -#define DOP_P_COEFFICIENT 0.0050 ///< P term for dialysate outlet pump control. +static const U32 DOP_CONTROL_INTERVAL = ( DOP_CONTROL_INTERVAL_SEC * MS_PER_SECOND / TASK_GENERAL_INTERVAL ); +#define DOP_P_COEFFICIENT 0.0010 ///< P term for dialysate outlet pump control. #define DOP_I_COEFFICIENT 0.0001 ///< I term for dialysate outlet pump control. #define DOP_HOME_RATE 100 ///< Target pump speed (in estimate mL/min) for homing. @@ -81,13 +83,9 @@ #define DOP_MAX_CURR_WHEN_RUNNING_MA 2000.0 ///< Motor controller current should not exceed this when pump should be running. #define DOP_MAX_CURR_ERROR_DURATION_MS 2000 ///< Motor controller current errors persisting beyond this duration will trigger an alarm. -#ifndef V2_0_SYSTEM - #define DOP_SPEED_ADC_TO_RPM_FACTOR 1.751752 ///< Conversion factor from ADC counts to RPM for dialysate outlet pump motor. - #define DOP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.00025 ///< ~40 BP motor RPM = 1% PWM duty cycle -#else - #define DOP_SPEED_ADC_TO_RPM_FACTOR 1.280938 ///< Conversion factor from ADC counts to RPM for blood pump motor - #define DOP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.0003125 ///< ~32 BP motor RPM = 1% PWM duty cycle -#endif +#define DOP_SPEED_ADC_TO_RPM_FACTOR 1.751752 ///< Conversion factor from ADC counts to RPM for dialysate outlet pump motor. +#define DOP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.000193 ///< ~52 BP motor RPM = 1% PWM duty cycle +#define UF_PWM_DC_PCT_PER_ML_PER_MIN 0.004 ///< Estimated DPo PWM duty cycle amount per each mL/min of ultrafiltration desired. #define DOP_CURRENT_ADC_TO_MA_FACTOR 3.002 ///< Conversion factor from ADC counts to mA for dialysate outlet pump motor. #define DOP_ADC_FULL_SCALE_V 3.0 ///< DPo analog signals are 0-3V (while int. ADC ref V may be different). #define DOP_ADC_ZERO 1998 ///< Mid-point (zero) for ADC readings. @@ -101,6 +99,8 @@ #define DOP_PWM_ZERO_OFFSET 0.1 ///< 10% PWM duty cycle = zero speed. /// Macro converts a flow rate to an estimated PWM duty cycle %. #define DOP_PWM_FROM_ML_PER_MIN(rate) ( (rate) * DOP_ML_PER_MIN_TO_PUMP_RPM_FACTOR * DOP_GEAR_RATIO * DOP_MOTOR_RPM_TO_PWM_DC_FACTOR + DOP_PWM_ZERO_OFFSET ) +/// Conversion from PWM duty cycle % to commanded pump motor speed. +#define DOP_PWM_TO_MOTOR_SPEED_RPM(pwm) ( ((pwm) - DOP_PWM_ZERO_OFFSET) * 4000.0 ) #define PUMP_DIR_ERROR_COUNT_MASK 0x3F ///< Bit mask for pump direction error counter. @@ -190,8 +190,6 @@ static void checkDialOutPumpSpeeds( void ); static void checkDialOutPumpMCCurrent( void ); -static U32 getPublishDialOutDataInterval( void ); - /*********************************************************************//** * @brief * The initDialOutFlow function initializes the DialOutFlow module. @@ -214,9 +212,8 @@ } // Initialize dialysate outlet flow PI controller - initializePIController( PI_CONTROLLER_ID_ULTRAFILTRATION, MIN_DIAL_OUT_PUMP_PWM_DUTY_CYCLE, - DOP_P_COEFFICIENT, DOP_I_COEFFICIENT, - MIN_DIAL_OUT_PUMP_PWM_DUTY_CYCLE, MAX_DIAL_OUT_PUMP_PWM_DUTY_CYCLE ); + initializePIController( PI_CONTROLLER_ID_ULTRAFILTRATION, 0.0, DOP_P_COEFFICIENT, DOP_I_COEFFICIENT, + MIN_DIAL_OUT_PUMP_PWM_OFFSET_CONTROL, MAX_DIAL_OUT_PUMP_PWM_OFFSET_CONTROL ); } /*********************************************************************//** @@ -237,15 +234,13 @@ // Direction change while pump is running is not allowed if ( ( FALSE == isDialOutPumpOn ) || ( 0 == flowRate ) || ( dir == dialOutPumpDirectionSet ) ) { +#ifndef NO_PUMP_FLOW_LIMITS // Verify flow rate if ( flowRate <= MAX_DIAL_OUT_FLOW_RATE ) +#endif { F32 adjFlow = (F32)flowRate; - if ( PUMP_CONTROL_MODE_CLOSED_LOOP == mode ) - { - adjFlow *= DPO_FLOW_ADJ_DUE_TO_HIGHER_INLET_PRES; - } lastGivenRate = flowRate; dialOutPumpDirection = dir; dialOutPumpControlMode = mode; @@ -283,10 +278,12 @@ } result = TRUE; } +#ifndef NO_PUMP_FLOW_LIMITS else // Requested flow rate too high { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_DIAL_OUT_FLOW_SET_TOO_HIGH, flowRate ) } +#endif } return result; @@ -511,14 +508,18 @@ // Have we reached end of ramp up? else if ( dialOutPumpPWMDutyCyclePctSet >= dialOutPumpPWMDutyCyclePct ) { - resetPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, dialOutPumpPWMDutyCyclePctSet ); - dialOutPumpControlModeSet = dialOutPumpControlMode; - // If open loop mode, set PWM to requested duty cycle where it will stay during control state - if ( dialOutPumpControlModeSet == PUMP_CONTROL_MODE_OPEN_LOOP ) + dialOutPumpPWMDutyCyclePctSet = dialOutPumpPWMDutyCyclePct; + if ( dialOutPumpControlMode == PUMP_CONTROL_MODE_OPEN_LOOP ) { - dialOutPumpPWMDutyCyclePctSet = dialOutPumpPWMDutyCyclePct; - setDialOutPumpControlSignalPWM( dialOutPumpPWMDutyCyclePct ); + resetPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, dialOutPumpPWMDutyCyclePctSet ); } + else + { // Closed loop UF control is only controlling offset from DPi PWM + resetPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, (F32)( (S32)lastGivenRate - getTargetDialInFlowRate() ) * UF_PWM_DC_PCT_PER_ML_PER_MIN ); + } + dialOutPumpControlModeSet = dialOutPumpControlMode; + setDialOutPumpControlSignalPWM( dialOutPumpPWMDutyCyclePctSet ); + dopControlTimerCounter = 0; result = DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE; } // Continue ramp up @@ -552,14 +553,18 @@ // Have we reached end of ramp down? else if ( dialOutPumpPWMDutyCyclePctSet <= dialOutPumpPWMDutyCyclePct ) { - resetPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, dialOutPumpPWMDutyCyclePctSet ); - dialOutPumpControlModeSet = dialOutPumpControlMode; - // If open loop mode, set PWM to requested duty cycle where it will stay during control state - if ( dialOutPumpControlModeSet == PUMP_CONTROL_MODE_OPEN_LOOP ) + dialOutPumpPWMDutyCyclePctSet = dialOutPumpPWMDutyCyclePct; + if ( ( dialOutPumpControlModeSet == PUMP_CONTROL_MODE_OPEN_LOOP ) || ( 0 == lastGivenRate ) ) { - dialOutPumpPWMDutyCyclePctSet = dialOutPumpPWMDutyCyclePct; - setDialOutPumpControlSignalPWM( dialOutPumpPWMDutyCyclePct ); + resetPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, dialOutPumpPWMDutyCyclePctSet ); } + else + { // Closed loop UF control is only controlling offset from DPi PWM + resetPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, (F32)( (S32)lastGivenRate - getTargetDialInFlowRate() ) * UF_PWM_DC_PCT_PER_ML_PER_MIN ); + } + dialOutPumpControlModeSet = dialOutPumpControlMode; + setDialOutPumpControlSignalPWM( dialOutPumpPWMDutyCyclePctSet ); + dopControlTimerCounter = 0; result = DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE; } // Continue ramp down @@ -591,22 +596,17 @@ { F32 refVol = getTotalTargetDialOutUFVolumeInMl(); F32 totVol = getTotalMeasuredUFVolumeInMl(); - F32 newPWMDutyCyclePct; - F32 deltaPWMDutyCyclePct; + F32 offsetPWMDutyCyclePct; - // Get new PWM from PI controller - newPWMDutyCyclePct = runPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, refVol, totVol ); - // Limit PWM change to max - deltaPWMDutyCyclePct = newPWMDutyCyclePct - dialOutPumpPWMDutyCyclePctSet; - if ( fabs( deltaPWMDutyCyclePct ) > MAX_DIAL_OUT_PUMP_PWM_STEP_UP_CHANGE ) - { - newPWMDutyCyclePct = ( deltaPWMDutyCyclePct < 0.0 ? \ - dialOutPumpPWMDutyCyclePctSet - MAX_DIAL_OUT_PUMP_PWM_STEP_UP_CHANGE : \ - dialOutPumpPWMDutyCyclePctSet + MAX_DIAL_OUT_PUMP_PWM_STEP_UP_CHANGE ); - } - // Set new PWM - dialOutPumpPWMDutyCyclePctSet = newPWMDutyCyclePct; - setDialOutPumpControlSignalPWM( newPWMDutyCyclePct ); + // Get new PWM offset from PI controller + offsetPWMDutyCyclePct = runPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, refVol, totVol ); + // Add PWM offset to DPi PWM mirror for our new DPo PWM + dialOutPumpPWMDutyCyclePctSet = getDialInPumpPWMDutyCyclePct() + offsetPWMDutyCyclePct; + // Limit PWM range + dialOutPumpPWMDutyCyclePctSet = MIN( dialOutPumpPWMDutyCyclePctSet, MAX_DIAL_OUT_PUMP_PWM_DUTY_CYCLE ); + dialOutPumpPWMDutyCyclePctSet = MAX( dialOutPumpPWMDutyCyclePctSet, MIN_DIAL_OUT_PUMP_PWM_DUTY_CYCLE ); + // Apply new PWM to DPo pump + setDialOutPumpControlSignalPWM( dialOutPumpPWMDutyCyclePctSet ); } dopControlTimerCounter = 0; } @@ -638,7 +638,7 @@ *************************************************************************/ static void stopDialOutPump( void ) { - dialOutPumpPWMDutyCyclePctSet = 0.0; + dialOutPumpPWMDutyCyclePctSet = DOP_PWM_ZERO_OFFSET; setDialOutPumpControlSignalPWM( 0 ); SET_DOP_STOP(); isDialOutPumpOn = FALSE; @@ -696,7 +696,7 @@ static void publishDialOutFlowData( void ) { // Publish dialysate outlet pump and UF volume data on interval - if ( ++dialOutFlowDataPublicationTimerCounter >= getPublishDialOutDataInterval() ) + if ( ++dialOutFlowDataPublicationTimerCounter >= getU32OverrideValue( &dialOutDataPublishInterval ) ) { DIAL_OUT_FLOW_DATA_T dialOutBroadCastVariables; @@ -850,9 +850,10 @@ static void checkDialOutPumpSpeeds( void ) { F32 measMotorSpeed = getMeasuredDialOutPumpSpeed(); + F32 measMCMotorSpeed = fabs( getMeasuredDialOutPumpMCSpeed() ); // Check for pump running while commanded off - if ( FALSE == isDialOutPumpOn ) + if ( dialOutPumpPWMDutyCyclePctSet > DOP_PWM_ZERO_OFFSET ) { if ( measMotorSpeed > DOP_MAX_MOTOR_SPEED_WHILE_OFF_RPM ) { @@ -876,19 +877,20 @@ if ( DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE == dialOutPumpState ) { - F32 controllerMotorSpeed = getMeasuredDialOutPumpMCSpeed(); - F32 deltaMotorSpeed = fabs( measMotorSpeed - controllerMotorSpeed ); + F32 cmdMotorSpeed = DOP_PWM_TO_MOTOR_SPEED_RPM( dialOutPumpPWMDutyCyclePctSet ); + F32 deltaMotorSpeed = fabs( measMotorSpeed - cmdMotorSpeed ); + F32 deltaMCMotorSpeed = fabs( measMCMotorSpeed - cmdMotorSpeed ); F32 measRotorSpeed = getMeasuredDialOutPumpRotorSpeed(); F32 measMotorSpeedInRotorRPM = measMotorSpeed / DOP_GEAR_RATIO; F32 deltaRotorSpeed = fabs( measRotorSpeed - measMotorSpeedInRotorRPM ); // Check measured motor speed vs. commanded motor speed while controlling to target - if ( deltaMotorSpeed > DOP_MAX_MOTOR_SPEED_ERROR_RPM ) + if ( ( deltaMotorSpeed > DOP_MAX_MOTOR_SPEED_ERROR_RPM ) || ( deltaMCMotorSpeed > DOP_MAX_MOTOR_SPEED_ERROR_RPM ) ) { if ( ++errorDialOutMotorSpeedPersistTimerCtr >= DOP_MOTOR_SPEED_ERROR_PERSIST ) { #ifndef DISABLE_PUMP_SPEED_CHECKS - SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DIAL_OUT_PUMP_MOTOR_SPEED_CHECK, controllerMotorSpeed, measMotorSpeed ); + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DIAL_OUT_PUMP_MOTOR_SPEED_CHECK, cmdMotorSpeed, measMotorSpeed ); #endif } } @@ -979,26 +981,6 @@ /*********************************************************************//** * @brief - * The getPublishDialOutFlowDataInterval function gets the dialysate out flow - * data publication interval. - * @details Inputs: dialOutDataPublishInterval - * @details Outputs: none - * @return the current dialysate out flow data publication interval (in ms). - *************************************************************************/ -static U32 getPublishDialOutDataInterval( void ) -{ - U32 result = dialOutDataPublishInterval.data; - - if ( OVERRIDE_KEY == dialOutDataPublishInterval.override ) - { - result = dialOutDataPublishInterval.ovData; - } - - return result; -} - -/*********************************************************************//** - * @brief * The getTotalTargetDialOutUFVolumeInMl function gets the target UF volume. * @details Inputs: referenceUFVolumeInMl * @details Outputs: none