Index: firmware/App/Controllers/ROPump.c =================================================================== diff -u -rd7024c2aa2768c24bab587d5db836f096d4a7989 -r55425a4c5370a6fa1faad61dc24fcd76b854d3ed --- firmware/App/Controllers/ROPump.c (.../ROPump.c) (revision d7024c2aa2768c24bab587d5db836f096d4a7989) +++ firmware/App/Controllers/ROPump.c (.../ROPump.c) (revision 55425a4c5370a6fa1faad61dc24fcd76b854d3ed) @@ -48,44 +48,53 @@ #define RO_PUMP_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) ///< Interval (ms/task time) at which the RO Pump data is published on the CAN bus. -#define MAX_RO_PUMP_DUTY_CYCLE 0.99 ///< max duty cycle. -#define MIN_RO_PUMP_DUTY_CYCLE 0.0 ///< min duty cycle. +#define MAX_RO_PUMP_DUTY_CYCLE 0.99 ///< Max duty cycle. +#define MIN_RO_PUMP_DUTY_CYCLE 0.0 ///< Min duty cycle. -#define ROP_CONTROL_INTERVAL ( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) ///< interval (ms/task time) at which the RO pump is controlled. +#define ROP_CONTROL_INTERVAL ( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) ///< Interval (ms/task time) at which the RO pump is controlled. +#define ROP_RAMP_UP_CONTROL_INTERVAL ( 500 / TASK_GENERAL_INTERVAL ) ///< Interval (ms/task time) at which the RO pump is controlled. #define ROP_P_COEFFICIENT 0.0020 ///< P term for RO pump pressure control. #define ROP_I_COEFFICIENT 0.0015 ///< I term for RO pump pressure control. #define ROP_RAMP_UP_P_COEFFICIENT 0.0 ///< P term for RO pump flow control. #define ROP_RAMP_UP_I_COEFFICIENT 0.1 ///< I term for RO pump flow control. -#define ROP_FLOW_TARGET_TOLERANCE 0.05 ///< Tolerance in between the target flow rate and the actual flow rate in liter. +#define ROP_FLOW_TARGET_TOLERANCE 0.03 ///< Tolerance in between the target flow rate and the actual flow rate in percentage. #define FLOW_SENSOR_ZERO_READING 0xFFFF ///< Flow sensor reading indicates zero flow (or flow lower than can be detected by sensor). #define FLOW_SAMPLES_TO_AVERAGE ( 250 / TASK_PRIORITY_INTERVAL ) ///< Averaging flow data over 250 ms intervals. #define FLOW_AVERAGE_MULTIPLIER ( 1.0 / (F32)FLOW_SAMPLES_TO_AVERAGE ) ///< Optimization - multiplying is faster than dividing. -#define FLOW_VERIFICATION_COUNTER_TARGET 40U ///< The time in counts to check the flow and make sure it is in range. -#define RO_FLOW_ADC_TO_LPM_FACTOR 10909.0909 ///< conversion factor from ADC counts to LPM (liters/min) for RO flow rate (multiply this by inverse of FPGA reading). +/// The time in counts to check the flow and make sure it is in range. +#define FLOW_VERIFICATION_COUNTER_TARGET ( 2 * MS_PER_SECOND / TASK_GENERAL_INTERVAL ) -#define ROP_FLOW_TO_PWM_DC(flow) ( (F32)( flow / MAX_RO_FLOWRATE_LPM ) ) ///< Initial conversion factor from target flow rate to PWM duty cycle estimate. +#define RO_FLOW_ADC_TO_LPM_FACTOR 5555 ///< Conversion factor from ADC counts to LPM (liters/min) for RO flow rate (multiply this by inverse of FPGA reading). +#define ROP_FLOW_TO_PWM_SLOPE 0.5 ///< Slope of flow to PWM line equation. +#define ROP_FLOW_TO_PWM_INTERCEPT 0.28 ///< Intercept of flow to PWM line equation. + + /// Initial conversion factor from target flow rate to PWM duty cycle estimate. +#define ROP_FLOW_TO_PWM_DC(flow) ( ROP_FLOW_TO_PWM_SLOPE * flow + ROP_FLOW_TO_PWM_INTERCEPT ) + #define FLOW_SENSOR_ZERO_READING 0xFFFF ///< Flow sensor reading indicates zero flow (or flow lower than can be detected by sensor). #define MAX_ALLOWED_FLOW_DEVIATION 0.1 ///< Max allowed deviation from target flow. -#define FLOW_OUT_OF_RANGE_PERSISTENT_INTERVAL ( 5 * MS_PER_SECOND ) ///< Flow out of range time out in counts. +#define FLOW_OUT_OF_RANGE_PERSISTENT_INTERVAL ( 10 * MS_PER_SECOND ) ///< Flow out of range time out in counts. #define MAX_PRESSURE_TARGET_TOLERANCE 5 ///< Pressure tolerance from maximum set pressure by user in psi. #define MAX_ALLOWED_PRESSURE_PSI 130 ///< Maximum allowed pressure that the RO pump can go to. #define MIN_ALLOWED_PRESSURE_PSI 10 ///< Minimum allowed pressure that the RO pump can go to. #define MAX_ALLOWED_MEASURED_PRESSURE_PSI 135 ///< Maximum allowed pressure that the sensor measures. RO pump shut off pressure is 140psi. #define MAX_PRESSURE_OUT_OF_RANGE_PERSISTENT_INTERVAL MS_PER_SECOND ///< Maximum allowed time that the pressure can be very high. -#define MAX_ALLOWED_RAMP_UP_TIME ( 5 * MS_PER_SECOND ) ///< Maximum allowed ramp up time to a flow rate in ms. -#define ROP_PSI_TO_PWM_DC(p) ( 0.2 + ( (F32)((p) - 100) * 0.01 ) ) ///< conversion factor from target PSI to PWM duty cycle estimate. -#define SAFETY_SHUTDOWN_TIMEOUT ( 2 * MS_PER_SECOND ) ///< RO pump safety shutdown activation timeout in ms. +#define MAX_ALLOWED_RAMP_UP_TIME ( 10 * MS_PER_SECOND ) ///< Maximum allowed ramp up time to a flow rate in ms. +#define ROP_PSI_TO_PWM_DC(p) ( 0.2 + ( (F32)((p) - 100) * 0.01 ) ) ///< Conversion factor from target PSI to PWM duty cycle estimate. +#define SAFETY_SHUTDOWN_TIMEOUT ( 3 * MS_PER_SECOND ) ///< RO pump safety shutdown activation timeout in ms. +#define ROP_FLOW_STABILIZE_TIME ( 3 * MS_PER_SECOND ) ///< Time required for RO flow to stabilize. /// Enumeration of RO pump states. typedef enum ROPump_States { RO_PUMP_OFF_STATE = 0, ///< RO pump off state + RO_PUMP_CONTROL_SETUP_STATE, ///< RO pump control setup state RO_PUMP_RAMP_UP_STATE, ///< RO pump ramp up to target flow rate state RO_PUMP_VERIFY_FLOW_STATE, ///< RO pump maintain the flow rate for a set period of time RO_PUMP_CONTROL_TO_TARGET_STATE, ///< RO pump control to target pressure state @@ -113,7 +122,7 @@ static PUMP_CONTROL_MODE_T roPumpControlModeSet = PUMP_CONTROL_MODE_CLOSED_LOOP; ///< currently set RO pump control mode. static F32 targetROPumpFlowRate = 0.0; ///< Target RO flow rate (in L/min). -static U32 targetROPumpPressure = 0; ///< Target RO pressure (in PSI). +static F32 targetROPumpPressure = 0; ///< Target RO pressure (in PSI). static OVERRIDE_U32_T roPumpDataPublishInterval = { RO_PUMP_DATA_PUB_INTERVAL, RO_PUMP_DATA_PUB_INTERVAL, @@ -133,10 +142,13 @@ static S32 measuredFlowReadingsSum = 0; ///< Raw flow reading sums for averaging. static U32 flowFilterCounter = 0; ///< Flow filtering counter. +static U32 roPumpOnStartTime = 0; ///< Start time when RO pump turned on. +static BOOL setupROPumpControl = 0; ///< Flag to indicate if we need to setup RO pump control. // ********** private function prototypes ********** static RO_PUMP_STATE_T handleROPumpOffState( void ); +static RO_PUMP_STATE_T handleROPumpControlSetupState( void ); static RO_PUMP_STATE_T handleROPumpRampUpState( void ); static RO_PUMP_STATE_T handleROPumpVerifyFlowState( void ); static RO_PUMP_STATE_T handleROPumpControlToTargetState( void ); @@ -227,6 +239,7 @@ roPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; // Get the initial guess of the duty cycle roPumpPWMDutyCyclePct = ROP_FLOW_TO_PWM_DC( roFlowRate ); + setupROPumpControl = TRUE; result = TRUE; } // Requested max pressure is out of range @@ -317,18 +330,15 @@ // meaning, it is controlling to a certain pressure if ( roPumpControlMode == PUMP_CONTROL_MODE_CLOSED_LOOP && roPumpState == RO_PUMP_CONTROL_TO_TARGET_STATE ) { - F32 currentFlow = getMeasuredROFlowRate(); - F32 targetFlow = getTargetROPumpFlowRate(); - F32 error = fabs( 1.0 - ( currentFlow / targetFlow ) ); - + F32 const currentFlow = getMeasuredROFlowRate(); + F32 const targetFlow = getTargetROPumpFlowRate(); + BOOL const isFlowOutOfRange = fabs( 1.0 - ( currentFlow / targetFlow ) ) > MAX_ALLOWED_FLOW_DEVIATION; // Figure out whether flow is out of range from which side - if ( error > MAX_ALLOWED_FLOW_DEVIATION ) - { - BOOL isFlowOutOfUpperRange = currentFlow > targetFlow; - BOOL isFlowOutOfLowerRange = currentFlow < targetFlow; - checkPersistentAlarm( ALARM_ID_FLOW_RATE_OUT_OF_UPPER_RANGE, isFlowOutOfUpperRange, currentFlow, targetFlow ); - checkPersistentAlarm( ALARM_ID_FLOW_RATE_OUT_OF_LOWER_RANGE, isFlowOutOfLowerRange, currentFlow, targetFlow ); - } + BOOL const isFlowOutOfUpperRange = isFlowOutOfRange && ( currentFlow > targetFlow ); + BOOL const isFlowOutOfLowerRange = isFlowOutOfRange && ( currentFlow < targetFlow ); + + checkPersistentAlarm( ALARM_ID_FLOW_RATE_OUT_OF_UPPER_RANGE, isFlowOutOfUpperRange, currentFlow, targetFlow ); + checkPersistentAlarm( ALARM_ID_FLOW_RATE_OUT_OF_LOWER_RANGE, isFlowOutOfLowerRange, currentFlow, targetFlow ); } // If the pump is off and PPi + 5psi < PPo for a certain period of time, activate safety shutdown @@ -365,6 +375,10 @@ roPumpState = handleROPumpOffState(); break; + case RO_PUMP_CONTROL_SETUP_STATE: + roPumpState = handleROPumpControlSetupState(); + break; + case RO_PUMP_RAMP_UP_STATE: roPumpState = handleROPumpRampUpState(); break; @@ -422,11 +436,8 @@ * @brief * The handleROPumpOffState function handles the RO pump off state of the * controller state machine. - * @details Inputs: roPumpControlMode, roPumpControlModeSet, roPumpControlMode, - * roPumpPWMDutyCyclePctSet, roPumpPWMDutyCyclePct, isROPumpOn, - * roPumpOpenLoopTargetDutyCycle - * @details Outputs: roPumpControlModeSet, roPumpPWMDutyCyclePctSet, - * roPumpPWMDutyCyclePct, isROPumpOn + * @details Inputs: roPumpControlMode, roPumpPWMDutyCyclePctSet, roPumpOpenLoopTargetDutyCycle + * @details Outputs: roPumpPWMDutyCyclePct, setupROPumpControl, isROPumpOn * @return next state of the controller state machine *************************************************************************/ static RO_PUMP_STATE_T handleROPumpOffState( void ) @@ -436,15 +447,10 @@ // If there is a flow, transition to the PI controller to get the corresponding pressure of that flow if ( getTargetROPumpFlowRate() > 0 && roPumpControlMode == PUMP_CONTROL_MODE_CLOSED_LOOP ) { - roPumpControlModeSet = roPumpControlMode; - // Set initial PWM duty cycle - roPumpDutyCyclePctSet = roPumpPWMDutyCyclePct; - setROPumpControlSignalDutyCycle( roPumpDutyCyclePctSet ); - // Reset controller - resetPIController( I_CONTROLLER_ID_RO_PUMP_RAMP_UP, roPumpDutyCyclePctSet ); // Set pump to on isROPumpOn = TRUE; - result = RO_PUMP_RAMP_UP_STATE; + result = RO_PUMP_CONTROL_SETUP_STATE; + setupROPumpControl = TRUE; } // If the target duty cycle is greater than zero (minimum is 10%) and the mode has been set to open // loop, set the duty cycle @@ -460,6 +466,38 @@ /*********************************************************************//** * @brief + * The handleROPumpControlSetupState function handles the RO pump control + * setup state of the controller state machine. + * @details Inputs: setupROPumpControl, roPumpPWMDutyCyclePct + * @details Outputs: setup RO pump PI controller and wait for flow stabilize + * @return next state of the controller state machine + *************************************************************************/ +static RO_PUMP_STATE_T handleROPumpControlSetupState( void ) +{ + RO_PUMP_STATE_T result = RO_PUMP_CONTROL_SETUP_STATE; + + if ( TRUE == setupROPumpControl ) + { + roPumpControlModeSet = roPumpControlMode; + // Set initial PWM duty cycle + roPumpDutyCyclePctSet = roPumpPWMDutyCyclePct; + setROPumpControlSignalDutyCycle( roPumpDutyCyclePctSet ); + // Reset controller + resetPIController( I_CONTROLLER_ID_RO_PUMP_RAMP_UP, roPumpDutyCyclePctSet ); + setupROPumpControl = FALSE; + roPumpOnStartTime = getMSTimerCount(); + } + + if ( TRUE == didTimeout( roPumpOnStartTime, ROP_FLOW_STABILIZE_TIME ) ) + { + result = RO_PUMP_RAMP_UP_STATE; + } + + return result; +} + +/*********************************************************************//** + * @brief * The handleROPumpRampUpState function handles the RO pump ramp up state * of the controller state machine. * @details Inputs: roControlTimerCounter, roPumpPWMDutyCyclePctSet, @@ -477,19 +515,18 @@ F32 targetFlowRate = getTargetROPumpFlowRate(); F32 actualFlowRate = (F32)getMeasuredROFlowRate(); - BOOL isFlowInRange = fabs( targetFlowRate - actualFlowRate ) > ROP_FLOW_TARGET_TOLERANCE; + F32 flowRateDeviation = fabs( targetFlowRate - actualFlowRate ) / targetFlowRate; + BOOL isFlowOutOfRange = flowRateDeviation > ROP_FLOW_TARGET_TOLERANCE; - // Check if the ramp up has timed out - checkPersistentAlarm( ALARM_ID_RO_PUMP_RAMP_UP_TO_FLOW_TIMEOUT, isFlowInRange, actualFlowRate, MAX_ALLOWED_RAMP_UP_TIME ); - // If the ramp up persistent alarm is active, turn off the pump and go to off state - if ( isAlarmActive( ALARM_ID_RO_PUMP_RAMP_UP_TO_FLOW_TIMEOUT ) ) + if ( TRUE == isPersistentAlarmTriggered( ALARM_ID_RO_PUMP_RAMP_UP_TO_FLOW_TIMEOUT, isFlowOutOfRange ) ) { stopROPump(); result = RO_PUMP_OFF_STATE; + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_RO_PUMP_RAMP_UP_TO_FLOW_TIMEOUT, actualFlowRate, MAX_ALLOWED_RAMP_UP_TIME ); } // Control at set interval - else if ( ++roControlTimerCounter >= ROP_CONTROL_INTERVAL ) + else if ( ++roControlTimerCounter >= ROP_RAMP_UP_CONTROL_INTERVAL ) { F32 targetPressure = getTargetROPumpPressure(); @@ -502,20 +539,31 @@ result = RO_PUMP_CONTROL_TO_TARGET_STATE; } // If the actual flow is still far from target flow, update the duty cycle using the I controller and stay in this state - else if ( fabs( actualFlowRate - targetFlowRate ) > ROP_FLOW_TARGET_TOLERANCE ) + else if ( TRUE == isFlowOutOfRange ) { roPumpDutyCyclePctSet = runPIController( I_CONTROLLER_ID_RO_PUMP_RAMP_UP, targetFlowRate, actualFlowRate ); setROPumpControlSignalDutyCycle( roPumpDutyCyclePctSet ); } // Reached to the target flow go to the next state else { + // Reset all the variables to prepare for transition + flowVerificationCounter = 0; + roPumpFlowRateRunningSum = 0; + roPumpPressureRunningSum = 0; result = RO_PUMP_VERIFY_FLOW_STATE; } roControlTimerCounter = 0; } + if ( TRUE == setupROPumpControl ) + { + // Clear persistent alarm + isPersistentAlarmTriggered( ALARM_ID_RO_PUMP_RAMP_UP_TO_FLOW_TIMEOUT, FALSE ); + result = RO_PUMP_CONTROL_SETUP_STATE; + } + return result; } @@ -540,39 +588,36 @@ // Check if the time for flow verification has elapsed if ( ++flowVerificationCounter >= FLOW_VERIFICATION_COUNTER_TARGET ) { - F32 actualPressure = getMeasuredDGPressure( PRESSURE_SENSOR_RO_PUMP_OUTLET ); - // If the flow has been achieved without reaching to the maximum pressure, set the new pressure - // otherwise, stay with the maximum allowed pressure as target pressure - if ( actualPressure < getTargetROPumpPressure() ) + // Calculate the average pressure and flow rate + F32 const targetFlowRate = getTargetROPumpFlowRate(); + F32 const avgFlowRate = roPumpFlowRateRunningSum / flowVerificationCounter; + F32 const flowRateDeviation = fabs( targetFlowRate - avgFlowRate ) / targetFlowRate; + + if ( flowRateDeviation < ROP_FLOW_TARGET_TOLERANCE ) { - // Calculate the average pressure and flow rate - F32 avgPressure = roPumpPressureRunningSum / flowVerificationCounter; - F32 avgFlowRate = roPumpFlowRateRunningSum / flowVerificationCounter; + F32 const avgPressure = roPumpPressureRunningSum / flowVerificationCounter; + // If the flow has been achieved without reaching to the maximum pressure, set the new pressure + // otherwise, stay with the maximum allowed pressure as target pressure + if ( avgPressure < getTargetROPumpPressure() ) + { + targetROPumpPressure = avgPressure; + } - F32 targetFlowRate = getTargetROPumpFlowRate(); - - // Calculate the flow rate deviation from the target flow rate - F32 flowRateDeviation = ( targetFlowRate - avgFlowRate ) / targetFlowRate; - // Use the flow rate deviation to adjust the average calculated pressure. This - // pressure is used as the target pressure - avgPressure = avgPressure + ( avgPressure * flowRateDeviation ); - // Save the target pressure - targetROPumpPressure = avgPressure; + // Set initial PWM duty cycle and reset controller + setROPumpControlSignalDutyCycle( roPumpDutyCyclePctSet ); + resetPIController( PI_CONTROLLER_ID_RO_PUMP, roPumpDutyCyclePctSet ); + result = RO_PUMP_CONTROL_TO_TARGET_STATE; } + else + { + result = RO_PUMP_RAMP_UP_STATE; + } + } - // Reset the I controller for the flow rate as it is no longer needed - resetPIController( I_CONTROLLER_ID_RO_PUMP_RAMP_UP, MIN_RO_PUMP_DUTY_CYCLE ); - - // Set initial PWM duty cycle - setROPumpControlSignalDutyCycle( roPumpDutyCyclePctSet ); - // Reset controller - resetPIController( PI_CONTROLLER_ID_RO_PUMP, roPumpDutyCyclePctSet ); - // Reset all the variables before leaving - flowVerificationCounter = 0; - roPumpFlowRateRunningSum = 0; - roPumpPressureRunningSum = 0; - result = RO_PUMP_CONTROL_TO_TARGET_STATE; + if ( TRUE == setupROPumpControl ) + { + result = RO_PUMP_CONTROL_SETUP_STATE; } return result; @@ -603,6 +648,11 @@ roControlTimerCounter = 0; } + if ( TRUE == setupROPumpControl ) + { + result = RO_PUMP_CONTROL_SETUP_STATE; + } + return result; }