Index: firmware/App/Controllers/DrainPump.c =================================================================== diff -u -r93dc3570f6a7581dd82f7f76e256784be29d5530 -rfba89d67dd2bef913e85a13563e2aa49f0e2e2f5 --- firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision 93dc3570f6a7581dd82f7f76e256784be29d5530) +++ firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision fba89d67dd2bef913e85a13563e2aa49f0e2e2f5) @@ -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,60 +60,62 @@ #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 { - DRAIN_PUMP_OFF_STATE = 0, ///< Drain pump off state - DRAIN_PUMP_CONTROL_TO_TARGET_STATE, ///< Drain pump control to target state - DRAIN_PUMP_OPEN_LOOP_STATE, ///< Drain pump open loop state - NUM_OF_DRAIN_PUMP_STATES ///< Number of drain pump states + DRAIN_PUMP_OFF_STATE = 0, ///< Drain pump off state + DRAIN_PUMP_CONTROL_TO_TARGET_STATE, ///< Drain pump control to target state + DRAIN_PUMP_OPEN_LOOP_STATE, ///< Drain pump open loop state + NUM_OF_DRAIN_PUMP_STATES ///< Number of drain pump states } DRAIN_PUMP_STATE_T; /// Enumeration of drain pump self-test states. typedef enum DrainPump_Self_Test_States { - DRAIN_PUMP_SELF_TEST_STATE_START = 0, ///< Drain pump self-test start state - DRAIN_PUMP_TEST_STATE_IN_PROGRESS, ///< Drain pump self-test in progress state - DRAIN_PUMP_TEST_STATE_COMPLETE, ///< Drain pump self-test completed state - NUM_OF_DRAIN_PUMP_SELF_TEST_STATES ///< Number of drain pump self-test states + DRAIN_PUMP_SELF_TEST_STATE_START = 0, ///< Drain pump self-test start state + DRAIN_PUMP_TEST_STATE_IN_PROGRESS, ///< Drain pump self-test in progress state + DRAIN_PUMP_TEST_STATE_COMPLETE, ///< Drain pump self-test completed state + 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 -static const F32 CONVERSION_COEFF = SEC_PER_MIN / ( TOGGLE_PERIOD_RESOLUTION_SECONDS * ROTATIONAL_TO_TOGGLE_PERIOD_CONVERSION ); +/// ADC to RPM conversion coefficient or RPM to ADC conversion. +static const F32 CONVERSION_COEFF = SEC_PER_MIN / ( 2 * TOGGLE_PERIOD_RESOLUTION_SECONDS * ROTATIONAL_TO_TOGGLE_PERIOD_CONVERSION ); // ********** private function prototypes ********** @@ -142,8 +145,10 @@ DRAIN_PUMP_MIN_DAC, DRAIN_PUMP_MAX_DAC ); // 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 ); + initPersistentAlarm( ALARM_ID_DRAIN_PUMP_RPM_OUT_OF_RANGE, 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( ALARM_ID_DRAIN_PUMP_OFF_FAULT, SAFETY_SHUTDOWN_TIMEOUT, SAFETY_SHUTDOWN_TIMEOUT ); } /*********************************************************************//** @@ -155,7 +160,7 @@ * @details Outputs: drainPumpDAC, targetDrainPumpSpeed, drainPumpControlMode, * drainPumpControlModeSet * @param rpm new drain pump target RPM - * @return: TRUE if new target RPM is set, FALSE if not + * @return TRUE if new target RPM is set, FALSE if not *************************************************************************/ BOOL setDrainPumpTargetRPM( U32 rpm ) { @@ -180,33 +185,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 - * @return: TRUE if new target speed is set, FALSE if not + * @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; @@ -247,18 +252,41 @@ // Convert speed ADC to RPM currentDrainPumpRPM = CONVERSION_COEFF / getFPGADrainPumpSpeed(); + /* TODO: The RPM is not converted properly. There will be a story to work on this issue. + * This part of code is commented out until the RPM is calculated from ADC correctly. + * There will be a story to address the RPM conversion. + */ +#ifndef IGNORE_DRAIN_PUMP_MONITOR // The RPM is only checked in open loop state that the pump is run at a fixed RPM. // The persistent alarm waits for a couple of seconds before raising an alarm, this is supposed to cover // 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( ALARM_ID_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( ALARM_ID_DRAIN_PUMP_OFF_FAULT, 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(); + BOOL test = FALSE; + } + } +#endif + // Publish drain pump data on interval publishDrainPumpData(); } @@ -268,7 +296,7 @@ * The execDrainPumpController function executes the drain pump controller. * @details Inputs: drainPumpState * @details Outputs: drainPumpState - * @return: none + * @return none *************************************************************************/ void execDrainPumpController( void ) { @@ -298,9 +326,9 @@ * @brief * The execDrainPumpSelfTest function executes the state machine for the drain * pump self-test. - * @detailsInputs: TODO FILL UP + * @details Inputs: TODO FILL UP * @details Outputs: TODO FILL UP - * @return: the current state of the Drain Pump self test. + * @return the current state of the Drain Pump self test. *************************************************************************/ SELF_TEST_STATUS_T execDrainPumpSelfTest( void ) { @@ -317,7 +345,7 @@ * RPM. * @details Inputs: targetDrainPumpRPM * @details Outputs: none - * @return: the current target drain pump RPM. + * @return the current target drain pump RPM. *************************************************************************/ U32 getTargetDrainPumpRPM( void ) { @@ -326,15 +354,15 @@ /*********************************************************************//** * @brief - * The getTargetDrainPumpDeltaP function gets the current target drain pump - * delta pressure. - * @details Inputs: targetDrainPumpDeltaPressure + * The getTargetDrainPumpOutletPressure function gets the current target + * drain pump delta pressure. + * @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 getTargetDrainPumpOutletPressure( void ) { - return targetDrainPumpDeltaPressure; + return targetDrainPumpOutletPressure; } /*********************************************************************//** @@ -344,7 +372,7 @@ * @details Inputs: drainPumpControlModeSet, drainPumpDACSet, drainPumpDAC * hasClosedLoopBeenRequested * @details Outputs: drainPumpDACSet - * @return: next state for the controller state machine + * @return next state for the controller state machine *************************************************************************/ static DRAIN_PUMP_STATE_T handleDrainPumpOffState( void ) { @@ -388,7 +416,7 @@ * target state of the drain pump controller state machine. * @details Inputs: drainControlTimerCounter, drainPumpDACSet * @details Outputs: drainControlTimerCounter, drainPumpDACSet - * @return: next state of the controller state machine + * @return next state of the controller state machine *************************************************************************/ static DRAIN_PUMP_STATE_T handleDrainPumpControlToTargetState( void ) { @@ -397,10 +425,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 outletDrainPressure = getMeasuredDGPressure( PRESSURE_SENSOR_DRAIN_PUMP_OUTLET ); + F32 dac = runPIController( PI_CONTROLLER_ID_DRAIN_PUMP, getTargetDrainPumpOutletPressure(), 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 ); @@ -421,7 +447,16 @@ *************************************************************************/ static DRAIN_PUMP_STATE_T handleDrainPumpOpenLoopState( void ) { - return DRAIN_PUMP_OPEN_LOOP_STATE; + DRAIN_PUMP_STATE_T state = DRAIN_PUMP_OPEN_LOOP_STATE; + + // Check if the RPM is 0, and if it is turn off the pump + if ( 0 == getTargetDrainPumpRPM() ) + { + state = DRAIN_PUMP_OFF_STATE; + signalDrainPumpHardStop(); + } + + return state; } /*********************************************************************//** @@ -446,7 +481,7 @@ * publication interval. * @details Inputs: drainPumpDataPublishInterval * @details Outputs: drainPumpDataPublishInterval - * @return: the current Drain pump data publication interval (in ms). + * @return the current Drain pump data publication interval (in ms). *************************************************************************/ static U32 getPublishDrainPumpDataInterval( void ) { @@ -466,7 +501,7 @@ * interval. * @details Inputs: drainPumpDataPublicationTimerCounter * @details Outputs: drainPumpDataPublicationTimerCounter - * @return: none + * @return none *************************************************************************/ static void publishDrainPumpData( void ) { @@ -499,7 +534,7 @@ * @details Inputs: drainPumpDataPublishInterval * @details Outputs: drainPumpDataPublishInterval * @param value override drain pump data publish interval with (in ms) - * @return: TRUE if override successful, FALSE if not + * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetDrainPumpDataPublishIntervalOverride( U32 value ) { @@ -546,16 +581,15 @@ * @details Inputs: none * @details Outputs: none * @param value override target drain pump RPM - * @return: TRUE if override successful, FALSE if not + * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetTargetDrainPumpRPM( U32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { - // Check for the RPM to be in range - if ( value >= MIN_DRAIN_PUMP_RPM && value <= MAX_DRAIN_PUMP_RPM ) + if ( ( 0 == value ) || ( value >= MIN_DRAIN_PUMP_RPM ) && ( value <= MAX_DRAIN_PUMP_RPM ) ) { result = setDrainPumpTargetRPM( value ); } @@ -573,16 +607,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 ); } }