Index: firmware/App/Controllers/ROPump.c =================================================================== diff -u -r6d85a90d2325163e936055a620a8b6aa1a6db65a -r30c175a7e10411efadb9b2b0f8b68528705e0621 --- firmware/App/Controllers/ROPump.c (.../ROPump.c) (revision 6d85a90d2325163e936055a620a8b6aa1a6db65a) +++ firmware/App/Controllers/ROPump.c (.../ROPump.c) (revision 30c175a7e10411efadb9b2b0f8b68528705e0621) @@ -14,6 +14,7 @@ * @date (original) 12-Nov-2024 * ***************************************************************************/ +#include // for log() #include "Flow.h" //#include "NVDataMgmt.h" @@ -38,21 +39,24 @@ #define RO_PUMP_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) ///< Interval (ms/task time) at which the RO Pump data is published on the CAN bus. #define ROP_CONTROL_INTERVAL ( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) ///< Interval (ms/task time) at which the RO pump is controlled. -#define ROP_FLOW_CONTROL_P_COEFFICIENT 0.001F ///< P term for RO pump flow control. -#define ROP_FLOW_CONTROL_I_COEFFICIENT 0.009F ///< I term for RO pump flow control. +#define ROP_FLOW_CONTROL_P_COEFFICIENT 0.1F ///< P term for RO pump flow control. +#define ROP_FLOW_CONTROL_I_COEFFICIENT 0.8F ///< I term for RO pump flow control. +#define ROP_MIN_FLOW_TO_CONTROL_PCT 0.75F #define ROP_PRESSURE_CONTROL_P_COEFFICIENT 0.15F ///< P term for RO pump pressure control. #define ROP_PRESSURE_CONTROL_I_COEFFICIENT 0.65F ///< I term for RO pump pressure control. -#define ROP_FLOW_TO_PWM_SLOPE 0.5F ///< Slope of flow to PWM line equation. -#define ROP_FLOW_TO_PWM_INTERCEPT 0.0F ///< Intercept of flow to PWM line equation. -#define ROP_PRESSURE_TO_PWM_SLOPE 0.5F ///< Slope of pressure to PWM line equation. +#define ROP_FLOW_TO_PWM_SLOPE 155.31F ///< Slope of flow to PWM line equation. +#define ROP_FLOW_TO_PWM_INTERCEPT -699.99F ///< Intercept of flow to PWM line equation. +#define ROP_PRESSURE_TO_PWM_SLOPE 0.5F ///< Slope of pressure to PWM line equation. #define ROP_PRESSURE_TO_PWM_INTERCEPT 0.0F ///< Intercept of pressure to PWM line equation. #define DATA_PUBLISH_COUNTER_START_COUNT 10 ///< Data publish counter start count. -#define ROP_FLOW_TO_PWM_PCT(flow) ( ( ROP_FLOW_TO_PWM_SLOPE * flow + ROP_FLOW_TO_PWM_INTERCEPT ) / MAX_FLUID_PUMP_PWM_DUTY_CYCLE ) ///< PWM line equation for flow converted to percentage. -#define ROP_PRESSURE_TO_PWM_PCT(pres) ( ( ROP_PRESSURE_TO_PWM_SLOPE * pres + ROP_PRESSURE_TO_PWM_INTERCEPT ) / MAX_FLUID_PUMP_PWM_DUTY_CYCLE ) ///< PWM line equation for pressure. converted to percentage +#define ROP_PRESSURE_TO_PWM_PCT(pres) ( ( ROP_PRESSURE_TO_PWM_SLOPE * pres + ROP_PRESSURE_TO_PWM_INTERCEPT ) / MAX_FLUID_PUMP_PWM_DUTY_CYCLE ) ///< PWM line equation for pressure. converted to percentage. +#define ROP_FLOW_TO_PWM_PCT(flow) ( ( ROP_FLOW_TO_PWM_SLOPE * log(flow) + ROP_FLOW_TO_PWM_INTERCEPT ) / MAX_FLUID_PUMP_PWM_DUTY_CYCLE ) ///< PWM line equation for flow converted to percentage. +#define ROP_FLOW_TO_PWM(flow) ( ROP_FLOW_TO_PWM_SLOPE * log(flow) + ROP_FLOW_TO_PWM_INTERCEPT ) ///< PWM line equation for flow converted to percentage. + // ********** private data ********** static RO_PUMP_STATE_T roPumpState; ///< Current state of pump controller state machine. @@ -66,6 +70,7 @@ static OVERRIDE_F32_T targetROPumpPressure; ///< Target RO max allowed pressure (in PSI). static F32 roPumpDutyCyclePctSet; ///< Currently set RO pump PWM duty cycle. static OVERRIDE_F32_T roPumpOpenLoopTargetDutyCycle; ///< Target RO pump open loop PWM. +static BOOL roPumpStartControl; ///< boolean to determine when closed loop flow control starts // ********** private function prototypes ********** @@ -93,8 +98,8 @@ initFluidPump(); // Initialize RO pump PI controller to flow - initializePIController( PI_CONTROLLER_ID_RO_PUMP_FLOW, MIN_FLUID_PUMP_DUTY_CYCLE_PCT, ROP_FLOW_CONTROL_P_COEFFICIENT, ROP_FLOW_CONTROL_I_COEFFICIENT, - MIN_FLUID_PUMP_DUTY_CYCLE_PCT, MAX_FLUID_PUMP_DUTY_CYCLE_PCT, FALSE, 0 ); + initializePIController( PI_CONTROLLER_ID_RO_PUMP_FLOW, MIN_RO_FLOWRATE_MLPM, ROP_FLOW_CONTROL_P_COEFFICIENT, ROP_FLOW_CONTROL_I_COEFFICIENT, + MIN_RO_FLOWRATE_MLPM, MAX_RO_FLOWRATE_MLPM, FALSE, 0 ); // Initialize RO pump PI controller to target pressure initializePIController( PI_CONTROLLER_ID_RO_PUMP_PRES, MIN_FLUID_PUMP_DUTY_CYCLE_PCT, ROP_PRESSURE_CONTROL_P_COEFFICIENT, ROP_PRESSURE_CONTROL_I_COEFFICIENT, @@ -106,7 +111,7 @@ stopPumpRequest = FALSE; roControlTimerCounter = 0; roPumpDutyCyclePctSet = 0.0F; - + roPumpStartControl = FALSE; roPumpDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; roPumpDataPublishInterval.data = RO_PUMP_DATA_PUB_INTERVAL; roPumpDataPublishInterval.ovData = RO_PUMP_DATA_PUB_INTERVAL; @@ -188,7 +193,7 @@ // Set pump to on isROPumpOn = TRUE; roPumpDutyCyclePctSet = roPumpPresToPWM( getTargetROPumpPressure() ); - resetPIController( PI_CONTROLLER_ID_RO_PUMP_PRES, roPumpDutyCyclePctSet, 0.0F ); + resetPIController( PI_CONTROLLER_ID_RO_PUMP_PRES, getTargetROPumpPressure(), 0.0F ); setFluidPumpPctToPWMDutyCycle( P12_PUMP, roPumpDutyCyclePctSet ); state = RO_PUMP_CONTROL_TO_TARGET_PRESSURE_STATE; } @@ -199,8 +204,9 @@ // Set pump to on isROPumpOn = TRUE; roPumpDutyCyclePctSet = roPumpFlowToPWM( getTargetROPumpFlowRateMLPM() ); - resetPIController( PI_CONTROLLER_ID_RO_PUMP_FLOW, roPumpDutyCyclePctSet, 0.0F ); + resetPIController( PI_CONTROLLER_ID_RO_PUMP_FLOW, getTargetROPumpFlowRateMLPM(), 0.0F ); setFluidPumpPctToPWMDutyCycle( P12_PUMP, roPumpDutyCyclePctSet ); + roPumpStartControl = FALSE; state = RO_PUMP_CONTROL_TO_TARGET_FLOW_STATE; } // If the target duty cycle is greater than zero (minimum is 10%) and the mode has been set to open @@ -238,15 +244,15 @@ if ( ( getTargetROPumpPressure() > 0.0F ) && ( PUMP_CONTROL_MODE_CLOSED_LOOP == roPumpControlMode ) ) { //transition to closed loop - resetPIController( PI_CONTROLLER_ID_RO_PUMP_PRES, getTargetROPumpDutyCyclePCT(), 0 ); + resetPIController( PI_CONTROLLER_ID_RO_PUMP_PRES, getTargetROPumpPressure(), 0 ); state = RO_PUMP_CONTROL_TO_TARGET_PRESSURE_STATE; } // If there is a target flow set, transition to the PI controller and control to flow else if ( ( getTargetROPumpFlowRateMLPM() > 0 ) && ( PUMP_CONTROL_MODE_CLOSED_LOOP == roPumpControlMode ) ) { ///transition to closed loop - resetPIController( PI_CONTROLLER_ID_RO_PUMP_FLOW, getTargetROPumpDutyCyclePCT(), 0 ); + resetPIController( PI_CONTROLLER_ID_RO_PUMP_FLOW, getTargetROPumpFlowRateMLPM(), 0 ); state = RO_PUMP_CONTROL_TO_TARGET_FLOW_STATE; } @@ -264,12 +270,13 @@ static RO_PUMP_STATE_T handleROPumpControlToTargetFlowState( void ) { RO_PUMP_STATE_T state = RO_PUMP_CONTROL_TO_TARGET_FLOW_STATE; + F32 nexttgtflow = 0.0; // Check if need to switch control modes if ( ( getTargetROPumpPressure() > 0.0F ) && ( PUMP_CONTROL_MODE_CLOSED_LOOP == roPumpControlMode ) ) { // Transition to target pressure - resetPIController( PI_CONTROLLER_ID_RO_PUMP_PRES, roPumpDutyCyclePctSet, 0 ); + resetPIController( PI_CONTROLLER_ID_RO_PUMP_PRES, getTargetROPumpPressure(), 0 ); state = RO_PUMP_CONTROL_TO_TARGET_PRESSURE_STATE; } else if ( ( getTargetROPumpDutyCyclePCT() > 0.0F ) && ( PUMP_CONTROL_MODE_OPEN_LOOP == roPumpControlMode ) ) @@ -283,11 +290,23 @@ if ( ( (F32)getTargetROPumpFlowRateMLPM() == 0.0F ) && ( PUMP_CONTROL_MODE_CLOSED_LOOP == roPumpControlMode ) ) { signalROPumpHardStop(); + roPumpStartControl = FALSE; } else if ( ( ++roControlTimerCounter >= ROP_CONTROL_INTERVAL ) && ( PUMP_CONTROL_MODE_CLOSED_LOOP == roPumpControlMode ) ) { - roPumpDutyCyclePctSet = runPIController( PI_CONTROLLER_ID_RO_PUMP_FLOW, (F32)getTargetROPumpFlowRateMLPM(), getFilteredFlow( P16_FLOW ) ); - setFluidPumpPctToPWMDutyCycle( P12_PUMP, roPumpDutyCyclePctSet ); + // P16 flow seems to lag in current Leahi HW. We will wait till we hit a % of target flow before we start changing control. + if( ( TRUE == roPumpStartControl ) || ( getFilteredFlow( P16_FLOW ) >= ( (F32)getTargetROPumpFlowRateMLPM() * ROP_MIN_FLOW_TO_CONTROL_PCT ) ) ) + { + //roPumpDutyCyclePctSet = runPIController( PI_CONTROLLER_ID_RO_PUMP_FLOW, (F32)getTargetROPumpFlowRateMLPM(), getFilteredFlow( P16_FLOW ) ); + //setFluidPumpPctToPWMDutyCycle( P12_PUMP, roPumpDutyCyclePctSet ); + nexttgtflow = runPIController( PI_CONTROLLER_ID_RO_PUMP_FLOW, (F32)getTargetROPumpFlowRateMLPM(), getFilteredFlow( P16_FLOW ) ); + nexttgtflow = (U16)MIN( ROP_FLOW_TO_PWM( nexttgtflow ), ( MAX_FLUID_PUMP_PWM_DUTY_CYCLE * MAX_FLUID_PUMP_DUTY_CYCLE_PCT ) ); + setFluidPumpPWMDutyCycle( P12_PUMP, nexttgtflow ); + if ( FALSE == roPumpStartControl ) + { + roPumpStartControl = TRUE; + } + } roControlTimerCounter = 0; } @@ -309,7 +328,7 @@ // Check if we are changing control if ( ( getTargetROPumpFlowRateMLPM() > 0 ) && ( PUMP_CONTROL_MODE_CLOSED_LOOP == roPumpControlMode ) ) { - resetPIController( PI_CONTROLLER_ID_RO_PUMP_FLOW, roPumpDutyCyclePctSet, 0 ); + resetPIController( PI_CONTROLLER_ID_RO_PUMP_FLOW, getTargetROPumpFlowRateMLPM(), 0 ); state = RO_PUMP_CONTROL_TO_TARGET_FLOW_STATE; } else if ( ( getTargetROPumpDutyCyclePCT() > 0.0F ) && ( PUMP_CONTROL_MODE_OPEN_LOOP == roPumpControlMode ) ) @@ -349,7 +368,7 @@ BOOL result = FALSE; // First of all, the flow rate must be in range - if ( ( roFlowRate <= MAX_RO_FLOWRATE_MLPM ) && ( roFlowRate >= MIN_RO_FLOWRATE_MLPM ) ) + if ( roFlowRate <= MAX_RO_FLOWRATE_MLPM ) { targetROPumpFlowRate.data = roFlowRate; roPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP;