Index: firmware/App/Controllers/DrainPump.c =================================================================== diff -u -rebbb1f85550a1f9b8f946655f7b2b63f76fbf67d -ra2c32d4d221603054ca9ad7a097112caebf08c4e --- firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision ebbb1f85550a1f9b8f946655f7b2b63f76fbf67d) +++ firmware/App/Controllers/DrainPump.c (.../DrainPump.c) (revision a2c32d4d221603054ca9ad7a097112caebf08c4e) @@ -43,27 +43,28 @@ // ********** 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_MIN_DAC GET_RPM_2_DAC_CONVERSION( MIN_DRAIN_PUMP_RPM ) ///< Drain pump minimum RPM to DAC conversion. +#define DRAIN_PUMP_MAX_DAC GET_RPM_2_DAC_CONVERSION( MAX_DRAIN_PUMP_RPM ) ///< 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 RPM_2_DAC_SLOPE 0.0547 ///< RPM to DAC conversion slope. +#define RPM_2_DAC_INTERCEPT 2.9968 ///< RPM to DAC conversion intercept. +#define GET_RPM_2_DAC_CONVERSION(rpm) ( ( RPM_2_DAC_SLOPE * rpm ) - RPM_2_DAC_INTERCEPT + \ + FLOAT_TO_INT_ROUNDUP_OFFSET ) ///< RPM to DAC conversion equation. #define TOGGLE_PERIOD_RESOLUTION_SECONDS 0.000005 ///< Toggle period to resolution in seconds. #define ROTATIONAL_TO_TOGGLE_PERIOD_CONVERSION 4 ///< Rotational to toggle period conversion coefficient. #define DRAIN_PUMP_P_COEFFICIENT 0.5 ///< P term for drain pump delta pressure control. #define DRAIN_PUMP_I_COEFFICIENT 1.0 ///< I term for drain pump delta pressure control. -#define MIN_ALLOWED_TARGET_OUTLET_PRESSURE -10.0 ///< Minimum allowed outlet pressure for closed loop control. -#define MAX_ALLOWED_TARGET_OUTLET_PRESSURE 10.0 ///< Maximum allowed outlet pressure for closed loop control. +#define MIN_ALLOWED_TARGET_OUTLET_PRESSURE -15.0 ///< Minimum allowed outlet pressure for closed loop control. +#define MAX_ALLOWED_TARGET_OUTLET_PRESSURE 15.0 ///< Maximum allowed outlet pressure for closed loop control. -#define OPEN_LOOP_RPM_OUT_OF_RANGE 0.1 ///< Maximum allowed RPM out of range from target RPM in open loop percent. +#define MAX_ALLOWED_OPEN_LOOP_RPM_OUT_OF_RANGE 100 ///< Maximum allowed RPM out of range from target RPM in open loop. #define OPEN_LOOP_RPM_OUT_OF_RANGE_TIME_OUT ( 5 * MS_PER_SECOND ) ///< Open loop RPM out of range time out in ms. #define DRAIN_PUMP_ENABLE_SPI3_PORT_MASK 0x00000020 ///< CS5 - Out put GPIO for pump enable. @@ -111,10 +112,9 @@ static BOOL hasClosedLoopBeenRequested = FALSE; ///< Closed loop pump control flag. static U32 currentDrainPumpRPM = 0; ///< Current drain pump RPM from feedback. -/* TODO These variables are used for POST. POST will be implemented later -static DRAIN_PUMP_SELF_TEST_STATE_T drainPumpSelfTestState = DRAIN_PUMP_SELF_TEST_STATE_START; ///< Current drain pump self test state. -static U32 drainPumpSelfTestTimerCount = 0; ///< Timer counter for drain pump self test. -*/ +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). /// ADC to RPM conversion coefficient or RPM to ADC conversion. static const F32 CONVERSION_COEFF = SEC_PER_MIN / ( 2 * TOGGLE_PERIOD_RESOLUTION_SECONDS * ROTATIONAL_TO_TOGGLE_PERIOD_CONVERSION ); @@ -170,8 +170,7 @@ 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); - + drainPumpDAC = (U32)GET_RPM_2_DAC_CONVERSION(rpm); targetDrainPumpRPM = rpm; drainPumpControlMode = PUMP_CONTROL_MODE_OPEN_LOOP; drainPumpControlModeSet = drainPumpControlMode; @@ -187,8 +186,38 @@ /*********************************************************************//** * @brief - * The setDrainPumpTargetOutletPressure function sets the target drain pump - * outlet pressure. + * The setDrainPumpTargetRPMDelayed function sets a new target RPM for the + * drain pump with delayed start. + * @details Inputs: drainPumpDAC, targetDrainPumpSpeed, drainPumpControlMode, + * drainPumpControlModeSet + * @details Outputs: pendingDrainPumpCmd, pendingDrainPumpCmdTarget, pendingDrainPumpCmdCountDown + * @param rpm new drain pump target RPM + * @param delayMs delay duration (in ms) before drain pump started + * @return TRUE if new target RPM is set, FALSE if not + *************************************************************************/ +BOOL setDrainPumpTargetRPMDelayed( U32 rpm, U32 delayMs ) +{ + BOOL result = FALSE; + + if ( ( 0 == rpm ) || ( ( rpm >= MIN_DRAIN_PUMP_RPM ) && ( rpm <= MAX_DRAIN_PUMP_RPM ) ) ) + { + pendingDrainPumpCmd = DRAIN_PUMP_OPEN_LOOP_STATE; + pendingDrainPumpCmdTarget = (F32)rpm; + pendingDrainPumpCmdCountDown = delayMs / TASK_GENERAL_INTERVAL; + result = TRUE; + } + else + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_DRAIN_PUMP_INVALID_RPM_SELECTED, rpm ) + } + + return result; +} + +/*********************************************************************//** + * @brief + * The setDrainPumpTargetOutletPressure function sets the drain pump to start + * with given target PDr pressure. * @details Inputs: targetDrainPumpDeltaPressure, hasClosedLoopBeenRequested, * drainPumpDAC, drainPumpControlMode, drainPumpControlModeSet * @details Outputs: targetDrainPumpDeltaPressure, hasClosedLoopBeenRequested, @@ -221,6 +250,38 @@ /*********************************************************************//** * @brief + * The setDrainPumpTargetOutletPressureDelayed function sets the drain pump + * to start with given target PDr pressure after given delay. + * @details Inputs: targetDrainPumpDeltaPressure, hasClosedLoopBeenRequested, + * drainPumpDAC, drainPumpControlMode, drainPumpControlModeSet + * @details Outputs: targetDrainPumpDeltaPressure, hasClosedLoopBeenRequested, + * drainPumpDAC, drainPumpControlMode, drainPumpControlModeSet + * @param pressure new target drain pump outlet pressure + * @param delayMS delay duration (in ms) before drain pump started + * @return TRUE if new target speed is set, FALSE if not + *************************************************************************/ +BOOL setDrainPumpTargetOutletPressureDelayed( F32 pressure, U32 delayMs ) +{ + BOOL result = FALSE; + + // Check the delta pressure is in range + if ( ( pressure >= MIN_ALLOWED_TARGET_OUTLET_PRESSURE ) && ( pressure <= MAX_ALLOWED_TARGET_OUTLET_PRESSURE ) ) + { + pendingDrainPumpCmd = DRAIN_PUMP_CONTROL_TO_TARGET_STATE; + pendingDrainPumpCmdTarget = pressure; + pendingDrainPumpCmdCountDown = delayMs / TASK_GENERAL_INTERVAL; + result = TRUE; + } + else + { + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_DRAIN_PUMP_INVALID_DELTA_PRESSURE_SELECTED, pressure ) + } + + return result; +} + +/*********************************************************************//** + * @brief * The signalDrainPumpHardStop function stops the Drain pump immediately. * @details Inputs: targetDrainPumpSpeed, drainPumpState, drainPumpControlMode, * hasClosedLoopBeenRequested, drainControlTimerCounter @@ -263,27 +324,22 @@ currentDrainPumpRPM = CONVERSION_COEFF / getFPGADrainPumpSpeed(); } - /* TODO: The RPM is not converted properly. There will be a story to work on this issue. - * This part of code is commented out until the RPM is calculated from ADC correctly. - * There will be a story to address the RPM conversion. - */ #ifndef IGNORE_DRAIN_PUMP_MONITOR // 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 ) + if( PUMP_CONTROL_MODE_OPEN_LOOP == drainPumpControlModeSet ) { U32 targetRPM = getTargetDrainPumpRPM(); - F32 threshold = OPEN_LOOP_RPM_OUT_OF_RANGE * targetRPM; // Check if RPM is out of range. Using fabs since the read RPM can be above or below the target. - BOOL isRPMOutOfRange = fabs( targetRPM - currentDrainPumpRPM ) > threshold; + BOOL isRPMOutOfRange = fabs( targetRPM - currentDrainPumpRPM ) > MAX_ALLOWED_OPEN_LOOP_RPM_OUT_OF_RANGE; - checkPersistentAlarm( ALARM_ID_DRAIN_PUMP_RPM_OUT_OF_RANGE, isRPMOutOfRange, currentDrainPumpRPM, threshold ); + checkPersistentAlarm( ALARM_ID_DRAIN_PUMP_RPM_OUT_OF_RANGE, isRPMOutOfRange, currentDrainPumpRPM, MAX_ALLOWED_OPEN_LOOP_RPM_OUT_OF_RANGE ); } // Check if the pump is in off state and the RPM is greater than the minimum RPM - if ( drainPumpState == DRAIN_PUMP_OFF_STATE ) + if ( DRAIN_PUMP_OFF_STATE == drainPumpState ) { BOOL isRPMTooHigh = currentDrainPumpRPM > MIN_DRAIN_PUMP_RPM; @@ -293,7 +349,6 @@ if ( isAlarmActive( ALARM_ID_DRAIN_PUMP_OFF_FAULT ) ) { activateSafetyShutdown(); - BOOL test = FALSE; } } #endif @@ -311,6 +366,33 @@ *************************************************************************/ void execDrainPumpController( void ) { + // Handle pending drain pump command + if ( pendingDrainPumpCmdCountDown > 0 ) + { + pendingDrainPumpCmdCountDown--; + if ( 0 == pendingDrainPumpCmdCountDown ) + { + if ( DRAIN_PUMP_CONTROL_TO_TARGET_STATE == pendingDrainPumpCmd ) + { + targetDrainPumpOutletPressure = pendingDrainPumpCmdTarget; + hasClosedLoopBeenRequested = TRUE; + drainPumpDAC = DRAIN_PUMP_MIN_DAC; + drainPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; + drainPumpControlModeSet = drainPumpControlMode; + } + else if ( DRAIN_PUMP_OPEN_LOOP_STATE == pendingDrainPumpCmd ) + { + drainPumpDAC = (U32)( pendingDrainPumpCmdTarget * RPM_2_DAC_SLOPE - RPM_2_DAC_INTERCEPT + FLOAT_TO_INT_ROUNDUP_OFFSET ); + targetDrainPumpRPM = (U32)pendingDrainPumpCmdTarget; + drainPumpControlMode = PUMP_CONTROL_MODE_OPEN_LOOP; + drainPumpControlModeSet = drainPumpControlMode; + } + pendingDrainPumpCmdTarget = 0.0; + pendingDrainPumpCmd = DRAIN_PUMP_OFF_STATE; + } + } + + // Execute drain pump state machine switch ( drainPumpState ) { case DRAIN_PUMP_OFF_STATE: @@ -335,23 +417,6 @@ /*********************************************************************//** * @brief - * The execDrainPumpSelfTest function executes the state machine for the drain - * pump self-test. - * @details Inputs: TODO FILL UP - * @details Outputs: TODO FILL UP - * @return the current state of the Drain Pump self test. - *************************************************************************/ -SELF_TEST_STATUS_T execDrainPumpSelfTest( void ) -{ - SELF_TEST_STATUS_T result = SELF_TEST_STATUS_FAILED; - - // TODO - implement self-test(s) - - return result; -} - -/*********************************************************************//** - * @brief * The getTargetDrainPumpRPM function gets the current target drain pump * RPM. * @details Inputs: targetDrainPumpRPM @@ -378,6 +443,18 @@ /*********************************************************************//** * @brief + * The isDrainPumpOn function determines whether the drain pump is on. + * @details Inputs: drainPumpDACSet + * @details Outputs: none + * @return TRUE if drain pump is on, FALSE if not. + *************************************************************************/ +BOOL isDrainPumpOn( void ) +{ + return ( drainPumpDACSet > 0 ? TRUE : FALSE ); +} + +/*********************************************************************//** + * @brief * The handleDrainPumpOffState function handles the drain pump off state of * the drain pump controller state machine. * @details Inputs: drainPumpControlModeSet, drainPumpDACSet, drainPumpDAC @@ -391,7 +468,7 @@ // 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 ) + if ( ( getTargetDrainPumpRPM() > 0 ) && ( PUMP_CONTROL_MODE_OPEN_LOOP == drainPumpControlModeSet ) ) { // Set drain pump enable pin SET_DRAIN_PUMP_ENABLE(); @@ -404,7 +481,7 @@ } // 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 ) + else if ( ( PUMP_CONTROL_MODE_CLOSED_LOOP == drainPumpControlModeSet ) && ( TRUE == hasClosedLoopBeenRequested ) ) { // set drain pump enable pin SET_DRAIN_PUMP_ENABLE();