Index: firmware/App/Controllers/DrainPump.c =================================================================== diff -u -ra7bf3ca23ea37a61000379facae628a31b3ecc59 -rf9b3862d01ce6c64bd05b8649e69b44fa155b8e0 --- firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision a7bf3ca23ea37a61000379facae628a31b3ecc59) +++ firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision f9b3862d01ce6c64bd05b8649e69b44fa155b8e0) @@ -7,31 +7,33 @@ * * @file DrainPump.c * -* @author (last) Sean Nash -* @date (last) 08-Jul-2020 +* @author (last) Quang Nguyen +* @date (last) 14-Sep-2020 * * @author (original) Sean * @date (original) 08-Apr-2020 * ***************************************************************************/ -#ifndef _VECTORCAST_ - #include -#endif +#include #include "etpwm.h" #include "mibspi.h" +#include "DrainPump.h" #include "FPGA.h" -#include "OperationModes.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" -#include "DrainPump.h" +#ifdef EMC_TEST_BUILD +#include "Heaters.h" +#endif /** * @addtogroup DrainPump @@ -40,148 +42,238 @@ // ********** 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 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_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 +#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. - 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. +/// 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_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; -// 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;} +#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 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 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 OVERRIDE_U32_T targetDrainPumpSpeed = { 0, 0, 0, 0 }; ///< Target RO pressure (in PSI). +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 -static U32 drainControlTimerCounter = 0; ///< determines when to perform control on 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 - +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 handleDrainPumpControlToTargetState( void ); +static DRAIN_PUMP_STATE_T handleDrainPumpOpenLoopState( void ); static void stopDrainPump( void ); static void publishDrainPumpData( void ); -static DATA_GET_PROTOTYPE( U32, getPublishDrainPumpDataInterval ); +static U32 getPublishDrainPumpDataInterval( void ); /*********************************************************************//** - * @brief initDrainPump + * @brief * The initDrainPump function initializes the DrainPump module. - * @details - * Inputs : none - * Outputs : DrainPump module initialized. + * @details Inputs: hasClosedLoopBeenRequested + * @details Outputs: hasClosedLoopBeenRequested * @return none *************************************************************************/ void initDrainPump( void ) -{ - stopDrainPump(); +{ + 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 speed for the \n + * The setDrainPumpTargetSpeed function sets a new target RPM for the * 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 + * @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 setDrainPumpTargetSpeed( U32 rpm ) +BOOL setDrainPumpTargetRPM( U32 rpm ) { - BOOL result = FALSE; + BOOL result = FALSE; - if ( ( 0 == rpm ) || - ( ( rpm >= MIN_DRAIN_PUMP_RPM_TARGET ) && ( rpm <= MAX_DRAIN_PUMP_RPM_TARGET ) ) ) + 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); - targetDrainPumpSpeed.data = rpm; - drainPumpControlMode = PUMP_CONTROL_MODE_OPEN_LOOP; +#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 + targetDrainPumpRPM = rpm; + drainPumpControlMode = PUMP_CONTROL_MODE_OPEN_LOOP; + drainPumpControlModeSet = drainPumpControlMode; result = TRUE; } return result; +} + +/*********************************************************************//** + * @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 : none - * Outputs : Drain pump stopped, set point reset, state changed to off + * @details Inputs: targetDrainPumpSpeed, drainPumpState, drainPumpControlMode, + * hasClosedLoopBeenRequested, drainControlTimerCounter + * @details Outputs: targetDrainPumpSpeed, drainPumpState, drainPumpControlMode, + * hasClosedLoopBeenRequested, drainControlTimerCounter * @return none *************************************************************************/ void signalDrainPumpHardStop( void ) -{ - targetDrainPumpSpeed.data = 0; - stopDrainPump(); - drainPumpState = DRAIN_PUMP_OFF_STATE; - drainControlTimerCounter = 0; +{ + 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 : none - * Outputs : none + * The execDrainPumpMonitor function executes the drain pump monitor. + * @details Inputs: currentDrainPumpRPM + * @details Outputs: currentDrainPumpRPM * @return none *************************************************************************/ void execDrainPumpMonitor( void ) -{ - U16 drnPumpSpd = getFPGADrainPumpSpeed(); +{ + // 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 ); + } - // TODO - convert drain pump speed to RPM - - // TODO - check(s) ??? - - // publish drain pump data on interval + // Publish drain pump data on interval publishDrainPumpData(); } /*********************************************************************//** * @brief - * The execDrainPumpController function executes the Drain Pump controller. - * @details - * Inputs : drainPumpState - * Outputs : drainPumpState - * @return none + * The execDrainPumpController function executes the drain pump controller. + * @details Inputs: drainPumpState + * @details Outputs: drainPumpState + * @return: none *************************************************************************/ void execDrainPumpController( void ) { @@ -193,27 +285,75 @@ case DRAIN_PUMP_CONTROL_TO_TARGET_STATE: drainPumpState = handleDrainPumpControlToTargetState(); + break; + + case DRAIN_PUMP_OPEN_LOOP_STATE: + drainPumpState = handleDrainPumpOpenLoopState(); break; - default: - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, 0, drainPumpState ) // TODO - replace 1st param with s/w fault enum + 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 \n - * of the drain pump controller state machine. - * @details - * Inputs : targetDrainPumpSpeed - * Outputs : drainPumpPWMDutyCyclePctSet, isDrainPumpOn - * @return next state + * 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; + DRAIN_PUMP_STATE_T result = DRAIN_PUMP_OFF_STATE; #ifdef DEBUG_ENABLED #ifdef ENABLE_DIP_SWITCHES @@ -223,37 +363,54 @@ setValveState( VPI, VALVE_STATE_OPEN ); setValveState( VRC, VALVE_STATE_DRAIN_C_TO_NO ); setValveState( VPO, VALVE_STATE_NOFILL_C_TO_NO ); - setDrainPumpTargetSpeed( 1000 ); + 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 ) + // 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 pin SET_DRAIN_PUMP_ENABLE(); - // set drain pump control mode - drainPumpControlModeSet = drainPumpControlMode; - // set drain pump DAC + + // Set drain pump DAC drainPumpDACSet = drainPumpDAC; setFPGADrainPumpSpeed( drainPumpDACSet ); - // set pump to on - isDrainPumpOn = TRUE; - result = DRAIN_PUMP_CONTROL_TO_TARGET_STATE; + + 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 \n - * target" state of the drain pump controller state machine. - * @details - * Inputs : none - * Outputs : drainPumpState - * @return next state + * 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 ) { @@ -262,19 +419,17 @@ // control at set interval if ( ++drainControlTimerCounter >= DRP_CONTROL_INTERVAL ) { - if ( drainPumpControlModeSet == PUMP_CONTROL_MODE_CLOSED_LOOP ) - { - // TODO - will drain pump have a closed loop? - } + 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; - } + } - if ( 0 == getTargetDrainPumpSpeed() ) - { - signalDrainPumpHardStop(); - result = DRAIN_PUMP_OFF_STATE; - } - #ifdef DEBUG_ENABLED #ifdef ENABLE_DIP_SWITCHES // TODO - test code - remove later @@ -284,41 +439,54 @@ 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 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. - * @details - * Inputs : none - * Outputs : isDrainPumpOn, DAC zeroed + * 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 ) { - isDrainPumpOn = FALSE; - drainPumpDAC = 0; + drainPumpDAC = 0; drainPumpDACSet = 0; setFPGADrainPumpSpeed( drainPumpDACSet ); CLR_DRAIN_PUMP_ENABLE(); } /*********************************************************************//** * @brief - * The getPublishDrainPumpDataInterval function gets the Drain pump data \n + * The getPublishDrainPumpDataInterval function gets the drain pump data * publication interval. - * @details - * Inputs : drainPumpDataPublishInterval - * Outputs : none - * @return the current Drain pump data publication interval (in ms). + * @details Inputs: drainPumpDataPublishInterval + * @details Outputs: drainPumpDataPublishInterval + * @return: the current Drain pump data publication interval (in ms). *************************************************************************/ -U32 getPublishDrainPumpDataInterval( void ) +static U32 getPublishDrainPumpDataInterval( void ) { U32 result = drainPumpDataPublishInterval.data; @@ -328,85 +496,48 @@ } 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 + * The publishDrainPumpData function publishes drain pump data at the set * interval. - * @details - * Inputs : target speed - * Outputs : Drain pump data is published to CAN bus. - * @return none + * @details Inputs: drainPumpDataPublicationTimerCounter + * @details Outputs: drainPumpDataPublicationTimerCounter + * @return: none *************************************************************************/ static void publishDrainPumpData( void ) { // publish Drain pump data on interval if ( ++drainPumpDataPublicationTimerCounter >= getPublishDrainPumpDataInterval() ) - { - U32 spdStPt = getTargetDrainPumpSpeed(); + { + 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( spdStPt, drainPumpDACSet ); + broadcastDrainPumpData( &drainPumpData ); + 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 + * The testSetDrainPumpDataPublishIntervalOverride function overrides the * 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 + * @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 ) { @@ -416,21 +547,20 @@ { U32 intvl = value / TASK_PRIORITY_INTERVAL; + drainPumpDataPublishInterval.ovData = intvl; + drainPumpDataPublishInterval.override = OVERRIDE_KEY; 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 + * 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 ) @@ -439,62 +569,62 @@ if ( TRUE == isTestingActivated() ) { - result = TRUE; drainPumpDataPublishInterval.override = OVERRIDE_RESET; - drainPumpDataPublishInterval.ovData = drainPumpDataPublishInterval.ovInitData; + drainPumpDataPublishInterval.ovData = drainPumpDataPublishInterval.ovInitData; + result = TRUE; } 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 + * 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 testSetTargetDrainPumpSpeedOverride( U32 value ) +BOOL testSetTargetDrainPumpRPM( 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 ); + { + // 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; +} -/*********************************************************************//** - * @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; -} - - +/**@}*/