Index: firmware/App/Controllers/DialOutFlow.c =================================================================== diff -u -r2eacb97f43f15f317779635681ed81051828af4f -r58afeb5a318882fa028894b51aefba526b2fb1f0 --- firmware/App/Controllers/DialOutFlow.c (.../DialOutFlow.c) (revision 2eacb97f43f15f317779635681ed81051828af4f) +++ firmware/App/Controllers/DialOutFlow.c (.../DialOutFlow.c) (revision 58afeb5a318882fa028894b51aefba526b2fb1f0) @@ -21,16 +21,17 @@ #include "gio.h" #include "mibspi.h" +#include "DialOutFlow.h" #include "FPGA.h" #include "InternalADC.h" +#include "ModeTreatment.h" #include "OperationModes.h" #include "PIControllers.h" #include "SafetyShutdown.h" #include "SystemCommMessages.h" #include "TaskGeneral.h" #include "TaskPriority.h" #include "Timers.h" -#include "DialOutFlow.h" /** * @addtogroup DialysateOutlet @@ -41,6 +42,8 @@ /// Interval (ms/task time) at which the dialysate outlet pump data is published on the CAN bus. #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 ) #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. @@ -52,12 +55,14 @@ #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 = ( 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 P_UF 0.001 +#define P_CORR 0.1666 +#define RPM_2_ML_MIN_CONVERSION 0.215964 +#define SIZE_OF_ROLLING_AVG 100 + #define DOP_HOME_RATE 100 ///< Target pump speed (in estimate mL/min) for homing. #define DOP_HOME_TIMEOUT_MS 10000 ///< Maximum time allowed for homing to complete (in ms). /// Interval (ms/task time) at which the blood pump speed is calculated (every 40 ms). @@ -88,10 +93,10 @@ #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.000193 ///< ~52 BP motor RPM = 1% PWM duty cycle +#define DOP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.000238 ///< ~42 BP motor RPM = 1% PWM duty cycle #define DOP_CURRENT_ADC_TO_MA_FACTOR 3.002 ///< Conversion factor from ADC counts to mA for dialysate outlet pump motor. -#define DOP_REV_PER_LITER 144.7 ///< Rotor revolutions per liter. +#define DOP_REV_PER_LITER 146.84 ///< 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.0 ///< Pump motor to pump gear ratio. #define DOP_PWM_ZERO_OFFSET 0.1 ///< 10% PWM duty cycle = zero speed. @@ -150,12 +155,20 @@ static OVERRIDE_F32_T dialOutPumpMCCurrentmA = { 0.0, 0.0, 0.0, 0 }; ///< Measured dialysate outlet pump motor controller current. static OVERRIDE_F32_T dialOutPumpRotorSpeedRPM = { 0.0, 0.0, 0.0, 0 }; ///< Measured dialysate outlet pump rotor speed. static OVERRIDE_F32_T dialOutPumpSpeedRPM = { 0.0, 0.0, 0.0, 0 }; ///< Measured dialysate outlet pump motor speed. -#if 1 // TODO - remove test code + +static F32 offsetPWMDutyCyclePct = 0.0; ///< PWM duty cycle percentage offset to add to inlet pump duty cycle for UF control. +static F32 dopMeasuredRate = 0.0; static F32 ufMeasuredRate = 0.0; -static F32 prevMeasuredVolumes[2] = { 0.0, 0.0 }; -#endif +static F32 dopRateCorrectionOffset = 0.0; -static U32 dopControlTimerCounter = 0; ///< Timer counter to determine when to control dialysate outlet pump. +static U32 flowFilterTimerCount = 0; +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. +static F64 flowReadingsTotal = 0.0; ///< Rolling total - used to calc average. +static U32 flowReadingsCount = 0; ///< Number of samples in flow rolling average buffer. + +static BOOL dopControlSignal = FALSE; + static U32 dopCurrErrorDurationCtr = 0; ///< Timer counter for motor current error persistence. static U32 dopRotorRevStartTime = 0; ///< Dialysate outlet pump rotor rotation start time (in ms) @@ -193,6 +206,9 @@ static void checkDialOutPumpSpeeds( void ); static void checkDialOutPumpMCCurrent( void ); +static void resetDialOutFlowMovingAverage( void ); +static void filterDialOutFlowReadings( F64 flow ); + /*********************************************************************//** * @brief * The initDialOutFlow function initializes the DialOutFlow module. @@ -214,6 +230,11 @@ dopLastMotorHallSensorCounts[ i ] = 0; } + dopMeasuredRate = 0.0; + ufMeasuredRate = 0.0; + dopRateCorrectionOffset = 40.0; + resetDialOutFlowMovingAverage(); + // Initialize dialysate outlet flow PI controller 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 ); @@ -244,6 +265,8 @@ { F32 adjFlow = (F32)flowRate; + resetDialOutFlowMovingAverage(); + dopControlSignal = FALSE; lastGivenRate = flowRate; dialOutPumpDirection = dir; dialOutPumpControlMode = mode; @@ -322,7 +345,7 @@ stopDialOutPump(); dialOutPumpState = DIAL_OUT_PUMP_OFF_STATE; dialOutPumpPWMDutyCyclePct = 0.0; - dopControlTimerCounter = 0; + resetDialOutFlowMovingAverage(); resetPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, MIN_DIAL_OUT_PUMP_PWM_DUTY_CYCLE ); } @@ -354,6 +377,56 @@ /*********************************************************************//** * @brief + * The signalDialOutControl function signals the dialysate outlet pump to + * perform a control. Call this function when dialysate inlet pump control + * has completed. + * @details Inputs: none + * @details Outputs: dopControlSignal + * @return none + *************************************************************************/ +void signalDialOutControl( void ) +{ + dopControlSignal = TRUE; +} + +/*********************************************************************//** + * @brief + * The resetDialOutFlowMovingAverage function resets the properties of the + * dialysate outlet flow moving average sample buffer. + * @details Inputs: none + * @details Outputs: flowReadingsTotal, flowReadingsIdx, flowReadingsCount all set to zero. + * @return none + *************************************************************************/ +static void resetDialOutFlowMovingAverage( void ) +{ + flowReadingsIdx = 0; + flowReadingsCount = 0; + flowReadingsTotal = 0.0; + offsetPWMDutyCyclePct = 0.0; +} + +/*********************************************************************//** + * @brief + * The filterDialOutFlowReadings function adds a new flow sample to the filter. + * @details Inputs: none + * @details Outputs: flowReadings[], flowReadingsIdx, flowReadingsCount, flowReadingsTotal + * @return none + *************************************************************************/ +static void filterDialOutFlowReadings( F64 flow ) +{ + if ( flowReadingsCount >= SIZE_OF_ROLLING_AVG ) + { + flowReadingsTotal -= flowReadings[ flowReadingsIdx ]; + } + flowReadings[ flowReadingsIdx ] = flow; + flowReadingsTotal += flow; + flowReadingsIdx = INC_WRAP( flowReadingsIdx, 0, SIZE_OF_ROLLING_AVG - 1 ); + flowReadingsCount = INC_CAP( flowReadingsCount, SIZE_OF_ROLLING_AVG ); + dopMeasuredRate = (F32)( flowReadingsTotal / (F64)flowReadingsCount ) + dopRateCorrectionOffset; +} + +/*********************************************************************//** + * @brief * The homeDialOutPump function initiates a dialysate outlet pump home operation. * @details Inputs: dialOutPumpState * @details Outputs: dopStopAtHomePosition, dopHomeStartTime, dialysate outlet pump started (slow) @@ -406,6 +479,16 @@ // Calculate dialysate outlet pump motor speed/direction from hall sensor count updateDialOutPumpSpeedAndDirectionFromHallSensors(); + // Filter estimated dialysate out flow rate and calculate UF rate + if ( ++flowFilterTimerCount >= DIAL_OUT_FILTER_INTERVAL ) + { + flowFilterTimerCount = 0; + // Calculate DPo flow in mL/min + filterDialOutFlowReadings( getMeasuredDialOutPumpMCSpeed() * RPM_2_ML_MIN_CONVERSION ); + // Calculate UF flow in mL/min + ufMeasuredRate = dopMeasuredRate - getMeasuredDialInFlowRate(); + } + // Do not start enforcing checks until out of init/POST mode if ( getCurrentOperationMode() != MODE_INIT ) { @@ -512,17 +595,9 @@ else if ( dialOutPumpPWMDutyCyclePctSet >= dialOutPumpPWMDutyCyclePct ) { dialOutPumpPWMDutyCyclePctSet = dialOutPumpPWMDutyCyclePct; - if ( dialOutPumpControlMode == PUMP_CONTROL_MODE_OPEN_LOOP ) - { - resetPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, dialOutPumpPWMDutyCyclePctSet ); - } - else - { // Closed loop UF control is only controlling offset from DPi PWM - resetPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, dialOutPumpPWMDutyCyclePctSet - getDialInPumpPWMDutyCyclePct( TRUE ) ); - } dialOutPumpControlModeSet = dialOutPumpControlMode; + resetDialOutFlowMovingAverage(); setDialOutPumpControlSignalPWM( dialOutPumpPWMDutyCyclePctSet ); - dopControlTimerCounter = 0; result = DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE; } // Continue ramp up @@ -557,17 +632,9 @@ else if ( dialOutPumpPWMDutyCyclePctSet <= dialOutPumpPWMDutyCyclePct ) { dialOutPumpPWMDutyCyclePctSet = dialOutPumpPWMDutyCyclePct; - if ( ( dialOutPumpControlModeSet == PUMP_CONTROL_MODE_OPEN_LOOP ) || ( 0 == lastGivenRate ) ) - { - resetPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, dialOutPumpPWMDutyCyclePctSet ); - } - else - { // Closed loop UF control is only controlling offset from DPi PWM - resetPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, dialOutPumpPWMDutyCyclePctSet - getDialInPumpPWMDutyCyclePct( TRUE ) ); - } dialOutPumpControlModeSet = dialOutPumpControlMode; + resetDialOutFlowMovingAverage(); setDialOutPumpControlSignalPWM( dialOutPumpPWMDutyCyclePctSet ); - dopControlTimerCounter = 0; result = DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE; } // Continue ramp down @@ -592,23 +659,23 @@ { DIAL_OUT_PUMP_STATE_T result = DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE; - // Control at set interval - if ( ++dopControlTimerCounter >= DOP_CONTROL_INTERVAL ) + // Control when signalled (sync'd with dialysate inlet pump control - we want this control to follow inlet pump) + if ( TRUE == dopControlSignal ) { if ( dialOutPumpControlModeSet == PUMP_CONTROL_MODE_CLOSED_LOOP ) { F32 refVol = getTotalTargetDialOutUFVolumeInMl(); F32 totVol = getTotalMeasuredUFVolumeInMl(); - F32 offsetPWMDutyCyclePct; + F32 volErr = refVol - totVol; + F32 ufrErr = getCurrentUFSetRate() - ufMeasuredRate; -#if 1 // TODO - remove test code - ufMeasuredRate = ( totVol - prevMeasuredVolumes[1] ) * 6.0; // calculate UF rate based on change in volume over last 10 seconds - prevMeasuredVolumes[1] = prevMeasuredVolumes[0]; // update last 2 volumes for next time - prevMeasuredVolumes[0] = totVol; -#endif + dopControlSignal = FALSE; - // Get new PWM offset from PI controller - offsetPWMDutyCyclePct = runPIController( PI_CONTROLLER_ID_ULTRAFILTRATION, refVol, totVol ); + dopRateCorrectionOffset += ( ( totVol - refVol ) * P_CORR ); + + offsetPWMDutyCyclePct += volErr * DOP_P_COEFFICIENT; + offsetPWMDutyCyclePct += ( ufrErr * P_UF ); + // Add PWM offset to DPi PWM mirror for our new DPo PWM dialOutPumpPWMDutyCyclePctSet = getDialInPumpPWMDutyCyclePct( FALSE ) + offsetPWMDutyCyclePct; // Limit PWM range @@ -617,7 +684,6 @@ // Apply new PWM to DPo pump setDialOutPumpControlSignalPWM( dialOutPumpPWMDutyCyclePctSet ); } - dopControlTimerCounter = 0; } return result;