Index: firmware/App/Controllers/DrainPump.c =================================================================== diff -u -r2510a84808ee5179d3f84f0b04f24c5a4122dbe5 -r1d235091e158221f1fa8b1579140905a13249a54 --- firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision 2510a84808ee5179d3f84f0b04f24c5a4122dbe5) +++ firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision 1d235091e158221f1fa8b1579140905a13249a54) @@ -40,22 +40,22 @@ // ********** 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_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_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 DRAIN_PUMP_P_COEFFICIENT 0.005 //TODO DARA -#define DRAIN_PUMP_I_COEFFICIENT 0.0015 //TODO DARA +#define DRAIN_PUMP_P_COEFFICIENT 0.005 ///< P term for drain pump delta pressure control +#define DRAIN_PUMP_I_COEFFICIENT 0.0015 ///< I term for drain pump delta pressure control /// Enumeration of drain pump states. typedef enum DrainPump_States { - DRAIN_PUMP_OFF_STATE = 0, ///< Drain pump off state + 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 - 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; @@ -82,27 +82,28 @@ 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_OPEN_LOOP; ///< requested drain pump control mode. -static PUMP_CONTROL_MODE_T drainPumpControlModeSet = PUMP_CONTROL_MODE_OPEN_LOOP; ///< currently set drain pump control mode. +static PUMP_CONTROL_MODE_T drainPumpControlMode = NUM_OF_PUMP_CONTROL_MODES; ///< requested 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 targetDrainPumpDeltaP = { 0, 0, 0, 0 }; ///< Target delta pressure for the drain pump + 0, 0 }; ///< interval (in ms) at which to publish RO flow data to CAN bus +static OVERRIDE_U32_T targetDrainPumpSpeed = { 0, 0, 0, 0 }; ///< Target RO pressure (in PSI) +static OVERRIDE_U32_T targetDrainPumpDeltaPressure = { 0, 0, 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 +/* 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 +*/ // ********** 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 ); @@ -119,7 +120,7 @@ { stopDrainPump(); - // Initialize the drain pump + // Initialize the drain pump PI controller initializePIController( PI_CONTROLLER_ID_DRAIN_PUMP, MIN_DRAIN_PUMP_RPM_TARGET, DRAIN_PUMP_P_COEFFICIENT, DRAIN_PUMP_I_COEFFICIENT, MIN_DRAIN_PUMP_RPM_TARGET, MAX_DRAIN_PUMP_RPM_TARGET ); @@ -151,11 +152,21 @@ return result; } +/*********************************************************************//** + * @brief + * The setDrainPumpTargetDeltaPressure function sets the target delta\n + * pressure in between the PRd and PDr sensors + * @details + * Inputs : targetDrainPumpDeltaP + * Outputs : targetDrainPumpDeltaP + * @param deltaP : new target drain pump delta pressure + * @return TRUE if new target speed is set, FALSE if not + *************************************************************************/ BOOL setDrainPumpTargetDeltaPressure ( U32 deltaP ) { BOOL result = FALSE; - targetDrainPumpDeltaP.data = deltaP; + targetDrainPumpDeltaPressure.data = deltaP; drainPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; result = TRUE; @@ -174,7 +185,8 @@ { targetDrainPumpSpeed.data = 0; stopDrainPump(); - drainPumpState = DRAIN_PUMP_OFF_STATE; + drainPumpState = DRAIN_PUMP_OFF_STATE; + drainPumpControlMode = NUM_OF_PUMP_CONTROL_MODES; // Set the control mode to none drainControlTimerCounter = 0; } @@ -216,6 +228,10 @@ case DRAIN_PUMP_CONTROL_TO_TARGET_STATE: drainPumpState = handleDrainPumpControlToTargetState(); + break; + + case DRAIN_PUMP_OPEN_LOOP_STATE: + drainPumpState = handleDrainPumpOpenLoopState(); break; default: @@ -231,7 +247,7 @@ * of the drain pump controller state machine. * @details * Inputs : targetDrainPumpSpeed - * Outputs : drainPumpPWMDutyCyclePctSet, isDrainPumpOn + * Outputs : drainPumpPWMDutyCyclePctSet * @return next state *************************************************************************/ static DRAIN_PUMP_STATE_T handleDrainPumpOffState( void ) @@ -251,39 +267,36 @@ #endif #endif - // if we've been given a pressure, transition to open loop state - // as it will not be controlling and it will just run + // If the target drain pump speed was not 0 and the control mode + // is open loop, set the drain pump to open loop if ( getTargetDrainPumpSpeed() > 0 && drainPumpControlMode == PUMP_CONTROL_MODE_OPEN_LOOP ) { // 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_OPEN_LOOP_STATE; } - - else if ( drainPumpControlMode == PUMP_CONTROL_MODE_OPEN_LOOP ) + // 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 ( drainPumpControlMode == PUMP_CONTROL_MODE_CLOSED_LOOP ) { // set drain pump enable pin SET_DRAIN_PUMP_ENABLE(); - // set drain pump control mode - drainPumpControlModeSet = drainPumpControlMode; resetPIController( PI_CONTROLLER_ID_DRAIN_PUMP, MIN_DRAIN_PUMP_RPM_TARGET ); - U32 rpm = 500; + U32 rpm = targetDrainPumpSpeed.data; drainPumpDAC = (U32)((F32)rpm * DRP_SPEED_RPM_TO_ADC_FACTOR + FLOAT_TO_INT_ROUNDUP_OFFSET); // set drain pump DAC drainPumpDACSet = drainPumpDAC; setFPGADrainPumpSpeed( drainPumpDACSet ); - // set pump to on - isDrainPumpOn = TRUE; + result = DRAIN_PUMP_CONTROL_TO_TARGET_STATE; } @@ -306,21 +319,19 @@ // control at set interval if ( ++drainControlTimerCounter >= DRP_CONTROL_INTERVAL ) { - if ( drainPumpControlModeSet == PUMP_CONTROL_MODE_CLOSED_LOOP ) - { - F32 inletDrainPressure = getMeasuredDGPressure ( PRESSURE_SENSOR_DRAIN_PUMP_INLET ); - F32 outletDrainPressure = getMeasuredDGPressure ( PRESSURE_SENSOR_DRAIN_PUMP_OUTLET ); - F32 pressureDiff = fabs( inletDrainPressure - outletDrainPressure ); - F32 rpm = runPIController( PI_CONTROLLER_ID_DRAIN_PUMP, targetDrainPumpDeltaP.data, pressureDiff ); - drainPumpDAC = (U32)((F32)rpm * DRP_SPEED_RPM_TO_ADC_FACTOR + FLOAT_TO_INT_ROUNDUP_OFFSET); - setFPGADrainPumpSpeed( drainPumpDAC ); - } + F32 inletDrainPressure = getMeasuredDGPressure ( PRESSURE_SENSOR_DRAIN_PUMP_INLET ); + F32 outletDrainPressure = getMeasuredDGPressure ( PRESSURE_SENSOR_DRAIN_PUMP_OUTLET ); + F32 pressureDiff = fabs( inletDrainPressure - outletDrainPressure ); + F32 rpm = runPIController( PI_CONTROLLER_ID_DRAIN_PUMP, targetDrainPumpDeltaPressure.data, pressureDiff ); + drainPumpDAC = (U32)(rpm * DRP_SPEED_RPM_TO_ADC_FACTOR + FLOAT_TO_INT_ROUNDUP_OFFSET); + setFPGADrainPumpSpeed( drainPumpDAC ); drainControlTimerCounter = 0; } if ( 0 == getTargetDrainPumpSpeed() ) { - signalDrainPumpHardStop(); + signalDrainPumpHardStop(); + result = DRAIN_PUMP_OFF_STATE; } @@ -339,19 +350,32 @@ #endif return result; +} + +/*********************************************************************//** + * @brief + * The handleDrainPumpOpenLoopState function handles the open loop \n + * state + * @details + * Inputs : none + * Outputs : none + * @return next state + *************************************************************************/ +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 + * Outputs : DAC zeroed * @return none *************************************************************************/ static void stopDrainPump( void ) { - isDrainPumpOn = FALSE; drainPumpDAC = 0; drainPumpDACSet = 0; setFPGADrainPumpSpeed( drainPumpDACSet ); @@ -414,9 +438,12 @@ // publish Drain pump data on interval if ( ++drainPumpDataPublicationTimerCounter >= getPublishDrainPumpDataInterval() ) { - U32 spdStPt = getTargetDrainPumpSpeed(); + U32 spdStPt = getTargetDrainPumpSpeed(); + + F32 pressureDiff = fabs( getMeasuredDGPressure ( PRESSURE_SENSOR_DRAIN_PUMP_INLET ) - + getMeasuredDGPressure ( PRESSURE_SENSOR_DRAIN_PUMP_OUTLET ) ); - broadcastDrainPumpData( spdStPt, drainPumpDACSet ); + broadcastDrainPumpData( spdStPt, drainPumpDACSet, pressureDiff, (U32)drainPumpState ); drainPumpDataPublicationTimerCounter = 0; } } @@ -441,7 +468,6 @@ /**@}*/ - /************************************************************************* * TEST SUPPORT FUNCTIONS *************************************************************************/ @@ -544,6 +570,56 @@ } return result; +} + +/*********************************************************************//** + * @brief + * The testSetTargetDrainPumpDeltaPressureOverride function overrides \n + * the target drain pump delta pressure + * @details + * Inputs : none + * Outputs : targetDrainPumpDeltaPressure + * @param value : override target drain pump delta pressure + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +BOOL testSetTargetDrainPumpDeltaPressureOverride( U32 value ) +{ + BOOL result = FALSE; + + if ( TRUE == isTestingActivated() ) + { + targetDrainPumpDeltaPressure.ovInitData = targetDrainPumpDeltaPressure.data; // backup current target delta pressure + targetDrainPumpDeltaPressure.ovData = value; + targetDrainPumpDeltaPressure.override = OVERRIDE_KEY; + result = setDrainPumpTargetDeltaPressure( value ); + } + + return result; +} + +/*********************************************************************//** + * @brief + * The testResetTargetDrainPumpDeltaPressureOverride function resets the \n + * override of the target drain pump delta pressure + * @details + * Inputs : none + * Outputs : targetDrainPumpDeltaPressure + * @return TRUE if override reset successful, FALSE if not + *************************************************************************/ +BOOL testResetTargetDrainPumpDeltaPressureOverride( void ) +{ + BOOL result = FALSE; + + if ( TRUE == isTestingActivated() ) + { + targetDrainPumpDeltaPressure.data = targetDrainPumpDeltaPressure.ovInitData; // restore pre-override target delta pressure + targetDrainPumpDeltaPressure.override = OVERRIDE_RESET; + targetDrainPumpDeltaPressure.ovInitData = 0; + targetDrainPumpDeltaPressure.ovData = 0; + result = setDrainPumpTargetDeltaPressure( targetDrainPumpDeltaPressure.data ); + } + + return result; }