Index: firmware/App/Controllers/DrainPump.c =================================================================== diff -u -r4d7d40a27130dc813d653f044cbb856b1b7d8481 -re4b79bd0d31e779619a787f9ce4352d0fd3d4dce --- firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision 4d7d40a27130dc813d653f044cbb856b1b7d8481) +++ firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision e4b79bd0d31e779619a787f9ce4352d0fd3d4dce) @@ -14,478 +14,617 @@ * @date (original) 08-Apr-2020 * ***************************************************************************/ - -#include - -#include "etpwm.h" -#include "mibspi.h" - -#include "DrainPump.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 + +#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 "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 at which the Drain Pump data is published on the CAN bus. -#define DRP_CONTROL_INTERVAL ( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) ///< interval 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-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. - -// 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; ///< Drain pump data publish timer counter. -static BOOL isDrainPumpOn = FALSE; ///< Flag indicates drain pump is running or off. -static U32 drainPumpDAC = 0; ///< Initial drain pump DAC value. -static U32 drainPumpDACSet = 0; ///< Current 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; ///< Current set drain pump control mode. - + +/** + * @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_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 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_TIME_OUT ( 5000 / TASK_PRIORITY_INTERVAL ) ///< Open loop RPM out of range time out in counts. + +/// 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; + +#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. + +// 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 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 }; ///< Drain pump data publish interval. -static OVERRIDE_U32_T targetDrainPumpSpeed = { 0, 0, 0, 0 }; ///< Target drain pump speed. -static U32 drainControlTimerCounter = 0; ///< Timer counter for control drain pump. - -static DRAIN_PUMP_SELF_TEST_STATE_T drainPumpSelfTestState; ///< 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 U32 getPublishDrainPumpDataInterval( void ); - -/*********************************************************************//** - * @brief - * The initDrainPump function initializes the DrainPump module. - * @details Inputs: none - * @details Outputs: DrainPump module initialized. - * @return none - *************************************************************************/ -void initDrainPump( void ) + 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 + +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 + +/* 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 +*/ + +/* + * Conversion of ADC count from micro seconds resolution to seconds and also converting toggle to pulse period. + * RPM = ( 1 / ADC ) * conversion coefficient. + * ADC = ( 1 / RPM ) * conversion coefficient. + */ +/// ADC to RPM conversion coefficient or RPM to ADC conversion +static const F32 conversionCoeff = 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: hasClosedLoopBeenRequested + * @details Outputs: hasClosedLoopBeenRequested + * @return none + *************************************************************************/ +void initDrainPump( void ) { - stopDrainPump(); -} - -/*********************************************************************//** - * @brief - * The setDrainPumpTargetSpeed function sets a new target speed for the drain pump. - * @details Inputs: none - * @details 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 ) ) ) - { + 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( 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 ); +} + +/*********************************************************************//** + * @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 ) ) ) + { #ifdef EMC_TEST_BUILD drainPumpDAC = (U32)((F32)2500 * DRP_SPEED_RPM_TO_ADC_FACTOR + FLOAT_TO_INT_ROUNDUP_OFFSET); #else 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 - * @details 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 - * @details Outputs: publish drain pump data - * @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 - * @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; - - 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; - } +#endif + targetDrainPumpRPM = rpm; + drainPumpControlMode = PUMP_CONTROL_MODE_OPEN_LOOP; + drainPumpControlModeSet = drainPumpControlMode; + result = TRUE; + } + + return result; } - -/*********************************************************************//** - * @brief + +/*********************************************************************//** + * @brief + * The setDrainPumpTargetDeltaPressure function sets the target delta + * pressure in between the PRd and PDr sensors. + * @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 + *************************************************************************/ +BOOL setDrainPumpTargetDeltaPressure( F32 deltaP ) +{ + BOOL result = FALSE; + + // Check the delta pressure is in range + if ( ( deltaP >= MIN_ALLOWED_TARGET_DELTA_PRESSURE ) && ( deltaP <= MIN_ALLOWED_TARGET_DELTA_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; + } + else + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_DRAIN_PUMP_INVALID_DELTA_PRESSURE_SELECTED, deltaP ) + } + + + 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. + * @details Inputs: currentDrainPumpRPM + * @details Outputs: currentDrainPumpRPM + * @return none + *************************************************************************/ +void execDrainPumpMonitor( void ) +{ + // Convert speed ADC to RPM + currentDrainPumpRPM = conversionCoeff / getFPGADrainPumpSpeed(); + + // 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( currentDrainPumpRPM - getTargetDrainPumpRPM() ) > MAX_ALLOWED_OPEN_LOOP_RPM_OUT_OF_RANGE; + checkPersistentAlarm( PERSISTENT_ALARM_DRAIN_PUMP_RPM_OUT_OF_RANGE, isRPMOutOfRange, currentDrainPumpRPM ); + } + + // 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 execDrainPumpTest function executes the state machine for the drain + * pump self-test. + * @detailsInputs: TODO FILL UP + * @details Outputs: TODO FILL UP + * @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; +} + +/*********************************************************************//** + * @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: targetDrainPumpDeltaPressure + * @details Outputs: none + * @return: the current target drain pump delta pressure. + *************************************************************************/ +F32 getTargetDrainPumpDeltaP( void ) +{ + return targetDrainPumpDeltaPressure; +} + +/*********************************************************************//** + * @brief * The handleDrainPumpOffState function handles the drain pump off state of - * the drain pump controller state machine. - * @details Inputs: targetDrainPumpSpeed - * @details 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 ); + * 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; + +#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 ); + setDrainPumpTargetRPM( 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 - * target" state of the drain pump controller state machine. - * @details Inputs: none - * @details 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 ); +#endif + + // 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 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 ); + // 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; + } + +#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 - * @details Outputs: isDrainPumpOn, DAC zeroed, clear drain pump enable - * @return none - *************************************************************************/ -static void stopDrainPump( void ) -{ - isDrainPumpOn = FALSE; - drainPumpDAC = 0; - drainPumpDACSet = 0; - setFPGADrainPumpSpeed( drainPumpDACSet ); - CLR_DRAIN_PUMP_ENABLE(); -} - -/*********************************************************************//** - * @brief + result = DRAIN_PUMP_OFF_STATE; + } +#endif +#endif + + 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: none - * @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 getTargetDrainPumpSpeed function gets the current target drain pump - * speed. - * @details Inputs: targetDrainPumpSpeed - * @details 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 interval. - * @details Inputs: target speed - * @details 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 const spdStPt = getTargetDrainPumpSpeed(); - broadcastDrainPumpData( spdStPt, drainPumpDACSet ); - drainPumpDataPublicationTimerCounter = 0; - } -} - -/*********************************************************************//** - * @brief - * The execDrainPumpTest function executes the state machine for the drain - * pump self-test. - * @details Inputs: none - * @details 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 - * drain pump data publish interval. - * @details Inputs: none - * @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; - - result = TRUE; - drainPumpDataPublishInterval.ovData = intvl; - drainPumpDataPublishInterval.override = OVERRIDE_KEY; - } - - return result; -} - -/*********************************************************************//** - * @brief + * 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: none - * @details 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 - * drain pump speed (in RPM). - * @details Inputs: none - * @details 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 - * target drain pump speed (in RPM). - * @details Inputs: none - * @details 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; -} - + * 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 testSetTargetDrainPumpDeltaPressure( 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 ) + { + result = setDrainPumpTargetDeltaPressure( value ); + } + } + + return result; +} + /**@}*/