Index: firmware/App/Controllers/DrainPump.c =================================================================== diff -u -r93dc3570f6a7581dd82f7f76e256784be29d5530 -r1abaeb395cb7ffb2cb0ea9c1ba65c353d9c416f8 --- firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision 93dc3570f6a7581dd82f7f76e256784be29d5530) +++ firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision 1abaeb395cb7ffb2cb0ea9c1ba65c353d9c416f8) @@ -26,6 +26,7 @@ #include "PersistentAlarm.h" #include "PIControllers.h" #include "Pressures.h" +#include "SafetyShutdown.h" #include "SystemCommMessages.h" #include "TaskGeneral.h" #include "TaskPriority.h" @@ -59,12 +60,18 @@ #define DRAIN_PUMP_P_COEFFICIENT 0.5 ///< P term for drain pump delta pressure control. #define DRAIN_PUMP_I_COEFFICIENT 1.0 ///< I term for drain pump delta pressure control. -#define MIN_ALLOWED_TARGET_DELTA_PRESSURE -10.0 ///< Minimum allowed delta pressure for closed loop control. -#define MAX_ALLOWED_TARGET_DELTA_PRESSURE 10.0 ///< Maximum allowed delta pressure for closed loop control. +#define MIN_ALLOWED_TARGET_OUTLET_PRESSURE -10.0 ///< Minimum allowed outlet pressure for closed loop control. +#define MAX_ALLOWED_TARGET_OUTLET_PRESSURE 10.0 ///< Maximum allowed outlet pressure for closed loop control. -#define MAX_ALLOWED_OPEN_LOOP_RPM_OUT_OF_RANGE 20 ///< Maximum allowed RPM out of range from target RPM in open loop. +#define OPEN_LOOP_RPM_OUT_OF_RANGE 0.1 ///< Maximum allowed RPM out of range from target RPM in open loop percent. #define OPEN_LOOP_RPM_OUT_OF_RANGE_TIME_OUT ( 5 * MS_PER_SECOND ) ///< Open loop RPM out of range time out in ms. +#define DRAIN_PUMP_ENABLE_SPI3_PORT_MASK 0x00000020 ///< CS5 - Out put GPIO for pump enable. +#define SET_DRAIN_PUMP_ENABLE() {mibspiREG3->PC3 |= DRAIN_PUMP_ENABLE_SPI3_PORT_MASK;} ///< Drain pump enable set macro. +#define CLR_DRAIN_PUMP_ENABLE() {mibspiREG3->PC3 &= ~DRAIN_PUMP_ENABLE_SPI3_PORT_MASK;} ///< Drain pump enable clear macro. + +#define SAFETY_SHUTDOWN_TIMEOUT MS_PER_SECOND ///< Drain pump safety shutdown activation timeout. + /// Enumeration of drain pump states. typedef enum DrainPump_States { @@ -83,35 +90,31 @@ NUM_OF_DRAIN_PUMP_SELF_TEST_STATES ///< Number of drain pump self-test states } DRAIN_PUMP_SELF_TEST_STATE_T; -#define DRAIN_PUMP_ENABLE_SPI3_PORT_MASK 0x00000020 ///< CS5 - Out put GPIO for pump enable. -#define SET_DRAIN_PUMP_ENABLE() {mibspiREG3->PC3 |= DRAIN_PUMP_ENABLE_SPI3_PORT_MASK;} ///< drain pump enable set macro. -#define CLR_DRAIN_PUMP_ENABLE() {mibspiREG3->PC3 &= ~DRAIN_PUMP_ENABLE_SPI3_PORT_MASK;} ///< drain pump enable clear macro. - // ********** private data ********** -static DRAIN_PUMP_STATE_T drainPumpState = DRAIN_PUMP_OFF_STATE; ///< current state of drain pump controller state machine -static U32 drainPumpDataPublicationTimerCounter = 0; ///< used to schedule drain pump data publication to CAN bus -static U32 drainPumpDAC = 0; ///< initial drain pump DAC value -static U32 drainPumpDACSet = 0; ///< currently set drain pump DAC value -static PUMP_CONTROL_MODE_T drainPumpControlMode = NUM_OF_PUMP_CONTROL_MODES; ///< requested drain pump control mode -static PUMP_CONTROL_MODE_T drainPumpControlModeSet = PUMP_CONTROL_MODE_CLOSED_LOOP; ///< currently set drain pump control mode +static DRAIN_PUMP_STATE_T drainPumpState = DRAIN_PUMP_OFF_STATE; ///< Current state of drain pump controller state machine. +static U32 drainPumpDataPublicationTimerCounter = 0; ///< Used to schedule drain pump data publication to CAN bus. +static U32 drainPumpDAC = 0; ///< Initial drain pump DAC value. +static U32 drainPumpDACSet = 0; ///< Currently set drain pump DAC value. +static PUMP_CONTROL_MODE_T drainPumpControlMode = NUM_OF_PUMP_CONTROL_MODES; ///< Requested drain pump control mode. +static PUMP_CONTROL_MODE_T drainPumpControlModeSet = PUMP_CONTROL_MODE_CLOSED_LOOP; ///< Currently set drain pump control mode. static OVERRIDE_U32_T drainPumpDataPublishInterval = { DRAIN_PUMP_DATA_PUB_INTERVAL, DRAIN_PUMP_DATA_PUB_INTERVAL, - 0, 0 }; ///< interval (in ms) at which to publish RO flow data to CAN bus -static U32 targetDrainPumpRPM = 0; ///< Target drain pump RPM -static F32 targetDrainPumpDeltaPressure = 0.0; ///< Target delta pressure for the drain pump + 0, 0 }; ///< Interval (in ms) at which to publish RO flow data to CAN bus. +static U32 targetDrainPumpRPM = 0; ///< Target drain pump RPM. +static F32 targetDrainPumpOutletPressure = 0.0; ///< Target outlet pressure for the drain pump. -static U32 drainControlTimerCounter = 0; ///< Determines when to perform control on drain pump -static BOOL hasClosedLoopBeenRequested = FALSE; ///< Closed loop pump control flag -static U32 currentDrainPumpRPM = 0; ///< Current drain pump RPM from feedback +static U32 drainControlTimerCounter = 0; ///< Determines when to perform control on drain pump. +static BOOL hasClosedLoopBeenRequested = FALSE; ///< Closed loop pump control flag. +static U32 currentDrainPumpRPM = 0; ///< Current drain pump RPM from feedback. /* TODO These variables are used for POST. POST will be implemented later -static DRAIN_PUMP_SELF_TEST_STATE_T drainPumpSelfTestState = DRAIN_PUMP_SELF_TEST_STATE_START; ///< current drain pump self test state -static U32 drainPumpSelfTestTimerCount = 0; ///< timer counter for drain pump self test +static DRAIN_PUMP_SELF_TEST_STATE_T drainPumpSelfTestState = DRAIN_PUMP_SELF_TEST_STATE_START; ///< Current drain pump self test state. +static U32 drainPumpSelfTestTimerCount = 0; ///< Timer counter for drain pump self test. */ -/// ADC to RPM conversion coefficient or RPM to ADC conversion +/// ADC to RPM conversion coefficient or RPM to ADC conversion. static const F32 CONVERSION_COEFF = SEC_PER_MIN / ( TOGGLE_PERIOD_RESOLUTION_SECONDS * ROTATIONAL_TO_TOGGLE_PERIOD_CONVERSION ); // ********** private function prototypes ********** @@ -134,7 +137,7 @@ { stopDrainPump(); - hasClosedLoopBeenRequested = FALSE; + hasClosedLoopBeenRequested = FALSE; // Initialize the drain pump PI controller initializePIController( PI_CONTROLLER_ID_DRAIN_PUMP, DRAIN_PUMP_MIN_DAC, @@ -144,6 +147,10 @@ // Initialize the persistent alarm for open loop RPM out of range initPersistentAlarm( PERSISTENT_ALARM_DRAIN_PUMP_RPM_OUT_OF_RANGE, ALARM_ID_DRAIN_PUMP_RPM_OUT_OF_RANGE, TRUE, OPEN_LOOP_RPM_OUT_OF_RANGE_TIME_OUT, OPEN_LOOP_RPM_OUT_OF_RANGE_TIME_OUT ); + + // Initialize the persistent alarm for RPM not to be less than the min RPM when the pump is off + initPersistentAlarm( PERSISTENT_ALARM_DRAIN_PUMP_OFF_ERROR, ALARM_ID_DRAIN_PUMP_OFF_FAULT, TRUE, + SAFETY_SHUTDOWN_TIMEOUT, SAFETY_SHUTDOWN_TIMEOUT ); } /*********************************************************************//** @@ -180,33 +187,33 @@ /*********************************************************************//** * @brief - * The setDrainPumpTargetDeltaPressure function sets the target delta - * pressure in between the PRd and PDr sensors. + * The setDrainPumpTargetOutletPressure function sets the target drain pump + * outlet pressure. * @details Inputs: targetDrainPumpDeltaPressure, hasClosedLoopBeenRequested, * drainPumpDAC, drainPumpControlMode, drainPumpControlModeSet * @details Outputs: targetDrainPumpDeltaPressure, hasClosedLoopBeenRequested, * drainPumpDAC, drainPumpControlMode, drainPumpControlModeSet - * @param deltaP new target drain pump delta pressure + * @param pressure new target drain pump outlet pressure * @return: TRUE if new target speed is set, FALSE if not *************************************************************************/ -BOOL setDrainPumpTargetDeltaPressure( F32 deltaP ) +BOOL setDrainPumpTargetOutletPressure( F32 pressure ) { BOOL result = FALSE; // Check the delta pressure is in range - if ( ( deltaP >= MIN_ALLOWED_TARGET_DELTA_PRESSURE ) && ( deltaP <= MAX_ALLOWED_TARGET_DELTA_PRESSURE ) ) + if ( ( pressure >= MIN_ALLOWED_TARGET_OUTLET_PRESSURE ) && ( pressure <= MAX_ALLOWED_TARGET_OUTLET_PRESSURE ) ) { // Set all the variables for closed loop mode - targetDrainPumpDeltaPressure = deltaP; - hasClosedLoopBeenRequested = TRUE; - drainPumpDAC = DRAIN_PUMP_MIN_DAC; - drainPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; - drainPumpControlModeSet = drainPumpControlMode; - result = TRUE; + targetDrainPumpOutletPressure = pressure; + hasClosedLoopBeenRequested = TRUE; + drainPumpDAC = DRAIN_PUMP_MIN_DAC; + drainPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; + drainPumpControlModeSet = drainPumpControlMode; + result = TRUE; } else { - SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_DRAIN_PUMP_INVALID_DELTA_PRESSURE_SELECTED, deltaP ) + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_DRAIN_PUMP_INVALID_DELTA_PRESSURE_SELECTED, pressure ) } return result; @@ -252,13 +259,29 @@ // when the pump is turned on and it takes a while to ramp up to target RPM. if( drainPumpControlModeSet == PUMP_CONTROL_MODE_OPEN_LOOP ) { - // Check if RPM out of range. Using fabs since the read RPM can be above or below the target. - BOOL isRPMOutOfRange = fabs( getTargetDrainPumpRPM() - currentDrainPumpRPM ) > MAX_ALLOWED_OPEN_LOOP_RPM_OUT_OF_RANGE; + U32 targetRPM = getTargetDrainPumpRPM(); + F32 threshold = OPEN_LOOP_RPM_OUT_OF_RANGE * targetRPM; - checkPersistentAlarm( PERSISTENT_ALARM_DRAIN_PUMP_RPM_OUT_OF_RANGE, isRPMOutOfRange, currentDrainPumpRPM, - MAX_ALLOWED_OPEN_LOOP_RPM_OUT_OF_RANGE ); + // Check if RPM is out of range. Using fabs since the read RPM can be above or below the target. + BOOL isRPMOutOfRange = fabs( targetRPM - currentDrainPumpRPM ) > threshold; + + checkPersistentAlarm( PERSISTENT_ALARM_DRAIN_PUMP_RPM_OUT_OF_RANGE, isRPMOutOfRange, currentDrainPumpRPM, threshold ); } + // Check if the pump is in off state and the RPM is greater than the minimum RPM + if ( drainPumpState == DRAIN_PUMP_OFF_STATE ) + { + BOOL isRPMTooHigh = currentDrainPumpRPM > MIN_DRAIN_PUMP_RPM; + + checkPersistentAlarm( PERSISTENT_ALARM_DRAIN_PUMP_OFF_ERROR, isRPMTooHigh, currentDrainPumpRPM, MIN_DRAIN_PUMP_RPM ); + + // If the off fault alarm has become active, trigger the safety shutdown + if ( isAlarmActive( ALARM_ID_DRAIN_PUMP_OFF_FAULT ) ) + { + activateSafetyShutdown(); + } + } + // Publish drain pump data on interval publishDrainPumpData(); } @@ -328,13 +351,13 @@ * @brief * The getTargetDrainPumpDeltaP function gets the current target drain pump * delta pressure. - * @details Inputs: targetDrainPumpDeltaPressure + * @details Inputs: targetDrainPumpOutletPressure * @details Outputs: none - * @return: the current target drain pump delta pressure. + * @return: the current target drain pump outlet pressure. *************************************************************************/ -F32 getTargetDrainPumpDeltaP( void ) +F32 getTargetDrainPumpOutletP( void ) { - return targetDrainPumpDeltaPressure; + return targetDrainPumpOutletPressure; } /*********************************************************************//** @@ -397,10 +420,8 @@ // control at set interval if ( ++drainControlTimerCounter >= DRP_CONTROL_INTERVAL ) { - F32 inletDrainPressure = getMeasuredDGPressure ( PRESSURE_SENSOR_DRAIN_PUMP_INLET ); F32 outletDrainPressure = getMeasuredDGPressure ( PRESSURE_SENSOR_DRAIN_PUMP_OUTLET ); - F32 pressureDiff = outletDrainPressure - inletDrainPressure; - F32 dac = runPIController( PI_CONTROLLER_ID_DRAIN_PUMP, getTargetDrainPumpDeltaP(), pressureDiff ); + F32 dac = runPIController( PI_CONTROLLER_ID_DRAIN_PUMP, getTargetDrainPumpOutletP(), outletDrainPressure ); // 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 ); @@ -573,16 +594,16 @@ * @param value override target drain pump delta pressure * @return TRUE if override successful, FALSE if not *************************************************************************/ -BOOL testSetTargetDrainPumpDeltaPressure( F32 value ) +BOOL testSetTargetDrainPumpOutletPressure( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { // Check if delta pressure is in range - if ( value >= MIN_ALLOWED_TARGET_DELTA_PRESSURE && value <= MAX_ALLOWED_TARGET_DELTA_PRESSURE ) + if ( value >= MIN_ALLOWED_TARGET_OUTLET_PRESSURE && value <= MAX_ALLOWED_TARGET_OUTLET_PRESSURE ) { - result = setDrainPumpTargetDeltaPressure( value ); + result = setDrainPumpTargetOutletPressure( value ); } }