Index: firmware/App/Controllers/DrainPump.c =================================================================== diff -u -r33d262afee2fd0dd9d714ed86a21b8907ed84e25 -r696e732c9742535a58b9c65f243df7cd797d1423 --- firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision 33d262afee2fd0dd9d714ed86a21b8907ed84e25) +++ firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision 696e732c9742535a58b9c65f243df7cd797d1423) @@ -7,8 +7,8 @@ * * @file DrainPump.c * -* @author (last) Bill Bracken -* @date (last) 22-Aug-2022 +* @author (last) Dara Navaei +* @date (last) 28-Aug-2022 * * @author (original) Sean * @date (original) 08-Apr-2020 @@ -47,7 +47,7 @@ #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. TODO original one is 1 second +#define DRP_CONTROL_INTERVAL ( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) ///< Interval (ms/task time) at which the Drain pump is controlled. #define RPM_2_DAC_SLOPE 0.0547F ///< RPM to DAC conversion slope. #define RPM_2_DAC_INTERCEPT 2.9968F ///< RPM to DAC conversion intercept. @@ -74,6 +74,11 @@ #define DRAIN_PUMP_OFF_RPM_ADC_COUNT 0xFFFF ///< ADC count value when pump is off. #define DATA_PUBLISH_COUNTER_START_COUNT 60 ///< Data publish counter start count. +#define DRAIN_PUMP_MAX_CURRENT_WHEN_OFF_A 0.1F ///< Drain pump maximum current when the pump is off in amps. +#define DRAIN_PUMP_MAX_CURRENT_A 2.2F ///< Drain pump maximum current in amps. +#define DRAIN_PUMP_CURRENT_OUT_OF_RANGE_TIMEOUT_MS ( 2 * MS_PER_SECOND ) ///< Drain pump current out of range timeout in milliseconds. +#define DRAIN_PUMP_DIR_OF_RANGE_TIMEOUT_MS ( 2 * MS_PER_SECOND ) ///< Drain pump direction out of range timeout in milliseconds. +#define DRAIN_PUMP_FORWARD_DIR 0 ///< Drain pump forward direction. /// Enumeration of drain pump states. typedef enum DrainPump_States @@ -95,32 +100,32 @@ // ********** private data ********** -static DRAIN_PUMP_STATE_T drainPumpState = DRAIN_PUMP_OFF_STATE; ///< Current state of drain pump controller state machine. +static DRAIN_PUMP_STATE_T drainPumpState; ///< Current state of drain pump controller state machine. static U32 drainPumpDataPublicationTimerCounter; ///< 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 U32 drainPumpDAC; ///< Initial drain pump DAC value. +static U32 drainPumpDACSet; ///< Currently set drain pump DAC value. +static PUMP_CONTROL_MODE_T drainPumpControlMode; ///< Requested drain pump control mode. +static PUMP_CONTROL_MODE_T drainPumpControlModeSet; ///< 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 drain pump 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 targetDrainPumpRPM; ///< Target drain pump RPM. +static F32 targetDrainPumpOutletPressure; ///< Target outlet pressure for the drain pump. +static U32 drainControlTimerCounter; ///< Determines when to perform control on drain pump. +static BOOL hasClosedLoopBeenRequested; ///< Closed loop pump control flag. static OVERRIDE_U32_T drainPumpMeasuredRPM = { 0, 0, 0, 0 }; ///< Measured drain pump RPM from feedback. -static BOOL signalNewRPMRequest = FALSE; ///< Signal flag the indicates there is a new RPM request. - -static DRAIN_PUMP_STATE_T pendingDrainPumpCmd = DRAIN_PUMP_OFF_STATE; ///< Delayed (pending) drain pump command. -static F32 pendingDrainPumpCmdTarget = 0.0; ///< Delayed (pending) drain pump command target (rpm or PSI depending on command). -static U32 pendingDrainPumpCmdCountDown = 0; ///< Delayed (pending) drain pump command count down timer (in task intervals). +static OVERRIDE_F32_T drainPumpMeasuredCurrentA = { 0.0F, 0.0F, 0.0F, 0 }; ///< Measured drain pump current feedback. +static OVERRIDE_U32_T drainPumpMeasuredDir = { 0, 0, 0, 0 }; ///< Measured drain pump direction. +static BOOL signalNewRPMRequest; ///< Signal flag the indicates there is a new RPM request. +static DRAIN_PUMP_STATE_T pendingDrainPumpCmd; ///< Delayed (pending) drain pump command. +static F32 pendingDrainPumpCmdTarget; ///< Delayed (pending) drain pump command target (rpm or PSI depending on command). +static U32 pendingDrainPumpCmdCountDown; ///< Delayed (pending) drain pump command count down timer (in task intervals). static DG_DRAIN_LINE_VOLUME_T drainLineVolumeRecord; ///< Drain line volume record. static DG_DRAIN_PUMP_CAL_RECORD_T drainPumpCalRecord; ///< Drain pump calibration record. /// ADC to RPM conversion coefficient or RPM to ADC conversion. -static const F32 CONVERSION_COEFF = (F32) SEC_PER_MIN / ( 2.0F * TOGGLE_PERIOD_RESOLUTION_SECONDS * ROTATIONAL_TO_TOGGLE_PERIOD_CONVERSION ); +static const F32 RPM_CONVERSION_COEFF = (F32) SEC_PER_MIN / ( 2.0F * TOGGLE_PERIOD_RESOLUTION_SECONDS * ROTATIONAL_TO_TOGGLE_PERIOD_CONVERSION ); +static const F32 CURRENT_CONVERSION_COEFF = (F32)( 2.5F / ( BITS_12_FULL_SCALE - 1.0F ) ); // ********** private function prototypes ********** @@ -129,18 +134,36 @@ static DRAIN_PUMP_STATE_T handleDrainPumpOpenLoopState( void ); static void stopDrainPump( void ); static void publishDrainPumpData( void ); +static F32 getDrainPumpMeasuredCurrentA( void ); +static U32 getDrainPumpMeasuredDirection( void ); /*********************************************************************//** * @brief * The initDrainPump function initializes the DrainPump module. * @details Inputs: none - * @details Outputs: hasClosedLoopBeenRequested, signalNewRPMRequest + * @details Outputs: drainPumpState, drainPumpDAC, drainPumpDACSet, + * drainPumpControlMode, drainPumpControlModeSet, targetDrainPumpRPM, + * targetDrainPumpOutletPressure, drainControlTimerCounter, + * pendingDrainPumpCmd, pendingDrainPumpCmdTarget, pendingDrainPumpCmdCountDown, + * hasClosedLoopBeenRequested, signalNewRPMRequest, + * drainPumpDataPublicationTimerCounter, * @return none *************************************************************************/ void initDrainPump( void ) { stopDrainPump(); + drainPumpState = DRAIN_PUMP_OFF_STATE; + drainPumpDAC = 0; + drainPumpDACSet = 0; + drainPumpControlMode = NUM_OF_PUMP_CONTROL_MODES; + drainPumpControlModeSet = PUMP_CONTROL_MODE_CLOSED_LOOP; + targetDrainPumpRPM = 0; + targetDrainPumpOutletPressure = 0.0F; + drainControlTimerCounter = 0; + pendingDrainPumpCmd = DRAIN_PUMP_OFF_STATE; + pendingDrainPumpCmdTarget = 0.0F; + pendingDrainPumpCmdCountDown = 0; hasClosedLoopBeenRequested = FALSE; signalNewRPMRequest = FALSE; drainPumpDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; @@ -149,11 +172,11 @@ 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 + // Initialize the persistent alarms 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 ); + initPersistentAlarm( ALARM_ID_DG_DRAIN_PUMP_CURRENT_OUT_OF_RANGE, DRAIN_PUMP_CURRENT_OUT_OF_RANGE_TIMEOUT_MS, DRAIN_PUMP_CURRENT_OUT_OF_RANGE_TIMEOUT_MS ); + initPersistentAlarm( ALARM_ID_DG_DRAIN_PUMP_DIRECTION_INVALID, DRAIN_PUMP_DIR_OF_RANGE_TIMEOUT_MS, DRAIN_PUMP_DIR_OF_RANGE_TIMEOUT_MS ); } /*********************************************************************//** @@ -315,49 +338,102 @@ * The execDrainPumpMonitor function executes the drain pump monitor. * RPM = ( 1 / ADC ) * conversion coefficient. * ADC = ( 1 / RPM ) * conversion coefficient. - * @details Inputs: drainPumpControlModeSet, drainPumpState - * @details Outputs: drainPumpMeasuredRPM + * @details Inputs: drainPumpMeasuredRPM, drainPumpMeasuredCurrentA, + * drainPumpMeasuredDir + * @details Outputs: drainPumpMeasuredRPM, drainPumpMeasuredCurrentA, + * drainPumpMeasuredDir * @return none *************************************************************************/ void execDrainPumpMonitor( void ) { - U16 fpgaADCSpeedCount = getFPGADrainPumpSpeed(); - drainPumpMeasuredRPM.data = ( DRAIN_PUMP_OFF_RPM_ADC_COUNT == fpgaADCSpeedCount ? 0 : (U32)( CONVERSION_COEFF / (F32)fpgaADCSpeedCount ) ); + U16 fpgaADCSpeedCount = getFPGADrainPumpSpeed(); + U16 fpgaADCCurrentCount = getFPGADrainPumpCurrentFeedback(); + F32 currentA = getDrainPumpMeasuredCurrentA(); + drainPumpMeasuredRPM.data = ( DRAIN_PUMP_OFF_RPM_ADC_COUNT == fpgaADCSpeedCount ? 0 : (U32)( RPM_CONVERSION_COEFF / (F32)fpgaADCSpeedCount ) ); + drainPumpMeasuredCurrentA.data = (F32)fpgaADCCurrentCount * CURRENT_CONVERSION_COEFF; + drainPumpMeasuredDir.data = ( (U32)getFPGADrainPumpDirection() & 0x40 ) >> 7; #ifndef _RELEASE_ if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_DRAIN_PUMP_MONITOR ) != SW_CONFIG_ENABLE_VALUE ) #endif { - // Check if the pump is in off state and the RPM is greater than the minimum RPM - if ( DRAIN_PUMP_OFF_STATE == drainPumpState ) + switch( drainPumpState ) { - BOOL isRPMTooHigh = ( getDrainPumpMeasuredRPM() > MIN_DRAIN_PUMP_RPM ? TRUE : FALSE ); + case DRAIN_PUMP_OFF_STATE: + { + BOOL isOffCurrentOut = ( currentA > DRAIN_PUMP_MAX_CURRENT_WHEN_OFF_A ? TRUE : FALSE ); + BOOL isRPMTooHigh = ( getDrainPumpMeasuredRPM() > MIN_DRAIN_PUMP_RPM ? TRUE : FALSE ); - checkPersistentAlarm( ALARM_ID_DRAIN_PUMP_RPM_OUT_OF_RANGE, FALSE, getDrainPumpMeasuredRPM(), MAX_ALLOWED_OPEN_LOOP_RPM_OUT_OF_RANGE ); - checkPersistentAlarm( ALARM_ID_DRAIN_PUMP_OFF_FAULT, isRPMTooHigh, getDrainPumpMeasuredRPM(), MIN_DRAIN_PUMP_RPM ); + checkPersistentAlarm( ALARM_ID_DRAIN_PUMP_RPM_OUT_OF_RANGE, FALSE, getDrainPumpMeasuredRPM(), MAX_ALLOWED_OPEN_LOOP_RPM_OUT_OF_RANGE ); + checkPersistentAlarm( ALARM_ID_DRAIN_PUMP_OFF_FAULT, isRPMTooHigh, getDrainPumpMeasuredRPM(), MIN_DRAIN_PUMP_RPM ); - // If the off fault alarm has become active, trigger the safety shutdown - if ( TRUE == isAlarmActive( ALARM_ID_DRAIN_PUMP_OFF_FAULT ) ) + // If the off fault alarm has become active, trigger the safety shutdown + if ( TRUE == isAlarmActive( ALARM_ID_DRAIN_PUMP_OFF_FAULT ) ) + { + activateSafetyShutdown(); + } + +#ifndef _RELEASE_ + if ( ( getSoftwareConfigStatus( SW_CONFIG_ENABLE_V3_SYSTEM ) != SW_CONFIG_ENABLE_VALUE ) && ( getCurrentOperationMode() != DG_MODE_INIT ) ) +#endif + { + checkPersistentAlarm( ALARM_ID_DG_DRAIN_PUMP_CURRENT_OUT_OF_RANGE, isOffCurrentOut, currentA, DRAIN_PUMP_MAX_CURRENT_WHEN_OFF_A ); + checkPersistentAlarm( ALARM_ID_DG_DRAIN_PUMP_DIRECTION_INVALID, FALSE, getFPGADrainPumpDirection(), DRAIN_PUMP_FORWARD_DIR ); + } + } + break; + + // The RPM is only checked in open loop state that the pump is run at a fixed RPM. + case DRAIN_PUMP_OPEN_LOOP_STATE: { - activateSafetyShutdown(); + U32 direction = getDrainPumpMeasuredDirection(); + BOOL isDirInvalid = ( direction != DRAIN_PUMP_FORWARD_DIR ? TRUE : FALSE ); + U32 rpmDiff = abs( getDrainPumpTargetRPM() - getDrainPumpMeasuredRPM() ); + BOOL isRPMOutOfRange = ( rpmDiff > MAX_ALLOWED_OPEN_LOOP_RPM_OUT_OF_RANGE ? TRUE : FALSE ); + + checkPersistentAlarm( ALARM_ID_DRAIN_PUMP_RPM_OUT_OF_RANGE, isRPMOutOfRange, getDrainPumpMeasuredRPM(), MAX_ALLOWED_OPEN_LOOP_RPM_OUT_OF_RANGE ); + checkPersistentAlarm( ALARM_ID_DRAIN_PUMP_OFF_FAULT, FALSE, getDrainPumpMeasuredRPM(), MIN_DRAIN_PUMP_RPM ); + +#ifndef _RELEASE_ + if ( ( getSoftwareConfigStatus( SW_CONFIG_ENABLE_V3_SYSTEM ) != SW_CONFIG_ENABLE_VALUE ) && ( getCurrentOperationMode() != DG_MODE_INIT ) ) +#endif + { + checkPersistentAlarm( ALARM_ID_DG_DRAIN_PUMP_DIRECTION_INVALID, isDirInvalid, (F32)direction, DRAIN_PUMP_FORWARD_DIR ); + } } + break; + + case DRAIN_PUMP_CONTROL_TO_TARGET_STATE: + { + U32 direction = getDrainPumpMeasuredDirection(); + BOOL isDirInvalid = ( direction != DRAIN_PUMP_FORWARD_DIR ? TRUE : FALSE ); + +#ifndef _RELEASE_ + if ( ( getSoftwareConfigStatus( SW_CONFIG_ENABLE_V3_SYSTEM ) != SW_CONFIG_ENABLE_VALUE ) && ( getCurrentOperationMode() != DG_MODE_INIT ) ) +#endif + { + checkPersistentAlarm( ALARM_ID_DG_DRAIN_PUMP_DIRECTION_INVALID, isDirInvalid, (F32)direction, DRAIN_PUMP_FORWARD_DIR ); + } + } + break; + + default: + // There are not out of range conditions, clear the alarms + checkPersistentAlarm( ALARM_ID_DRAIN_PUMP_RPM_OUT_OF_RANGE, FALSE, getDrainPumpMeasuredRPM(), MAX_ALLOWED_OPEN_LOOP_RPM_OUT_OF_RANGE ); + checkPersistentAlarm( ALARM_ID_DRAIN_PUMP_OFF_FAULT, FALSE, getDrainPumpMeasuredRPM(), MIN_DRAIN_PUMP_RPM ); + checkPersistentAlarm( ALARM_ID_DG_DRAIN_PUMP_DIRECTION_INVALID, FALSE, getFPGADrainPumpDirection(), DRAIN_PUMP_FORWARD_DIR ); + break; } - // The RPM is only checked in open loop state that the pump is run at a fixed RPM. - else if ( DRAIN_PUMP_OPEN_LOOP_STATE == drainPumpState ) + +#ifndef _RELEASE_ + if ( ( getSoftwareConfigStatus( SW_CONFIG_ENABLE_V3_SYSTEM ) != SW_CONFIG_ENABLE_VALUE ) && ( getCurrentOperationMode() != DG_MODE_INIT ) ) +#endif { - // Using abs since the read RPM can be above or below the target - U32 rpmDiff = abs( getDrainPumpTargetRPM() - getDrainPumpMeasuredRPM() ); - BOOL isRPMOutOfRange = ( rpmDiff > MAX_ALLOWED_OPEN_LOOP_RPM_OUT_OF_RANGE ? TRUE : FALSE ); + // Check the persistent alarm for the maximum drain pump current + BOOL isCurrentOutOfRange = ( currentA > DRAIN_PUMP_MAX_CURRENT_A ? TRUE : FALSE ); - checkPersistentAlarm( ALARM_ID_DRAIN_PUMP_RPM_OUT_OF_RANGE, isRPMOutOfRange, getDrainPumpMeasuredRPM(), MAX_ALLOWED_OPEN_LOOP_RPM_OUT_OF_RANGE ); - checkPersistentAlarm( ALARM_ID_DRAIN_PUMP_OFF_FAULT, FALSE, getDrainPumpMeasuredRPM(), MIN_DRAIN_PUMP_RPM ); + checkPersistentAlarm( ALARM_ID_DG_DRAIN_PUMP_CURRENT_OUT_OF_RANGE, isCurrentOutOfRange, currentA, DRAIN_PUMP_MAX_CURRENT_A ); } - else - { - // There are not out of range conditions, clear the alarms - checkPersistentAlarm( ALARM_ID_DRAIN_PUMP_RPM_OUT_OF_RANGE, FALSE, getDrainPumpMeasuredRPM(), MAX_ALLOWED_OPEN_LOOP_RPM_OUT_OF_RANGE ); - checkPersistentAlarm( ALARM_ID_DRAIN_PUMP_OFF_FAULT, FALSE, getDrainPumpMeasuredRPM(), MIN_DRAIN_PUMP_RPM ); - } } // Publish drain pump data on interval @@ -485,13 +561,8 @@ *************************************************************************/ U32 getDrainPumpMeasuredRPM( void ) { - U32 rpm = drainPumpMeasuredRPM.data; + U32 rpm = getU32OverrideValue( &drainPumpMeasuredRPM ); - if ( OVERRIDE_KEY == drainPumpMeasuredRPM.override ) - { - rpm = drainPumpMeasuredRPM.ovData; - } - return rpm; } @@ -649,7 +720,7 @@ * The publishDrainPumpData function publishes drain pump data at the set * interval. * @details Inputs: drainPumpDataPublicationTimerCounter - * @details Outputs: drainPumpDataPublicationTimerCounter + * @details Outputs: none * @return none *************************************************************************/ static void publishDrainPumpData( void ) @@ -660,19 +731,51 @@ DRAIN_PUMP_DATA_T drainPumpData; // Populate the data structure for publication - drainPumpData.rpmSetPoint = getDrainPumpTargetRPM(); - drainPumpData.pumpDACSet = drainPumpDACSet; - drainPumpData.drainPumpState = (U32)drainPumpState; - drainPumpData.drainPumpRPM = getDrainPumpMeasuredRPM(); - drainPumpData.trgtOutletPrsr = targetDrainPumpOutletPressure; + drainPumpData.rpmSetPoint = getDrainPumpTargetRPM(); + drainPumpData.pumpDACSet = drainPumpDACSet; + drainPumpData.drainPumpState = (U32)drainPumpState; + drainPumpData.drainPumpRPM = getDrainPumpMeasuredRPM(); + drainPumpData.trgtOutletPrsr = targetDrainPumpOutletPressure; + drainPumpData.drainPumpCurrentA = getDrainPumpMeasuredCurrentA(); + drainPumpData.drainPumpDirection = getDrainPumpMeasuredDirection(); broadcastData( MSG_ID_DRAIN_PUMP_DATA, COMM_BUFFER_OUT_CAN_DG_BROADCAST, (U08*)&drainPumpData, sizeof( DRAIN_PUMP_DATA_T ) ); drainPumpDataPublicationTimerCounter = 0; } } +/*********************************************************************//** + * @brief + * The getDrainPumpMeasuredCurrentA function returns the drain pump current + * feedback. + * @details Inputs: drainPumpMeasuredCurrent + * @details Outputs: none + * @return drain pump current feedback + *************************************************************************/ +static F32 getDrainPumpMeasuredCurrentA( void ) +{ + F32 current = getF32OverrideValue( &drainPumpMeasuredCurrentA ); + return current; +} + +/*********************************************************************//** + * @brief + * The getDrainPumpMeasuredDirection function returns the drain pump current + * direction. + * @details Inputs: drainPumpMeasuredDir + * @details Outputs: none + * @return drain pump current direction + *************************************************************************/ +static U32 getDrainPumpMeasuredDirection( void ) +{ + U32 direction = getU32OverrideValue( &drainPumpMeasuredDir ); + + return direction; +} + + /************************************************************************* * TEST SUPPORT FUNCTIONS *************************************************************************/