Index: firmware/App/Controllers/DrainPump.c =================================================================== diff -u -r9cc5da6947aa143a10f95519eb7f366c1b095d61 -r4b9ae4e44d44ee6577fec4c8b75aa160fea04cc7 --- firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision 9cc5da6947aa143a10f95519eb7f366c1b095d61) +++ firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision 4b9ae4e44d44ee6577fec4c8b75aa160fea04cc7) @@ -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,12 @@ #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_TERM 1 ///< Drain pump psi 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. @@ -61,9 +68,6 @@ #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 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 +118,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 +130,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 +152,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 +168,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,72 +258,41 @@ /*********************************************************************//** * @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, @@ -334,7 +309,7 @@ hasClosedLoopBeenRequested = FALSE; drainPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; drainPumpControlModeSet = drainPumpControlMode; - drainControlTimerCounter = 0; + drainControlTimerCtr = 0; pendingDrainPumpCmd = DRAIN_PUMP_OFF_STATE; pendingDrainPumpCmdTarget = 0.0; pendingDrainPumpCmdCountDown = 0; @@ -480,7 +455,7 @@ { if ( DRAIN_PUMP_CONTROL_TO_TARGET_STATE == pendingDrainPumpCmd ) { - targetDrainPumpOutletPressure = pendingDrainPumpCmdTarget; + targetDrainPumpOutletFlowLPM = pendingDrainPumpCmdTarget; hasClosedLoopBeenRequested = TRUE; drainPumpDAC = DRAIN_PUMP_MIN_DAC; drainPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; @@ -583,15 +558,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 +642,68 @@ * @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 ); - // 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 ) + { + // If the flow control flag is on, Control to flow + F32 targetROFlowLPM = getDrainPumpTargetOutletFlowLPM(); + F32 msrdROFlowLPM = getMeasuredFlowRateLPM( RO_FLOW_SENSOR ); + 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 + { + // 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_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_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 +768,7 @@ drainPumpData.pumpDACSet = drainPumpDACSet; drainPumpData.drainPumpState = (U32)drainPumpState; drainPumpData.drainPumpRPM = getDrainPumpMeasuredRPM(); - drainPumpData.trgtOutletPrsr = targetDrainPumpOutletPressure; + drainPumpData.trgtOutletPrsr = targetDrainPumpOutletFlowLPM; drainPumpData.drainPumpCurrentA = getDrainPumpMeasuredCurrentA(); drainPumpData.drainPumpDirection = getDrainPumpMeasuredDirection(); @@ -815,7 +833,7 @@ drainPumpDataPublishInterval.ovData = intvl; drainPumpDataPublishInterval.override = OVERRIDE_KEY; - result = TRUE; + result = TRUE; } return result; @@ -837,7 +855,7 @@ { drainPumpDataPublishInterval.override = OVERRIDE_RESET; drainPumpDataPublishInterval.ovData = drainPumpDataPublishInterval.ovInitData; - result = TRUE; + result = TRUE; } return result; @@ -874,23 +892,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 ); } }