Index: firmware/App/Controllers/RinsePump.c =================================================================== diff -u -r2554891fdb4109192b94f711b140ab68c902e90e -r60db0506b1a90ed00b0e83159f6e7510ab6e4b7b --- firmware/App/Controllers/RinsePump.c (.../RinsePump.c) (revision 2554891fdb4109192b94f711b140ab68c902e90e) +++ firmware/App/Controllers/RinsePump.c (.../RinsePump.c) (revision 60db0506b1a90ed00b0e83159f6e7510ab6e4b7b) @@ -7,14 +7,15 @@ * * @file RinsePump.c * -* @author (last) “Raghu -* @date (last) 04-Feb-2026 +* @author (last) Jashwant Gantyada +* @date (last) 20-Feb-2026 * * @author (original) Vinayakam Mani * @date (original) 02-Oct-2025 * ***************************************************************************/ +#include "AlarmMgmtDD.h" #include "FpgaDD.h" #include "Messaging.h" #include "OperationModes.h" @@ -34,29 +35,37 @@ #define RINSE_PUMP_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) ///< Rinse pump data publish interval. #define DATA_PUBLISH_COUNTER_START_COUNT 13 ///< Rinse pump data publish start counter. #define RINSE_PUMP_OFF_COUNT 0U ///< Rinse Pump OFF value in count -#define RINSE_PUMP_DEFAULT_PWM_PERCENT 10.0F ///< Initial Rinse pump PWM percentage +#define RINSE_PUMP_DEFAULT_PWM_PERCENT 45.0F ///< Initial Rinse pump PWM percentage #define RINSE_PUMP_MAX_PWM_PERCENT 100.0F ///< Max Rinse pump PWM percentage -#define RINSE_PUMP_PWM_IN_COUNT_MAX 255.0F ///< Rinse pump max count (100% PWM = 255) +#define RINSE_PUMP_PWM_IN_COUNT_MAX 250.0F ///< Rinse pump max count (100% PWM = 250) #define RINSE_PUMP_PWM_PULSE_RESOLUTION_US 10 ///< Rinse pump PWM pulse resolution in 10us // TODO remove once PMW control is implemented #define RINSE_PUMP_TURN_OFF_CONTROL_BIT 0xFE ///< Rinse pump turn off control bit. #define RINSE_PUMP_TURN_ON_CONTROL_BIT 0xFF ///< Rinse pump turn on control bit. +#define RINSE_PUMP_PULSE_WIDTH_INVALID_MIN 0U ///< Invalid pulse width minimum (no rotation). +#define RINSE_PUMP_PULSE_WIDTH_INVALID_MAX 0xFFFFU ///< Invalid pulse width maximum (sensor fault). +//TODO: to be tested once the Beta 2 units are out +#define RINSE_PUMP_SPD_OUT_OF_RANGE_TOL_PCT 0.10F ///< Rinse pump commanded vs measured speed tolerance (10%). +#define RINSE_PUMP_SPEED_OUT_PERSISTENCE_MS 500 ///< Persistence time (ms) before declaring speed-out-of-range alarm. +#define RINSE_PUMP_MAX_RPM_ESTIMATE 3000U ///< Estimated max rinse pump RPM (for PWM-to-target conversion). + // ********** private data ********** static RINSE_PUMP_STATE_T currentRinsePumpState; ///< Current rinse pump control state. static U32 rinsePumpDataPublicationTimerCounter; ///< Rinse pump data broadcast timer counter. -static U32 rinsePumpMeasuredSpeed; ///< Rinse pump measured speed +static U32 rinsePumpMeasuredSpeed; ///< Rinse pump measured speed (RPM) +static U32 rinsePumpTargetSpeedRPM; ///< Rinse pump commanded target speed (RPM) for monitor static OVERRIDE_F32_T rinsePumpPwmPercentage; ///< Rinse pump PWM percentage. static OVERRIDE_U32_T rinsePumpDataPublishInterval; ///< Rinse pump data broadcast interval (in ms). // ********** private function prototypes ********** -static RINSE_PUMP_STATE_T handleRinsePumpStartState( void ); static RINSE_PUMP_STATE_T handleRinsePumpOffState( void ); static RINSE_PUMP_STATE_T handleRinsePumpOnState ( void ); static void publishRinsePumpData( void ); +static void setRinsePumpPwmCount( RINSE_PUMP_ID_T pumpId, U32 pwmCount ); /*********************************************************************//** * @brief @@ -79,6 +88,9 @@ rinsePumpPwmPercentage.ovInitData = RINSE_PUMP_DEFAULT_PWM_PERCENT; rinsePumpPwmPercentage.override = OVERRIDE_RESET; rinsePumpMeasuredSpeed = 0; + rinsePumpTargetSpeedRPM = 0; + + initPersistentAlarm( ALARM_ID_DD_D79_RINSE_PUMP_SPEED_OUT_OF_RANGE, 0, RINSE_PUMP_SPEED_OUT_PERSISTENCE_MS ); } /*********************************************************************//** @@ -116,6 +128,32 @@ /*********************************************************************//** * @brief + * The getRinsePumpMeasuredSpeed function returns the rinse pump measured + * speed from FPGA feedback. + * @details \b Inputs: rinsePumpMeasuredSpeed + * @details \b Outputs: none + * @return Measured rinse pump speed in RPM + *************************************************************************/ +U32 getRinsePumpMeasuredSpeed( void ) +{ + return rinsePumpMeasuredSpeed; +} + +/*********************************************************************//** + * @brief + * The getRinsePumpTargetSpeedRPM function returns the rinse pump commanded + * target speed for monitor tolerance checking. + * @details \b Inputs: rinsePumpTargetSpeedRPM + * @details \b Outputs: none + * @return Target rinse pump speed in RPM + *************************************************************************/ +U32 getRinsePumpTargetSpeedRPM( void ) +{ + return rinsePumpTargetSpeedRPM; +} + +/*********************************************************************//** + * @brief * The calculateRinsePumpSpeed function calculate rinse pump speed based * on FPGA report pulse width value. * @details \b Inputs: fpgaD79PumpSpeed @@ -126,14 +164,40 @@ { // Pulse width in 10us resolution U16 pumpPulseWidth = getFPGAD79RinsePumpPulseWidth(); - U32 pumpSpeedPerSec = US_PER_SECOND / ( pumpPulseWidth * RINSE_PUMP_PWM_PULSE_RESOLUTION_US ); + U32 pumpSpeedPerSec = 0; - //Speed in RPM - rinsePumpMeasuredSpeed = pumpSpeedPerSec * SEC_PER_MIN; + if ( ( RINSE_PUMP_PULSE_WIDTH_INVALID_MIN != pumpPulseWidth ) && ( RINSE_PUMP_PULSE_WIDTH_INVALID_MAX != pumpPulseWidth ) ) + { + pumpSpeedPerSec = US_PER_SECOND / ( pumpPulseWidth * RINSE_PUMP_PWM_PULSE_RESOLUTION_US ); + + // Speed in RPM + rinsePumpMeasuredSpeed = pumpSpeedPerSec * SEC_PER_MIN; + } + else + { + rinsePumpMeasuredSpeed = 0; + } } /*********************************************************************//** * @brief + * The execRinsePumpMonitor function executes the rinse pump monitor which + * calculates the rinse pump speed and monitors the rinse pump speed. + * @details \b Inputs: none + * @details \b Outputs: none + * @return none + *************************************************************************/ +void execRinsePumpMonitor( void ) +{ + // Calculate rinse pump speed + calculateRinsePumpSpeed(); + + // Monitor pump speed + monitorRinsePumpSpeed(); +} + +/*********************************************************************//** + * @brief * The execRinsePumpController function executes the rinse pump state machine. * @details \b Alarm: ALARM_ID_DD_SOFTWARE_FAULT if the current state is invalid. * @details \b Inputs: currentRinsePumpState @@ -144,10 +208,6 @@ { switch( currentRinsePumpState ) { - case RINSE_PUMP_STATE_INIT: - currentRinsePumpState = handleRinsePumpStartState(); - break; - case RINSE_PUMP_STATE_OFF: currentRinsePumpState = handleRinsePumpOffState(); break; @@ -161,21 +221,43 @@ break; } + // Publish rinse pump data publishRinsePumpData(); } /*********************************************************************//** * @brief - * The handleRinsePumpStartState function starts the rinse pump state machine. + * The monitorRinsePumpSpeed function monitors the rinse pump speed and + * triggers the alarm via the persistent alarm API when out of range for + * RINSE_PUMP_SPEED_OUT_PERSISTENCE_MS. * @details \b Inputs: none * @details \b Outputs: none - * @return next state of the rinse pump state machine + * @details \b Alarm: ALARM_ID_DD_D79_RINSE_PUMP_SPEED_OUT_OF_RANGE when + * commanded vs measured speed deviates beyond tolerance for RINSE_PUMP_SPEED_OUT_PERSISTENCE_MS. + * @return none *************************************************************************/ -static RINSE_PUMP_STATE_T handleRinsePumpStartState( void ) +static void monitorRinsePumpSpeed( void ) { - RINSE_PUMP_STATE_T state = RINSE_PUMP_STATE_OFF; + F32 rpTargetSpeed = (F32)rinsePumpTargetSpeedRPM; + F32 rpMeasSpeed = (F32)rinsePumpMeasuredSpeed; + F32 rpError = 0.0; + BOOL isRpSpeedOut = FALSE; + F32 tolerance = RINSE_PUMP_SPD_OUT_OF_RANGE_TOL_PCT; - return state; + if ( RINSE_PUMP_STATE_ON == currentRinsePumpState ) + { + rpError = fabs( rpMeasSpeed - rpTargetSpeed ); + + if ( rpTargetSpeed > 0.0 ) + { + rpError = rpError / rpTargetSpeed; + } + + isRpSpeedOut = ( rpError > tolerance ? TRUE : FALSE ); + } + + //TODO: to be tested once the Beta 2 units are out + checkPersistentAlarm( ALARM_ID_DD_D79_RINSE_PUMP_SPEED_OUT_OF_RANGE, isRpSpeedOut, (F32)rinsePumpMeasuredSpeed, (F32)rinsePumpTargetSpeedRPM ); } /*********************************************************************//** @@ -189,10 +271,12 @@ { RINSE_PUMP_STATE_T state = RINSE_PUMP_STATE_OFF; - if ( TRUE == getTestConfigStatus( TEST_CONFIG_DD_ENABLE_D79_PWM_CONTROL ) ) + rinsePumpTargetSpeedRPM = 0; + + if ( TRUE == getTestConfigStatus( TEST_CONFIG_DD_FP_ENABLE_BETA_2_0_HW ) ) { - // Set PWM count zero to stop the pump - setFPGAD79RinsePumpPWMControl( RINSE_PUMP_OFF_COUNT ); + // Set PWM count zero to stop the logical rinse pump + setRinsePumpPwmCount( D79_RINSE_PUMP, RINSE_PUMP_OFF_COUNT ); } else { @@ -214,13 +298,15 @@ { RINSE_PUMP_STATE_T state = RINSE_PUMP_STATE_ON; - if ( TRUE == getTestConfigStatus( TEST_CONFIG_DD_ENABLE_D79_PWM_CONTROL ) ) + if ( TRUE == getTestConfigStatus( TEST_CONFIG_DD_FP_ENABLE_BETA_2_0_HW ) ) { - F32 pwmPercent = getF32OverrideValue( &rinsePumpPwmPercentage ); - U32 pwmInCount = (U32)( ( pwmPercent / RINSE_PUMP_MAX_PWM_PERCENT ) * RINSE_PUMP_PWM_IN_COUNT_MAX ); + F32 pwmPercent = getF32OverrideValue( &rinsePumpPwmPercentage ); // TODO should this conversion happen in setPWMCount function? + U32 pwmInCount = (U32)( ( pwmPercent / RINSE_PUMP_MAX_PWM_PERCENT ) * RINSE_PUMP_PWM_IN_COUNT_MAX ); - //Turn on Rinse pump with given PWM value - setFPGAD79RinsePumpPWMControl( pwmInCount ); + rinsePumpTargetSpeedRPM = (U32)( ( (F32)pwmInCount / RINSE_PUMP_PWM_IN_COUNT_MAX ) * (F32)RINSE_PUMP_MAX_RPM_ESTIMATE ); + + // Turn on logical rinse pump with given PWM value + setRinsePumpPwmCount( D79_RINSE_PUMP, (U32)pwmInCount ); } else { @@ -233,6 +319,30 @@ /*********************************************************************//** * @brief + * The setRinsePumpPwmCount function maps a logical rinse pump ID to the + * underlying FPGA control and applies the requested PWM count. + * @details \b Inputs: pumpId, pwmCount + * @details \b Outputs: FPGA rinse pump control register + * @param pumpId Logical rinse pump identifier + * @param pwmCount PWM magnitude (0..RINSE_PUMP_PWM_IN_COUNT_MAX) + * @return none + *************************************************************************/ +static void setRinsePumpPwmCount( RINSE_PUMP_ID_T pumpId, U32 pwmCount ) +{ + switch ( pumpId ) + { + case D79_RINSE_PUMP: + setFPGAD79RinsePumpPWMControl( (U08)pwmCount ); + break; + + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_INVALID_RINSE_PUMP, pumpId ); + break; + } +} + +/*********************************************************************//** + * @brief * The publishRinsePumpData function constructs and sends the rinse pump data * broadcast message. * @details \b Message \b Sent: MSG_ID_DD_RINSE_PUMP_DATA @@ -336,4 +446,3 @@ } /**@}*/ -