Index: firmware/App/Controllers/DrainPump.c =================================================================== diff -u -r6499ea25921fcf67826fa0c35bb03caf411ba542 -r5c430c3ac17fc8ad836fd70b8a3b8a12af44319e --- firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision 6499ea25921fcf67826fa0c35bb03caf411ba542) +++ firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision 5c430c3ac17fc8ad836fd70b8a3b8a12af44319e) @@ -21,13 +21,15 @@ #include "mibspi.h" #include "DrainPump.h" +#include "FlowSensors.h" #include "FPGA.h" #include "MessageSupport.h" #include "NVDataMgmt.h" #include "OperationModes.h" #include "PersistentAlarm.h" #include "PIControllers.h" #include "Pressures.h" +#include "ROPump.h" #include "SafetyShutdown.h" #include "SystemCommMessages.h" #include "TaskGeneral.h" @@ -48,7 +50,13 @@ #define DRAIN_PUMP_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) ///< Interval (ms/task time) at which the Drain Pump data is published on the CAN bus. -#define DRP_CONTROL_INTERVAL ( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) ///< Interval (ms/task time) at which the Drain pump is controlled. +#define DRP_CONTROL_INTERVAL ( 200 / TASK_GENERAL_INTERVAL ) ///< Interval (ms/task time) at which the Drain pump is controlled. +#define DRP_CONTROL_MAX_ALLOWED_PPO_PSI 30.0F ///< Drain pump maximum allowed PPo pressure in psi. +#define DRP_CONTROL_MAX_ALLOWED_PDR_PSI 40.0F ///< Drain pump maximum allowed PDr pressure in psi. +#define DRP_CONTROL_PSI_TO_DAC_P_ONLY_TERM 0.2F ///< Drain pump psi to DAC conversion. +#define DRP_CONTROL_FLOW_TO_DAC_P_ONLY_TERM 100.0F ///< Drain pump flow (L/min) to DAC conversion. +#define DRP_CONTROL_PRES_TO_MAX_THRESHOLD_PSI 1.0F ///< Drain pump pressure to maximum pressure threshold in psi. +#define DRP_CONTROL_MIN_DAC_CHANGE 1 ///< Drain pump minimum DAC change #define RPM_2_DAC_SLOPE 0.0547F ///< RPM to DAC conversion slope. #define RPM_2_DAC_INTERCEPT 2.9968F ///< RPM to DAC conversion intercept. @@ -58,12 +66,9 @@ #define TOGGLE_PERIOD_RESOLUTION_SECONDS 0.000005F ///< Toggle period to resolution in seconds. #define ROTATIONAL_TO_TOGGLE_PERIOD_CONVERSION 4.0F ///< Rotational to toggle period conversion coefficient. -#define DRAIN_PUMP_P_COEFFICIENT 0.5F ///< P term for drain pump delta pressure control. -#define DRAIN_PUMP_I_COEFFICIENT 1.0F ///< I term for drain pump delta pressure control. +#define DRAIN_PUMP_P_COEFFICIENT 20.0F ///< P term for drain pump delta pressure control. +#define DRAIN_PUMP_I_COEFFICIENT 80.0F ///< I term for drain pump delta pressure control. -#define MIN_ALLOWED_TARGET_OUTLET_PRESSURE -15.0F ///< Minimum allowed outlet pressure for closed loop control. -#define MAX_ALLOWED_TARGET_OUTLET_PRESSURE 15.0F ///< Maximum allowed outlet pressure for closed loop control. - #define MAX_ALLOWED_OPEN_LOOP_RPM_OUT_OF_RANGE 100 ///< Maximum allowed RPM out of range from target RPM in open loop. #define OPEN_LOOP_RPM_OUT_OF_RANGE_TIME_OUT ( 10 * MS_PER_SECOND ) ///< Open loop RPM out of range time out in ms. @@ -114,8 +119,8 @@ DRAIN_PUMP_DATA_PUB_INTERVAL, 0, 0 }; ///< Interval (in ms) at which to publish drain pump data to CAN bus. static U32 targetDrainPumpRPM; ///< Target drain pump RPM. -static F32 targetDrainPumpOutletPressure; ///< Target outlet pressure for the drain pump. -static U32 drainControlTimerCounter; ///< Determines when to perform control on drain pump. +static F32 targetDrainPumpOutletFlowLPM; ///< Target outlet pressure for the drain pump. +static U32 drainControlTimerCtr; ///< Determines when to perform control on drain pump. static BOOL hasClosedLoopBeenRequested; ///< Closed loop pump control flag. static OVERRIDE_U32_T drainPumpMeasuredRPM = { 0, 0, 0, 0 }; ///< Measured drain pump RPM from feedback. static OVERRIDE_F32_T drainPumpMeasuredCurrentA = { 0.0F, 0.0F, 0.0F, 0 }; ///< Measured drain pump current feedback. @@ -126,6 +131,7 @@ static U32 pendingDrainPumpCmdCountDown; ///< Delayed (pending) drain pump command count down timer (in task intervals). static DG_DRAIN_LINE_VOLUME_T drainLineVolumeRecord; ///< Drain line volume record. static DG_DRAIN_PUMP_CAL_RECORD_T drainPumpCalRecord; ///< Drain pump calibration record. +static BOOL isDrainPumpControlInFlowMode; ///< Drain pump flag to indicate whether the drain pump in flow control or not. /// ADC to RPM conversion coefficient or RPM to ADC conversion. static const F32 RPM_CONVERSION_COEFF = (F32) SEC_PER_MIN / ( 2.0F * TOGGLE_PERIOD_RESOLUTION_SECONDS * ROTATIONAL_TO_TOGGLE_PERIOD_CONVERSION ); @@ -147,10 +153,10 @@ * @details Inputs: none * @details Outputs: drainPumpState, drainPumpDAC, drainPumpDACSet, * drainPumpControlMode, drainPumpControlModeSet, targetDrainPumpRPM, - * targetDrainPumpOutletPressure, drainControlTimerCounter, + * targetDrainPumpOutletFlowLPM, drainControlTimerCtr, * pendingDrainPumpCmd, pendingDrainPumpCmdTarget, pendingDrainPumpCmdCountDown, * hasClosedLoopBeenRequested, signalNewRPMRequest, - * drainPumpDataPublicationTimerCounter, + * drainPumpDataPublicationTimerCounter, isDrainPumpControlInFlowMode * @return none *************************************************************************/ void initDrainPump( void ) @@ -163,14 +169,15 @@ drainPumpControlMode = NUM_OF_PUMP_CONTROL_MODES; drainPumpControlModeSet = PUMP_CONTROL_MODE_CLOSED_LOOP; targetDrainPumpRPM = 0; - targetDrainPumpOutletPressure = 0.0F; - drainControlTimerCounter = 0; + targetDrainPumpOutletFlowLPM = 0.0F; + drainControlTimerCtr = 0; pendingDrainPumpCmd = DRAIN_PUMP_OFF_STATE; pendingDrainPumpCmdTarget = 0.0F; pendingDrainPumpCmdCountDown = 0; hasClosedLoopBeenRequested = FALSE; signalNewRPMRequest = FALSE; drainPumpDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; + isDrainPumpControlInFlowMode = TRUE; // Initialize the drain pump PI controller initializePIController( PI_CONTROLLER_ID_DRAIN_PUMP, DRAIN_PUMP_MIN_DAC, DRAIN_PUMP_P_COEFFICIENT, DRAIN_PUMP_I_COEFFICIENT, @@ -252,76 +259,47 @@ /*********************************************************************//** * @brief - * The setDrainPumpTargetOutletPressure function sets the drain pump to start - * with given target PDr pressure. - * @details Inputs: targetDrainPumpDeltaPressure, hasClosedLoopBeenRequested, - * drainPumpDAC, drainPumpControlMode, drainPumpControlModeSet - * @details Outputs: targetDrainPumpDeltaPressure, hasClosedLoopBeenRequested, - * drainPumpDAC, drainPumpControlMode, drainPumpControlModeSet - * @param pressure new target drain pump outlet pressure + * The setDrainPumpTargetOutletFlowLPM function sets the drain pump target flow + * rate in L/min. + * @details Inputs: none + * @details Outputs: targetDrainPumpOutletFlowFMP, hasClosedLoopBeenRequested, + * drainPumpDAC, drainPumpControlMode, drainPumpControlModeSet, + * isDrainPumpControlInFlowMode + * @param flow new target flow in L/min * @return TRUE if new target speed is set, FALSE if not *************************************************************************/ -BOOL setDrainPumpTargetOutletPressure( F32 pressure ) +BOOL setDrainPumpTargetOutletFlowLPM( F32 flow ) { BOOL result = FALSE; // Check the delta pressure is in range - if ( ( pressure >= MIN_ALLOWED_TARGET_OUTLET_PRESSURE ) && ( pressure <= MAX_ALLOWED_TARGET_OUTLET_PRESSURE ) ) + if ( ( flow >= MIN_RO_FLOWRATE_LPM ) && ( flow <= MAX_RO_FLOWRATE_LPM ) ) { // Set all the variables for closed loop mode - targetDrainPumpOutletPressure = pressure; - hasClosedLoopBeenRequested = TRUE; - drainPumpDAC = DRAIN_PUMP_MIN_DAC; - drainPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; - drainPumpControlModeSet = drainPumpControlMode; - result = TRUE; + targetDrainPumpOutletFlowLPM = flow; + hasClosedLoopBeenRequested = TRUE; + drainPumpDAC = DRAIN_PUMP_MIN_DAC; + drainPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; + drainPumpControlModeSet = drainPumpControlMode; + result = TRUE; + isDrainPumpControlInFlowMode = TRUE; } else { - SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_DRAIN_PUMP_INVALID_DELTA_PRESSURE_SELECTED, pressure ) + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_DRAIN_PUMP_TARGET_FLOW_LPM_SELECTED, flow ) } return result; } /*********************************************************************//** * @brief - * The setDrainPumpTargetOutletPressureDelayed function sets the drain pump - * to start with given target PDr pressure after given delay. - * @details Inputs: targetDrainPumpDeltaPressure, hasClosedLoopBeenRequested, - * drainPumpDAC, drainPumpControlMode, drainPumpControlModeSet - * @details Outputs: targetDrainPumpDeltaPressure, hasClosedLoopBeenRequested, - * drainPumpDAC, drainPumpControlMode, drainPumpControlModeSet - * @param pressure new target drain pump outlet pressure - * @param delayMS delay duration (in ms) before drain pump started - * @return TRUE if new target speed is set, FALSE if not - *************************************************************************/ -BOOL setDrainPumpTargetOutletPressureDelayed( F32 pressure, U32 delayMs ) -{ - BOOL result = FALSE; - - // Check the delta pressure is in range - if ( ( pressure >= MIN_ALLOWED_TARGET_OUTLET_PRESSURE ) && ( pressure <= MAX_ALLOWED_TARGET_OUTLET_PRESSURE ) ) - { - pendingDrainPumpCmd = DRAIN_PUMP_CONTROL_TO_TARGET_STATE; - pendingDrainPumpCmdTarget = pressure; - pendingDrainPumpCmdCountDown = delayMs / TASK_GENERAL_INTERVAL; - result = TRUE; - } - else - { - SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_DRAIN_PUMP_INVALID_DELTA_PRESSURE_SELECTED, pressure ) - } - - return result; -} - -/*********************************************************************//** - * @brief * The signalDrainPumpHardStop function stops the Drain pump immediately. * @details Inputs: none * @details Outputs: targetDrainPumpRPM, drainPumpState, drainPumpControlMode, - * hasClosedLoopBeenRequested, drainControlTimerCounter, drainPumpControlModeSet + * hasClosedLoopBeenRequested, drainControlTimerCounter, drainPumpControlModeSet, + * drainControlTimerCtr, pendingDrainPumpCmd, pendingDrainPumpCmdTarget, + * pendingDrainPumpCmdCountDown * @return none *************************************************************************/ void signalDrainPumpHardStop( void ) @@ -334,9 +312,9 @@ hasClosedLoopBeenRequested = FALSE; drainPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; drainPumpControlModeSet = drainPumpControlMode; - drainControlTimerCounter = 0; + drainControlTimerCtr = 0; pendingDrainPumpCmd = DRAIN_PUMP_OFF_STATE; - pendingDrainPumpCmdTarget = 0.0; + pendingDrainPumpCmdTarget = 0.0F; pendingDrainPumpCmdCountDown = 0; } @@ -458,8 +436,11 @@ /*********************************************************************//** * @brief * The execDrainPumpController function executes the drain pump controller. - * @details Inputs: drainPumpState - * @details Outputs: drainPumpState + * @details Inputs: drainPumpState, pendingDrainPumpCmdCountDown, + * pendingDrainPumpCmd + * @details Outputs: drainPumpState, pendingDrainPumpCmdCountDown, + * pendingDrainPumpCmd, targetDrainPumpOutletFlowLPM, drainPumpDAC, + * hasClosedLoopBeenRequested, drainPumpControlMode, drainPumpControlModeSet * @return none *************************************************************************/ void execDrainPumpController( void ) @@ -480,11 +461,11 @@ { if ( DRAIN_PUMP_CONTROL_TO_TARGET_STATE == pendingDrainPumpCmd ) { - targetDrainPumpOutletPressure = pendingDrainPumpCmdTarget; - hasClosedLoopBeenRequested = TRUE; - drainPumpDAC = DRAIN_PUMP_MIN_DAC; - drainPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; - drainPumpControlModeSet = drainPumpControlMode; + targetDrainPumpOutletFlowLPM = pendingDrainPumpCmdTarget; + hasClosedLoopBeenRequested = TRUE; + drainPumpDAC = DRAIN_PUMP_MIN_DAC; + drainPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; + drainPumpControlModeSet = drainPumpControlMode; } else if ( DRAIN_PUMP_OPEN_LOOP_STATE == pendingDrainPumpCmd ) { @@ -583,15 +564,15 @@ /*********************************************************************//** * @brief - * The getTargetDrainPumpOutletPressure function gets the current target - * drain pump delta pressure. + * The targetDrainPumpOutletFlowLPM function gets the current target + * drain pump flow in L/min. * @details Inputs: targetDrainPumpOutletPressure * @details Outputs: none - * @return the current target drain pump outlet pressure. + * @return the current target drain pump flow in L/min *************************************************************************/ -F32 getDrainPumpTargetOutletPressure( void ) +F32 getDrainPumpTargetOutletFlowLPM( void ) { - return targetDrainPumpOutletPressure; + return targetDrainPumpOutletFlowLPM; } /*********************************************************************//** @@ -667,25 +648,74 @@ * @brief * The handleDrainPumpControlToTargetState function handles the control to * target state of the drain pump controller state machine. - * @details Inputs: drainControlTimerCounter, drainPumpDACSet - * @details Outputs: drainControlTimerCounter, drainPumpDACSet + * @details Inputs: drainControlTimerCtr + * @details Outputs: drainControlTimerCtr, drainPumpDACSet * @return next state of the controller state machine *************************************************************************/ static DRAIN_PUMP_STATE_T handleDrainPumpControlToTargetState( void ) { DRAIN_PUMP_STATE_T state = DRAIN_PUMP_CONTROL_TO_TARGET_STATE; // control at set interval - if ( ++drainControlTimerCounter >= DRP_CONTROL_INTERVAL ) + if ( ++drainControlTimerCtr >= DRP_CONTROL_INTERVAL ) { - F32 outletDrainPressure = getMeasuredDGPressure( PRESSURE_SENSOR_DRAIN_PUMP_OUTLET ); - F32 dac = runPIController( PI_CONTROLLER_ID_DRAIN_PUMP, getDrainPumpTargetOutletPressure(), outletDrainPressure ); + U32 tempDACSet; + F32 drainPumpOutletPressurePSI = getMeasuredDGPressure( PRESSURE_SENSOR_DRAIN_PUMP_OUTLET ); + F32 roPumpOutletPressurePSI = getMeasuredDGPressure( PRESSURE_SENSOR_RO_PUMP_OUTLET ); + F32 targetROFlowLPM = getDrainPumpTargetOutletFlowLPM(); + F32 msrdROFlowLPM = getMeasuredFlowRateLPM( RO_FLOW_SENSOR ); + F32 flowSubLPM = msrdROFlowLPM - targetROFlowLPM; - // The PI controller sends the DAC out and it is rounded to the nearest offset and is fed to the FPGA - drainPumpDACSet = (U32)( dac + FLOAT_TO_INT_ROUNDUP_OFFSET ); - setFPGADrainPumpSpeed( drainPumpDACSet ); + if ( TRUE == isDrainPumpControlInFlowMode ) + { + F32 dac = runPIController( PI_CONTROLLER_ID_DRAIN_PUMP, targetROFlowLPM, msrdROFlowLPM ); + drainPumpDACSet = (U32)( dac + FLOAT_TO_INT_ROUNDUP_OFFSET ); - drainControlTimerCounter = 0; + if ( ( drainPumpOutletPressurePSI > DRP_CONTROL_MAX_ALLOWED_PDR_PSI ) || ( roPumpOutletPressurePSI > DRP_CONTROL_MAX_ALLOWED_PPO_PSI ) ) + { + // If either of the pressure sensors are above range, transition to control to pressure + isDrainPumpControlInFlowMode = FALSE; + } + } + else if ( flowSubLPM > NEARLY_ZERO ) + { + // If measured flow if greater than the target flow, adjust the DAC to lower the flow rate + tempDACSet = (U32)( ( flowSubLPM * DRP_CONTROL_FLOW_TO_DAC_P_ONLY_TERM ) + FLOAT_TO_INT_ROUNDUP_OFFSET ); + drainPumpDACSet -= ( tempDACSet > 0.0F ? tempDACSet : DRP_CONTROL_MIN_DAC_CHANGE ); + } + else + { + // Get the status of the current pressure readings with respect to their maximum pressures + // Get the maximum of the pressure difference of PPo and PDr + F32 pdrSubPSI = drainPumpOutletPressurePSI - DRP_CONTROL_MAX_ALLOWED_PDR_PSI; + F32 ppoSubPSI = roPumpOutletPressurePSI - DRP_CONTROL_MAX_ALLOWED_PPO_PSI; + F32 maxPresPSI = MAX( pdrSubPSI, ppoSubPSI ); + + if ( ( pdrSubPSI > NEARLY_ZERO ) || ( ppoSubPSI > NEARLY_ZERO ) ) + { + // If either of the pressure values are greater than the maximum pressure, recalculate the DAC by the maximum pressure difference + // If the calculated DAC value turned out to be 0, set it to 1 so it changes at least by 1 DAC + tempDACSet = (U32)( ( maxPresPSI * DRP_CONTROL_PSI_TO_DAC_P_ONLY_TERM ) + FLOAT_TO_INT_ROUNDUP_OFFSET ); + drainPumpDACSet -= ( tempDACSet > 0.0F ? tempDACSet : DRP_CONTROL_MIN_DAC_CHANGE ); + } + else if ( ( fabs(pdrSubPSI) > DRP_CONTROL_PRES_TO_MAX_THRESHOLD_PSI ) && ( fabs(ppoSubPSI) > DRP_CONTROL_PRES_TO_MAX_THRESHOLD_PSI ) ) + { + // If both of the DACs turned out to be less than their maximum pressure limits by more than the specified value, recalculate the + // DAC. Using fabs for the subtraction means both pressure subtractions are negative so they are below their limits. + // In the calculations use fabs for max pressure because it is negative so it will end of subtracting DAC but it should be adding + tempDACSet = (U32)( ( fabs(maxPresPSI) * DRP_CONTROL_PSI_TO_DAC_P_ONLY_TERM ) + FLOAT_TO_INT_ROUNDUP_OFFSET ); + drainPumpDACSet += ( tempDACSet > 0.0F ? tempDACSet : DRP_CONTROL_MIN_DAC_CHANGE ); + } + } + + // Check the DAC to make sure it is not out the min and max ranges of the drain pump + tempDACSet = drainPumpDACSet; + tempDACSet = MIN( tempDACSet, DRAIN_PUMP_MAX_DAC ); + tempDACSet = MAX( tempDACSet, DRAIN_PUMP_MIN_DAC ); + drainPumpDACSet = tempDACSet; + + drainControlTimerCtr = 0; + setFPGADrainPumpSpeed( drainPumpDACSet ); } return state; @@ -750,7 +780,7 @@ drainPumpData.pumpDACSet = drainPumpDACSet; drainPumpData.drainPumpState = (U32)drainPumpState; drainPumpData.drainPumpRPM = getDrainPumpMeasuredRPM(); - drainPumpData.trgtOutletPrsr = targetDrainPumpOutletPressure; + drainPumpData.trgtOutletFlowLPM = targetDrainPumpOutletFlowLPM; drainPumpData.drainPumpCurrentA = getDrainPumpMeasuredCurrentA(); drainPumpData.drainPumpDirection = getDrainPumpMeasuredDirection(); @@ -815,7 +845,7 @@ drainPumpDataPublishInterval.ovData = intvl; drainPumpDataPublishInterval.override = OVERRIDE_KEY; - result = TRUE; + result = TRUE; } return result; @@ -837,7 +867,7 @@ { drainPumpDataPublishInterval.override = OVERRIDE_RESET; drainPumpDataPublishInterval.ovData = drainPumpDataPublishInterval.ovInitData; - result = TRUE; + result = TRUE; } return result; @@ -874,23 +904,23 @@ /*********************************************************************//** * @brief - * The testSetTargetDrainPumpDeltaPressureOverride function overrides - * the target drain pump delta pressure. + * The testSetTargetDrainPumpOutletFlowLPM function overrides + * the target drain pump outlet flow in L/min/ * @details Inputs: none * @details Outputs: none - * @param value override target drain pump delta pressure + * @param value of flow in L/min * @return TRUE if override successful, FALSE if not *************************************************************************/ -BOOL testSetTargetDrainPumpOutletPressure( F32 value ) +BOOL testSetTargetDrainPumpOutletFlowLPM( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { // Check if delta pressure is in range - if ( ( value >= MIN_ALLOWED_TARGET_OUTLET_PRESSURE ) && ( value <= MAX_ALLOWED_TARGET_OUTLET_PRESSURE ) ) + if ( ( value >= MIN_RO_FLOWRATE_LPM ) && ( value <= MAX_RO_FLOWRATE_LPM ) ) { - result = setDrainPumpTargetOutletPressure( value ); + result = setDrainPumpTargetOutletFlowLPM( value ); } }