Index: firmware/App/Controllers/DrainPump.c =================================================================== diff -u -rf308cc4c35eab630ebbbde405cfe47d049afeafb -rbbd5ac2589c8093f681f2284367975ddd220b553 --- firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision f308cc4c35eab630ebbbde405cfe47d049afeafb) +++ firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision bbd5ac2589c8093f681f2284367975ddd220b553) @@ -8,505 +8,605 @@ * @file DrainPump.c * * @author (last) Quang Nguyen -* @date (last) 22-Jul-2020 +* @date (last) 14-Sep-2020 * * @author (original) Sean * @date (original) 08-Apr-2020 * ***************************************************************************/ - -#include - -#include "etpwm.h" -#include "mibspi.h" - -#include "FPGA.h" -#include "OperationModes.h" -#include "PIControllers.h" -#include "Pressures.h" -#include "SystemCommMessages.h" -#include "TaskGeneral.h" -#include "TaskPriority.h" -#include "Timers.h" -#include "Valves.h" -#include "DrainPump.h" + +#include + +#include "etpwm.h" +#include "mibspi.h" + +#include "DrainPump.h" +#include "FPGA.h" +#include "OperationModes.h" +#include "PersistentAlarm.h" +#include "PIControllers.h" +#include "Pressures.h" +#include "SafetyShutdown.h" +#include "SystemCommMessages.h" +#include "TaskGeneral.h" +#include "TaskPriority.h" +#include "Timers.h" +#include "Valves.h" #ifdef EMC_TEST_BUILD #include "Heaters.h" #endif - -/** - * @addtogroup DrainPump - * @{ - */ - -// ********** private definitions ********** - -#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_SPEED_ADC_TO_RPM_FACTOR 12.94 ///< conversion factor from ADC counts to RPM for Drain pump -#define DRP_SPEED_RPM_TO_ADC_FACTOR ( 1.0 / DRP_SPEED_ADC_TO_RPM_FACTOR ) ///< conversion factor from RPM to ADC counts for Drain pump - -/// 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. - 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 tests in progress state. - DRAIN_PUMP_TEST_STATE_COMPLETE, ///< Drain pump self tests completed state. - NUM_OF_DRAIN_PUMP_SELF_TEST_STATES ///< Number of drain pump self test states. -} DRAIN_PUMP_SELF_TEST_STATE_T; - -// pin assignment for pump enable -#define DRAIN_PUMP_ENABLE_SPI3_PORT_MASK 0x00000020 // (CS5 - re-purposed as output GPIO) -// drain pump enable macros -#define SET_DRAIN_PUMP_ENABLE() {mibspiREG3->PC3 |= DRAIN_PUMP_ENABLE_SPI3_PORT_MASK;} -#define CLR_DRAIN_PUMP_ENABLE() {mibspiREG3->PC3 &= ~DRAIN_PUMP_ENABLE_SPI3_PORT_MASK;} - -// TODO - test code - remove later -#define DRAIN_PUMP_TEST2_SPI1_PORT_MASK 0x00000004 // (CS2 - re-purposed as input GPIO) -#define GET_DIP_SW2_TEST() ( ( mibspiREG1->PC2 & DRAIN_PUMP_TEST2_SPI1_PORT_MASK ) != 0 ) - -// ********** 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 BOOL isDrainPumpOn = FALSE; ///< Drain pump is currently running -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 = PUMP_CONTROL_MODE_CLOSED_LOOP; ///< 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 OVERRIDE_U32_T targetDrainPumpSpeed = { 0, 0, 0, 0 }; ///< Target RO pressure (in PSI). - -static U32 drainControlTimerCounter = 0; ///< determines when to perform control on drain pump - -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 - -// ********** private function prototypes ********** - -static DRAIN_PUMP_STATE_T handleDrainPumpOffState( void ); -static DRAIN_PUMP_STATE_T handleDrainPumpControlToTargetState( void ); -static void stopDrainPump( void ); -static void publishDrainPumpData( void ); -static DATA_GET_PROTOTYPE( U32, getPublishDrainPumpDataInterval ); - -/*********************************************************************//** - * @brief initDrainPump - * The initDrainPump function initializes the DrainPump module. - * @details - * Inputs : none - * Outputs : DrainPump module initialized. - * @return none - *************************************************************************/ -void initDrainPump( void ) -{ - stopDrainPump(); -} - -/*********************************************************************//** - * @brief - * The setDrainPumpTargetSpeed function sets a new target speed for the \n - * drain pump. - * @details - * Inputs : none - * Outputs : targetDrainPumpPressure - * @param rpm new target drain pump speed (in RPM) - * @return TRUE if new target speed is set, FALSE if not - *************************************************************************/ -BOOL setDrainPumpTargetSpeed( U32 rpm ) -{ - BOOL result = FALSE; - - if ( ( 0 == rpm ) || - ( ( rpm >= MIN_DRAIN_PUMP_RPM_TARGET ) && ( rpm <= MAX_DRAIN_PUMP_RPM_TARGET ) ) ) - { -#ifdef EMC_TEST_BUILD - drainPumpDAC = (U32)((F32)2500 * DRP_SPEED_RPM_TO_ADC_FACTOR + FLOAT_TO_INT_ROUNDUP_OFFSET); -#else + +/** + * @addtogroup DrainPump + * @{ + */ + +// ********** private definitions ********** + +#define DRAIN_PUMP_MIN_DAC ( ( (F32)MIN_DRAIN_PUMP_RPM * \ + DRP_SPEED_RPM_TO_ADC_FACTOR ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ///< Drain pump minimum RPM to DAC conversion. +#define DRAIN_PUMP_MAX_DAC ( ( (F32)MAX_DRAIN_PUMP_RPM * \ + DRP_SPEED_RPM_TO_ADC_FACTOR ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ///< Drain pump maximum RPM to DAC conversion. +#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_SPEED_ADC_TO_RPM_CONVERSION 12.94 ///< Conversion factor from ADC counts to RPM for Drain pump. +#define DRP_SPEED_RPM_TO_ADC_FACTOR ( 1.0 / DRP_SPEED_ADC_TO_RPM_CONVERSION ) ///< Conversion factor from RPM to ADC counts for Drain pump. + +#define TOGGLE_PERIOD_RESOLUTION_SECONDS 0.000005 ///< Toggle period to resolution in seconds. +#define ROTATIONAL_TO_TOGGLE_PERIOD_CONVERSION 4 ///< Rotational to toggle period conversion coefficient. + +#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_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 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_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_T; + +// ********** 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 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 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. + +/* 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. +*/ + +/// 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 ********** + +static DRAIN_PUMP_STATE_T handleDrainPumpOffState( void ); +static DRAIN_PUMP_STATE_T handleDrainPumpControlToTargetState( void ); +static DRAIN_PUMP_STATE_T handleDrainPumpOpenLoopState( void ); +static void stopDrainPump( void ); +static void publishDrainPumpData( void ); +static U32 getPublishDrainPumpDataInterval( void ); + +/*********************************************************************//** + * @brief + * The initDrainPump function initializes the DrainPump module. + * @details Inputs: none + * @details Outputs: hasClosedLoopBeenRequested + * @return none + *************************************************************************/ +void initDrainPump( void ) +{ + stopDrainPump(); + + hasClosedLoopBeenRequested = FALSE; + + // Initialize the drain pump PI controller + initializePIController( PI_CONTROLLER_ID_DRAIN_PUMP, DRAIN_PUMP_MIN_DAC, + DRAIN_PUMP_P_COEFFICIENT, DRAIN_PUMP_I_COEFFICIENT, + DRAIN_PUMP_MIN_DAC, DRAIN_PUMP_MAX_DAC ); + + // Initialize the persistent alarm for open loop RPM out of range + 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 ); +} + +/*********************************************************************//** + * @brief + * The setDrainPumpTargetSpeed function sets a new target RPM for the + * drain pump. + * @details Inputs: drainPumpDAC, targetDrainPumpSpeed, drainPumpControlMode, + * drainPumpControlModeSet + * @details Outputs: drainPumpDAC, targetDrainPumpSpeed, drainPumpControlMode, + * drainPumpControlModeSet + * @param rpm new drain pump target RPM + * @return TRUE if new target RPM is set, FALSE if not + *************************************************************************/ +BOOL setDrainPumpTargetRPM( U32 rpm ) +{ + BOOL result = FALSE; + + if ( ( 0 == rpm ) || ( ( rpm >= MIN_DRAIN_PUMP_RPM ) && ( rpm <= MAX_DRAIN_PUMP_RPM ) ) ) + { drainPumpDAC = (U32)((F32)rpm * DRP_SPEED_RPM_TO_ADC_FACTOR + FLOAT_TO_INT_ROUNDUP_OFFSET); -#endif - targetDrainPumpSpeed.data = rpm; - drainPumpControlMode = PUMP_CONTROL_MODE_OPEN_LOOP; - result = TRUE; - } - - return result; -} - -/*********************************************************************//** - * @brief - * The signalDrainPumpHardStop function stops the Drain pump immediately. - * @details - * Inputs : none - * Outputs : Drain pump stopped, set point reset, state changed to off - * @return none - *************************************************************************/ -void signalDrainPumpHardStop( void ) -{ - targetDrainPumpSpeed.data = 0; - stopDrainPump(); - drainPumpState = DRAIN_PUMP_OFF_STATE; - drainControlTimerCounter = 0; -} - -/*********************************************************************//** - * @brief - * The execDrainPumpMonitor function executes the Drain Pump monitor. - * @details - * Inputs : none - * Outputs : none - * @return none - *************************************************************************/ -void execDrainPumpMonitor( void ) -{ - U16 drnPumpSpd = getFPGADrainPumpSpeed(); - - // TODO - convert drain pump speed to RPM - - // TODO - check(s) ??? - - // publish drain pump data on interval - publishDrainPumpData(); -} - -/*********************************************************************//** - * @brief - * The execDrainPumpController function executes the Drain Pump controller. - * @details - * Inputs : drainPumpState - * Outputs : drainPumpState - * @return none - *************************************************************************/ -void execDrainPumpController( void ) -{ - switch ( drainPumpState ) - { - case DRAIN_PUMP_OFF_STATE: - drainPumpState = handleDrainPumpOffState(); - break; - - case DRAIN_PUMP_CONTROL_TO_TARGET_STATE: - drainPumpState = handleDrainPumpControlToTargetState(); - break; - - default: - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, 0, drainPumpState ) // TODO - replace 1st param with s/w fault enum - drainPumpState = DRAIN_PUMP_OFF_STATE; - break; - } -} - -/*********************************************************************//** - * @brief - * The handleDrainPumpOffState function handles the drain pump off state \n - * of the drain pump controller state machine. - * @details - * Inputs : targetDrainPumpSpeed - * Outputs : drainPumpPWMDutyCyclePctSet, isDrainPumpOn - * @return next state - *************************************************************************/ -static DRAIN_PUMP_STATE_T handleDrainPumpOffState( void ) -{ - DRAIN_PUMP_STATE_T result = DRAIN_PUMP_OFF_STATE; - -#ifdef DEBUG_ENABLED -#ifdef ENABLE_DIP_SWITCHES - // TODO - test code - remove later - if ( GET_DIP_SW2_TEST() ) - { - setValveState( VPI, VALVE_STATE_OPEN ); - setValveState( VRC, VALVE_STATE_DRAIN_C_TO_NO ); - setValveState( VPO, VALVE_STATE_NOFILL_C_TO_NO ); - setDrainPumpTargetSpeed( 1000 ); -#ifdef EMC_TEST_BUILD - setTrimmerHeaterTargetTemperature( 50.0 ); - startTrimmerHeater(); -#endif - } -#endif -#endif - - // if we've been given a pressure, transition to control to target state - if ( getTargetDrainPumpSpeed() > 0 ) - { - // set drain pump enable pin - SET_DRAIN_PUMP_ENABLE(); - // set drain pump control mode - drainPumpControlModeSet = drainPumpControlMode; - // set drain pump DAC - drainPumpDACSet = drainPumpDAC; - setFPGADrainPumpSpeed( drainPumpDACSet ); - // set pump to on - isDrainPumpOn = TRUE; - result = DRAIN_PUMP_CONTROL_TO_TARGET_STATE; - } - - return result; -} - -/*********************************************************************//** - * @brief - * The handleDrainPumpControlToTargetState function handles the "control to \n - * target" state of the drain pump controller state machine. - * @details - * Inputs : none - * Outputs : drainPumpState - * @return next state - *************************************************************************/ -static DRAIN_PUMP_STATE_T handleDrainPumpControlToTargetState( void ) -{ - DRAIN_PUMP_STATE_T result = DRAIN_PUMP_CONTROL_TO_TARGET_STATE; - - // control at set interval - if ( ++drainControlTimerCounter >= DRP_CONTROL_INTERVAL ) - { - if ( drainPumpControlModeSet == PUMP_CONTROL_MODE_CLOSED_LOOP ) - { -#ifndef EMC_TEST_BUILD - // TODO - will drain pump have a closed loop? -#endif - } - drainControlTimerCounter = 0; - } - - if ( 0 == getTargetDrainPumpSpeed() ) - { - signalDrainPumpHardStop(); - result = DRAIN_PUMP_OFF_STATE; - } - -#ifdef DEBUG_ENABLED -#ifdef ENABLE_DIP_SWITCHES - // TODO - test code - remove later - if ( !GET_DIP_SW2_TEST() ) - { - signalDrainPumpHardStop(); - setValveState( VPI, VALVE_STATE_OPEN ); - setValveState( VRC, VALVE_STATE_DRAIN_C_TO_NO ); - setValveState( VPO, VALVE_STATE_FILL_C_TO_NC ); -#ifdef EMC_TEST_BUILD - stopTrimmerHeater(); -#endif - result = DRAIN_PUMP_OFF_STATE; - } -#endif -#endif - - return result; -} - -/*********************************************************************//** - * @brief - * The stopDrainPump function sets the Drain pump DAC to zero. - * @details - * Inputs : none - * Outputs : isDrainPumpOn, DAC zeroed - * @return none - *************************************************************************/ -static void stopDrainPump( void ) -{ - isDrainPumpOn = FALSE; - drainPumpDAC = 0; - drainPumpDACSet = 0; - setFPGADrainPumpSpeed( drainPumpDACSet ); - CLR_DRAIN_PUMP_ENABLE(); -} - -/*********************************************************************//** - * @brief - * The getPublishDrainPumpDataInterval function gets the Drain pump data \n - * publication interval. - * @details - * Inputs : drainPumpDataPublishInterval - * Outputs : none - * @return the current Drain pump data publication interval (in ms). - *************************************************************************/ -U32 getPublishDrainPumpDataInterval( void ) -{ - U32 result = drainPumpDataPublishInterval.data; - - if ( OVERRIDE_KEY == drainPumpDataPublishInterval.override ) - { - result = drainPumpDataPublishInterval.ovData; - } - - return result; -} - -/*********************************************************************//** - * @brief - * The getTargetDrainPumpSpeed function gets the current target Drain pump \n - * speed. - * @details - * Inputs : targetDrainPumpSpeed - * Outputs : none - * @return the current target drain pump speed. - *************************************************************************/ -U32 getTargetDrainPumpSpeed( void ) -{ - U32 result = targetDrainPumpSpeed.data; - - if ( OVERRIDE_KEY == targetDrainPumpSpeed.override ) - { - result = targetDrainPumpSpeed.ovData; - } - - return result; -} - -/*********************************************************************//** - * @brief - * The publishDrainPumpData function publishes drain pump data at the set \n - * interval. - * @details - * Inputs : target speed - * Outputs : Drain pump data is published to CAN bus. - * @return none - *************************************************************************/ -static void publishDrainPumpData( void ) -{ - // publish Drain pump data on interval - if ( ++drainPumpDataPublicationTimerCounter >= getPublishDrainPumpDataInterval() ) - { - U32 spdStPt = getTargetDrainPumpSpeed(); - - broadcastDrainPumpData( spdStPt, drainPumpDACSet ); - drainPumpDataPublicationTimerCounter = 0; - } -} - -/*********************************************************************//** - * @brief - * The execDrainPumpTest function executes the state machine for the Drain \n - * Pump self test. - * @details - * Inputs : none - * Outputs : none - * @return the current state of the Drain Pump self test. - *************************************************************************/ -SELF_TEST_STATUS_T execDrainPumpTest( void ) -{ - SELF_TEST_STATUS_T result = SELF_TEST_STATUS_FAILED; - - // TODO - implement self test(s) - - return result; -} - - -/************************************************************************* - * TEST SUPPORT FUNCTIONS - *************************************************************************/ - - -/*********************************************************************//** - * @brief - * The testSetDrainPumpDataPublishIntervalOverride function overrides the \n - * drain pump data publish interval. - * @details - * Inputs : none - * Outputs : drainPumpDataPublishInterval - * @param value override RO pump data publish interval with (in ms) - * @return TRUE if override successful, FALSE if not - *************************************************************************/ -BOOL testSetDrainPumpDataPublishIntervalOverride( U32 value ) -{ - BOOL result = FALSE; - - if ( TRUE == isTestingActivated() ) - { - U32 intvl = value / TASK_PRIORITY_INTERVAL; - - result = TRUE; - drainPumpDataPublishInterval.ovData = intvl; - drainPumpDataPublishInterval.override = OVERRIDE_KEY; - } - - return result; -} - -/*********************************************************************//** - * @brief - * The testResetDrainPumpDataPublishIntervalOverride function resets the override \n - * of the drain pump data publish interval. - * @details - * Inputs : none - * Outputs : drainPumpDataPublishInterval - * @return TRUE if override reset successful, FALSE if not - *************************************************************************/ -BOOL testResetDrainPumpDataPublishIntervalOverride( void ) -{ - BOOL result = FALSE; - - if ( TRUE == isTestingActivated() ) - { - result = TRUE; - drainPumpDataPublishInterval.override = OVERRIDE_RESET; - drainPumpDataPublishInterval.ovData = drainPumpDataPublishInterval.ovInitData; - } - - return result; -} - -/*********************************************************************//** - * @brief - * The testSetTargetDrainPumpSpeedOverride function overrides the target \n - * drain pump speed (in RPM). - * @details - * Inputs : none - * Outputs : targetDrainPumpSpeed - * @param value override target drain pump speed (in RPM) - * @return TRUE if override successful, FALSE if not - *************************************************************************/ -BOOL testSetTargetDrainPumpSpeedOverride( U32 value ) -{ - BOOL result = FALSE; - - if ( TRUE == isTestingActivated() ) - { - targetDrainPumpSpeed.ovInitData = targetDrainPumpSpeed.data; // backup current target pressure - targetDrainPumpSpeed.ovData = value; - targetDrainPumpSpeed.override = OVERRIDE_KEY; - result = setDrainPumpTargetSpeed( value ); - } - - return result; -} - -/*********************************************************************//** - * @brief - * The testResetTargetDrainPumpSpeedOverride function resets the override of the \n - * target drain pump speed (in RPM). - * @details - * Inputs : none - * Outputs : targetDrainPumpSpeed - * @return TRUE if override reset successful, FALSE if not - *************************************************************************/ -BOOL testResetTargetDrainPumpSpeedOverride( void ) -{ - BOOL result = FALSE; - - if ( TRUE == isTestingActivated() ) - { - targetDrainPumpSpeed.data = targetDrainPumpSpeed.ovInitData; // restore pre-override target speed - targetDrainPumpSpeed.override = OVERRIDE_RESET; - targetDrainPumpSpeed.ovInitData = 0; - targetDrainPumpSpeed.ovData = 0; - result = setDrainPumpTargetSpeed( targetDrainPumpSpeed.data ); - } - - return result; -} - + + targetDrainPumpRPM = rpm; + drainPumpControlMode = PUMP_CONTROL_MODE_OPEN_LOOP; + drainPumpControlModeSet = drainPumpControlMode; + result = TRUE; + } + else + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_DRAIN_PUMP_INVALID_RPM_SELECTED, rpm ) + } + + return result; +} + +/*********************************************************************//** + * @brief + * 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 pressure new target drain pump outlet pressure + * @return TRUE if new target speed is set, FALSE if not + *************************************************************************/ +BOOL setDrainPumpTargetOutletPressure( F32 pressure ) +{ + BOOL result = FALSE; + + // Check the delta pressure is in range + if ( ( pressure >= MIN_ALLOWED_TARGET_OUTLET_PRESSURE ) && ( pressure <= MAX_ALLOWED_TARGET_OUTLET_PRESSURE ) ) + { + // 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; + } + 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: targetDrainPumpSpeed, drainPumpState, drainPumpControlMode, + * hasClosedLoopBeenRequested, drainControlTimerCounter + * @details Outputs: targetDrainPumpSpeed, drainPumpState, drainPumpControlMode, + * hasClosedLoopBeenRequested, drainControlTimerCounter + * @return none + *************************************************************************/ +void signalDrainPumpHardStop( void ) +{ + stopDrainPump(); + + // Reset all the variables to stop mode + targetDrainPumpRPM = 0; + drainPumpState = DRAIN_PUMP_OFF_STATE; + hasClosedLoopBeenRequested = FALSE; + drainPumpControlMode = NUM_OF_PUMP_CONTROL_MODES; // Set the control mode to none + drainControlTimerCounter = 0; +} + +/*********************************************************************//** + * @brief + * The execDrainPumpMonitor function executes the drain pump monitor. + * RPM = ( 1 / ADC ) * conversion coefficient. + * ADC = ( 1 / RPM ) * conversion coefficient. + * @details Inputs: currentDrainPumpRPM + * @details Outputs: currentDrainPumpRPM + * @return none + *************************************************************************/ +void execDrainPumpMonitor( void ) +{ + // Convert speed ADC to RPM + currentDrainPumpRPM = CONVERSION_COEFF / getFPGADrainPumpSpeed(); + + // TODO this is disabled until RPM is calculated propely. + // 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 ) + { + U32 targetRPM = getTargetDrainPumpRPM(); + F32 threshold = OPEN_LOOP_RPM_OUT_OF_RANGE * targetRPM; + + // 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(); + } + }*/ + + // Publish drain pump data on interval + publishDrainPumpData(); +} + +/*********************************************************************//** + * @brief + * The execDrainPumpController function executes the drain pump controller. + * @details Inputs: drainPumpState + * @details Outputs: drainPumpState + * @return none + *************************************************************************/ +void execDrainPumpController( void ) +{ + switch ( drainPumpState ) + { + case DRAIN_PUMP_OFF_STATE: + drainPumpState = handleDrainPumpOffState(); + break; + + case DRAIN_PUMP_CONTROL_TO_TARGET_STATE: + drainPumpState = handleDrainPumpControlToTargetState(); + break; + + case DRAIN_PUMP_OPEN_LOOP_STATE: + drainPumpState = handleDrainPumpOpenLoopState(); + break; + + default: + // Wrong state was selected + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_DRAIN_PUMP_INVALID_EXEC_STATE, drainPumpState ) + drainPumpState = DRAIN_PUMP_OFF_STATE; + break; + } +} + +/*********************************************************************//** + * @brief + * The execDrainPumpSelfTest function executes the state machine for the drain + * pump self-test. + * @details Inputs: TODO FILL UP + * @details Outputs: TODO FILL UP + * @return the current state of the Drain Pump self test. + *************************************************************************/ +SELF_TEST_STATUS_T execDrainPumpSelfTest( void ) +{ + SELF_TEST_STATUS_T result = SELF_TEST_STATUS_FAILED; + + // TODO - implement self-test(s) + + return result; +} + +/*********************************************************************//** + * @brief + * The getTargetDrainPumpRPM function gets the current target drain pump + * RPM. + * @details Inputs: targetDrainPumpRPM + * @details Outputs: none + * @return the current target drain pump RPM. + *************************************************************************/ +U32 getTargetDrainPumpRPM( void ) +{ + return targetDrainPumpRPM; +} + +/*********************************************************************//** + * @brief + * The getTargetDrainPumpDeltaP function gets the current target drain pump + * delta pressure. + * @details Inputs: targetDrainPumpOutletPressure + * @details Outputs: none + * @return the current target drain pump outlet pressure. + *************************************************************************/ +F32 getTargetDrainPumpOutletP( void ) +{ + return targetDrainPumpOutletPressure; +} + +/*********************************************************************//** + * @brief + * The handleDrainPumpOffState function handles the drain pump off state of + * the drain pump controller state machine. + * @details Inputs: drainPumpControlModeSet, drainPumpDACSet, drainPumpDAC + * hasClosedLoopBeenRequested + * @details Outputs: drainPumpDACSet + * @return next state for the controller state machine + *************************************************************************/ +static DRAIN_PUMP_STATE_T handleDrainPumpOffState( void ) +{ + DRAIN_PUMP_STATE_T result = DRAIN_PUMP_OFF_STATE; + + // If the target drain pump speed was not 0 and the control mode + // is open loop, set the drain pump to open loop + if ( getTargetDrainPumpRPM() > 0 && drainPumpControlModeSet == PUMP_CONTROL_MODE_OPEN_LOOP ) + { + // Set drain pump enable pin + SET_DRAIN_PUMP_ENABLE(); + + // Set drain pump DAC + drainPumpDACSet = drainPumpDAC; + setFPGADrainPumpSpeed( drainPumpDACSet ); + + result = DRAIN_PUMP_OPEN_LOOP_STATE; + } + // If the drain pump is set to closed loop, call the proper state + // It is checked for the value of delta pressure because it can be anything including 0 + else if ( drainPumpControlModeSet == PUMP_CONTROL_MODE_CLOSED_LOOP && hasClosedLoopBeenRequested ) + { + // set drain pump enable pin + SET_DRAIN_PUMP_ENABLE(); + + resetPIController( PI_CONTROLLER_ID_DRAIN_PUMP, DRAIN_PUMP_MIN_DAC ); + + // set drain pump DAC + drainPumpDACSet = drainPumpDAC; + setFPGADrainPumpSpeed( drainPumpDACSet ); + + result = DRAIN_PUMP_CONTROL_TO_TARGET_STATE; + } + + return result; +} + +/*********************************************************************//** + * @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 + * @return next state of the controller state machine + *************************************************************************/ +static DRAIN_PUMP_STATE_T handleDrainPumpControlToTargetState( void ) +{ + DRAIN_PUMP_STATE_T result = DRAIN_PUMP_CONTROL_TO_TARGET_STATE; + + // control at set interval + if ( ++drainControlTimerCounter >= DRP_CONTROL_INTERVAL ) + { + F32 outletDrainPressure = getMeasuredDGPressure ( PRESSURE_SENSOR_DRAIN_PUMP_OUTLET ); + 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 ); + setFPGADrainPumpSpeed( drainPumpDACSet ); + + drainControlTimerCounter = 0; + } + + return result; +} + +/*********************************************************************//** + * @brief + * The handleDrainPumpOpenLoopState function handles the open loop state. + * @details Inputs: none + * @details Outputs: none + * @return next state of the controller state machine + *************************************************************************/ +static DRAIN_PUMP_STATE_T handleDrainPumpOpenLoopState( void ) +{ + return DRAIN_PUMP_OPEN_LOOP_STATE; +} + +/*********************************************************************//** + * @brief + * The stopDrainPump function sets the drain pump DAC to zero and stops + * the pump. + * @details Inputs: drainPumpDAC, drainPumpDACSet + * @details Outputs: drainPumpDAC, drainPumpDACSet + * @return none + *************************************************************************/ +static void stopDrainPump( void ) +{ + drainPumpDAC = 0; + drainPumpDACSet = 0; + setFPGADrainPumpSpeed( drainPumpDACSet ); + CLR_DRAIN_PUMP_ENABLE(); +} + +/*********************************************************************//** + * @brief + * The getPublishDrainPumpDataInterval function gets the drain pump data + * publication interval. + * @details Inputs: drainPumpDataPublishInterval + * @details Outputs: drainPumpDataPublishInterval + * @return the current Drain pump data publication interval (in ms). + *************************************************************************/ +static U32 getPublishDrainPumpDataInterval( void ) +{ + U32 result = drainPumpDataPublishInterval.data; + + if ( OVERRIDE_KEY == drainPumpDataPublishInterval.override ) + { + result = drainPumpDataPublishInterval.ovData; + } + + return result; +} + +/*********************************************************************//** + * @brief + * The publishDrainPumpData function publishes drain pump data at the set + * interval. + * @details Inputs: drainPumpDataPublicationTimerCounter + * @details Outputs: drainPumpDataPublicationTimerCounter + * @return none + *************************************************************************/ +static void publishDrainPumpData( void ) +{ + // publish Drain pump data on interval + if ( ++drainPumpDataPublicationTimerCounter >= getPublishDrainPumpDataInterval() ) + { + DRAIN_PUMP_DATA_T drainPumpData; + + // Populate the data structure for publication + drainPumpData.rpmSetPoint = getTargetDrainPumpRPM(); + drainPumpData.pumpDACSet = drainPumpDACSet; + drainPumpData.drainPumpState = (U32)drainPumpState; + drainPumpData.drainPumpRPM = currentDrainPumpRPM; + + broadcastDrainPumpData( &drainPumpData ); + + drainPumpDataPublicationTimerCounter = 0; + } +} + +/************************************************************************* + * TEST SUPPORT FUNCTIONS + *************************************************************************/ + + +/*********************************************************************//** + * @brief + * The testSetDrainPumpDataPublishIntervalOverride function overrides the + * drain pump data publish interval. + * @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 + *************************************************************************/ +BOOL testSetDrainPumpDataPublishIntervalOverride( U32 value ) +{ + BOOL result = FALSE; + + if ( TRUE == isTestingActivated() ) + { + U32 intvl = value / TASK_PRIORITY_INTERVAL; + + drainPumpDataPublishInterval.ovData = intvl; + drainPumpDataPublishInterval.override = OVERRIDE_KEY; + result = TRUE; + } + + return result; +} + +/*********************************************************************//** + * @brief + * The testResetDrainPumpDataPublishIntervalOverride function resets the + * override of the drain pump data publish interval. + * @details Inputs: drainPumpDataPublishInterval + * @details Outputs: drainPumpDataPublishInterval + * @return TRUE if override reset successful, FALSE if not + *************************************************************************/ +BOOL testResetDrainPumpDataPublishIntervalOverride( void ) +{ + BOOL result = FALSE; + + if ( TRUE == isTestingActivated() ) + { + drainPumpDataPublishInterval.override = OVERRIDE_RESET; + drainPumpDataPublishInterval.ovData = drainPumpDataPublishInterval.ovInitData; + result = TRUE; + } + + return result; +} + +/*********************************************************************//** + * @brief + * The testSetTargetDrainPumpRPMOverride function overrides the target + * drain pump RPM. + * @details Inputs: none + * @details Outputs: none + * @param value override target drain pump RPM + * @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 ) + { + result = setDrainPumpTargetRPM( value ); + } + } + + return result; +} + +/*********************************************************************//** + * @brief + * The testSetTargetDrainPumpDeltaPressureOverride function overrides + * the target drain pump delta pressure. + * @details Inputs: none + * @details Outputs: none + * @param value override target drain pump delta pressure + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +BOOL testSetTargetDrainPumpOutletPressure( 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 ) + { + result = setDrainPumpTargetOutletPressure( value ); + } + } + + return result; +} + /**@}*/