Index: firmware/App/Controllers/RinsePump.c =================================================================== diff -u -r711ff90b0aab45c4acde333f4c9f11be9b62f383 -r94789b2f2324d5901685b6ff7b6224d4af3a0276 --- firmware/App/Controllers/RinsePump.c (.../RinsePump.c) (revision 711ff90b0aab45c4acde333f4c9f11be9b62f383) +++ firmware/App/Controllers/RinsePump.c (.../RinsePump.c) (revision 94789b2f2324d5901685b6ff7b6224d4af3a0276) @@ -1,24 +1,26 @@ /************************************************************************** * -* Copyright (c) 2024-2025 Diality Inc. - All Rights Reserved. +* Copyright (c) 2025-2026 Diality Inc. - All Rights Reserved. * * THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN * WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER. * * @file RinsePump.c * * @author (last) Vinayakam Mani -* @date (last) 22-Oct-2025 +* @date (last) 14-Apr-2026 * * @author (original) Vinayakam Mani -* @date (original) 22-Oct-2025 +* @date (original) 02-Oct-2025 * ***************************************************************************/ -#include "RinsePump.h" +#include "AlarmMgmtDD.h" +#include "FpgaDD.h" #include "Messaging.h" #include "OperationModes.h" #include "PersistentAlarm.h" +#include "RinsePump.h" #include "TaskGeneral.h" #include "Timers.h" #include "Valves.h" @@ -30,21 +32,41 @@ // ********** private definitions ********** -#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_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 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 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 OVERRIDE_U32_T rinsePumpDataPublishInterval; ///< Rinse pump data broadcast interval (in ms). +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 (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 monitorRinsePumpSpeed( void ); static void publishRinsePumpData( void ); +static void setRinsePumpPwmCount( RINSE_PUMP_ID_T pumpId, U32 pwmCount ); /*********************************************************************//** * @brief @@ -62,6 +84,14 @@ rinsePumpDataPublishInterval.ovData = RINSE_PUMP_DATA_PUB_INTERVAL; rinsePumpDataPublishInterval.ovInitData = RINSE_PUMP_DATA_PUB_INTERVAL; rinsePumpDataPublishInterval.override = OVERRIDE_RESET; + rinsePumpPwmPercentage.data = RINSE_PUMP_DEFAULT_PWM_PERCENT; + rinsePumpPwmPercentage.ovData = RINSE_PUMP_DEFAULT_PWM_PERCENT; + 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 ); } /*********************************************************************//** @@ -99,6 +129,76 @@ /*********************************************************************//** * @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 + * @details \b Outputs: rinsePumpMeasuredSpeed + * @return none. + *************************************************************************/ +void calculateRinsePumpSpeed( void ) +{ + // Pulse width in 10us resolution + U16 pumpPulseWidth = getFPGAD79RinsePumpPulseWidth(); + U32 pumpSpeedPerSec = 0; + + 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 @@ -109,10 +209,6 @@ { switch( currentRinsePumpState ) { - case RINSE_PUMP_STATE_INIT: - currentRinsePumpState = handleRinsePumpStartState(); - break; - case RINSE_PUMP_STATE_OFF: currentRinsePumpState = handleRinsePumpOffState(); break; @@ -126,21 +222,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 ); } /*********************************************************************//** @@ -154,10 +272,19 @@ { RINSE_PUMP_STATE_T state = RINSE_PUMP_STATE_OFF; - // Current alpha system uses valve D65 to turn on the rinse pump - // and its just On/off control for now. - setValveState( D79_PMP_VALV, VALVE_STATE_CLOSED ); + rinsePumpTargetSpeedRPM = 0; + if ( TRUE == getTestConfigStatus( TEST_CONFIG_DD_FP_ENABLE_BETA_2_0_HW ) ) + { + // Set PWM count zero to stop the logical rinse pump + setRinsePumpPwmCount( D79_RINSE_PUMP, RINSE_PUMP_OFF_COUNT ); + } + else + { + // Current Beta 1.9 system uses on/off bit + setValveState( D88_79_VALV, VALVE_STATE_CLOSED ); + } + return state; } @@ -172,14 +299,51 @@ { RINSE_PUMP_STATE_T state = RINSE_PUMP_STATE_ON; - // D79 Pump On/off control - setValveState( D79_PMP_VALV, VALVE_STATE_OPEN ); + if ( TRUE == getTestConfigStatus( TEST_CONFIG_DD_FP_ENABLE_BETA_2_0_HW ) ) + { + 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 ); + 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 + { + // Current Beat 1.9 system uses on/off bit + setValveState( D88_79_VALV, VALVE_STATE_OPEN ); + } + return state; } /*********************************************************************//** * @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 @@ -193,7 +357,9 @@ { RINSE_PUMP_PAYLOAD_T data; - data.d79State = getRinsePumpState(); + data.d79State = getRinsePumpState(); + data.d79PumpPWM = getF32OverrideValue( &rinsePumpPwmPercentage ); + data.d79PumpRPM = rinsePumpMeasuredSpeed; broadcastData( MSG_ID_DD_RINSE_PUMP_DATA, COMM_BUFFER_OUT_CAN_DD_BROADCAST, (U08*)&data, sizeof( RINSE_PUMP_PAYLOAD_T ) ); rinsePumpDataPublicationTimerCounter = 0; @@ -223,5 +389,61 @@ return result; } -/**@}*/ +/*********************************************************************//** + * @brief + * The testRinsePumpPWMPercentOverride function sets the override value + * of the rinse pump PWM percentage. + * @details Inputs: rinsePumpPwmPercentage (0 to 100%) + * @details Outputs: rinsePumpPwmPercentage + * @param message Override message from Dialin which includes the override + * value of the rinse pump PWM percentage. + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +BOOL testRinsePumpPWMPercentOverride( MESSAGE_T *message ) +{ + BOOL result = f32Override( message, &rinsePumpPwmPercentage ); + return result; +} + +/*********************************************************************//** + * @brief + * The testRinsePumpTurnOnOffRequest function sets the rinse pump status to + * On or Off. + * @details Inputs: none + * @details Outputs: none + * @param message Override message from Dialin which includes the rinse pump + * value of being 0 (Off) or 1 (On) + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +BOOL testRinsePumpTurnOnOffRequest( MESSAGE_T *message ) +{ + BOOL result = FALSE; + + if ( TRUE == isTestingActivated() ) + { + if ( message->hdr.payloadLen == sizeof( U32 ) ) + { + U32 rinsePumpOnOff; + RINSE_PUMP_STATE_T rinsePumpState; + + U08* payloadPtr = message->payload; + + memcpy( &rinsePumpOnOff, payloadPtr, sizeof( U32 ) ); + + if ( ( OFF == rinsePumpOnOff ) || ( ON == rinsePumpOnOff ) ) + { + // Only On or Off requests are accepted + rinsePumpState = ( OFF == rinsePumpOnOff ? RINSE_PUMP_STATE_OFF : RINSE_PUMP_STATE_ON ); + + setRinsePumpState( rinsePumpState ); + + result = TRUE; + } + } + } + + return result; +} + +/**@}*/