Index: firmware/App/Drivers/PeristalticPump.c =================================================================== diff -u -reffc2a8283d6528109b2401bbad70dc554db4e53 -re24a98a344ba13ceb0663b415268a7e1dd5ce99e --- firmware/App/Drivers/PeristalticPump.c (.../PeristalticPump.c) (revision effc2a8283d6528109b2401bbad70dc554db4e53) +++ firmware/App/Drivers/PeristalticPump.c (.../PeristalticPump.c) (revision e24a98a344ba13ceb0663b415268a7e1dd5ce99e) @@ -1,17 +1,17 @@ /************************************************************************** * -* Copyright (c) 2024-2024 Diality Inc. - All Rights Reserved. +* Copyright (c) 2024-2025 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 PeristalticPump.c * -* @author (last) Sean -* @date (last) 03-Oct-2024 +* @author (last) Dara Navaei +* @date (last) 25-Sep-2025 * -* @author (original) Sean -* @date (original) 03-Oct-2024 +* @author (original) Sean Nash +* @date (original) 24-Oct-2024 * ***************************************************************************/ @@ -27,7 +27,6 @@ // ********** private definitions ********** -#define MAX_PUMP_SPEED_RPM 3080U ///< Maximum speed (in RPM) that a peristaltic pump can be set to. #define H4_PUMP_CMD_GAIN 0.9F ///< Gain for blood pump commanded RPM that must be applied before sending to FPGA. #define H4_PUMP_CMD_OFFSET 80.0F ///< Offset for blood pump commanded RPM that must be applied before sending to FPGA. #define H4_PUMP_CMD_CAL(r) ((r) * H4_PUMP_CMD_GAIN + H4_PUMP_CMD_OFFSET ) ///< Macro to calibrate a commanded blood pump RPM before sending to FPGA. @@ -38,11 +37,20 @@ #define H4_HZ_TO_RPM(h) ((h) * H4_HZ_TO_RPM_SCALAR) ///< Blood pump Hz to RPM conversion macro. #define H4_ZERO_SPEED_PERIOD 0xFFFF ///< Blood pump period reported when pump is stopped. +#define H4_ROT_GEAR_RATIO 64.0F ///< Blood pump rotor to motor gear ratio. +#define H4_ROT_PERIOD_SEC 0.001F ///< Blood pump rotor feedback period in seconds. +#define H4_ROT_PERIOD_TO_HZ(p) ( 1.0F / (F32)((S32)(p) * H4_ROT_PERIOD_SEC) ) ///< Blood pump rotor period to Hz conversion macro. +#define H4_ROT_PERIOD_TO_RPM(p) ((H4_ROT_PERIOD_TO_HZ(p)) * (F32)SEC_PER_MIN) ///< Blood pump rotor period to RPM conversion macro. + +#define H6_STATUS_HOME_BIT_POS 0x1 ///< Bit position in H6 status register that indicates whether rotor is in home position. + // ********** private data ********** static S32 pumpSetSpeedRPM; ///< Current set speed for the pump (in RPM). Negative indicates reverse direction. -static OVERRIDE_F32_T pumpMeasSpeedRPM; ///< Latest measured pump speed (in RPM). -static BOOL pumpHomeRequested; ///< Flag indicates a pump home operation has been requested. +static F32 pumpMeasSpeedRPM; ///< Latest measured pump speed (in RPM). +static F32 pumpMeasRotSpeedRPM; ///< Latest measured pump rotor speed (in RPM). +static BOOL pumpHomeInProgress; ///< Flag indicates a pump home operation is in progress. +static BOOL pumpHomeLowSignalSeen; ///< Flag indicates a pump home operation has seen a "not home" signal. // ********** private function prototypes ********** @@ -56,15 +64,13 @@ *************************************************************************/ void initPeristalticPumpDriver(void) { - pumpHomeRequested = FALSE; - pumpSetSpeedRPM = 0; - pumpMeasSpeedRPM.data = 0.0F; - pumpMeasSpeedRPM.ovData = 0.0F; - pumpMeasSpeedRPM.ovInitData = 0.0F; - pumpMeasSpeedRPM.override = OVERRIDE_RESET; + pumpHomeInProgress = FALSE; + pumpHomeLowSignalSeen = FALSE; + pumpSetSpeedRPM = 0; + pumpMeasSpeedRPM = 0.0F; + pumpMeasRotSpeedRPM = 0.0F; setH4Direction( MOTOR_DIR_FORWARD ); - resetH4HomeRequest(); setH4SetSpeed( 0 ); setH4Enabled( TRUE ); } @@ -75,56 +81,123 @@ * peristaltic pump from the FPGA. * @note This function should be called periodically to maintain fresh * sensor readings for the pump. - * @details \b Inputs: pumpSetSpeedRPM, pumpHomeRequested, FPGA - * @details \b Outputs: pumpMeasSpeedRPM, pumpHomeRequested + * @details \b Inputs: pumpSetSpeedRPM, pumpHomeInProgress, FPGA + * @details \b Outputs: pumpMeasSpeedRPM, pumpHomeInProgress * @return none *************************************************************************/ void readPeristalticPumps( void ) { U16 period = getH4Period(); - F32 Hz = H4_PERIOD_TO_HZ(period); - F32 rpm = H4_HZ_TO_RPM(Hz); + S16 rotPer = getH4RotorCount(); - // update measured pump speed + // update measured pump motor speed if ( period != H4_ZERO_SPEED_PERIOD ) { + F32 Hz = H4_PERIOD_TO_HZ( period ); + F32 rpm = H4_HZ_TO_RPM( Hz ); + if ( pumpSetSpeedRPM < 0 ) // TODO - can we get a real measured direction to base measured speed sign on? { - pumpMeasSpeedRPM.data = rpm * -1.0F; + pumpMeasSpeedRPM = rpm * -1.0F; } else { - pumpMeasSpeedRPM.data = rpm; + pumpMeasSpeedRPM = rpm; } } else { - pumpMeasSpeedRPM.data = 0.0F; + pumpMeasSpeedRPM = 0.0F; } - // clear home command if previously requested - if ( TRUE == pumpHomeRequested ) + // update measured pump rotor speed + if ( rotPer != 0 ) { - pumpHomeRequested = FALSE; - resetH4HomeRequest(); + pumpMeasRotSpeedRPM = H4_ROT_PERIOD_TO_RPM( rotPer ); } + else + { + pumpMeasRotSpeedRPM = 0.0F; + } + + // monitor home command status if in progress, stop pump if rotor is in home position + if ( ( TRUE == pumpHomeInProgress ) && ( TRUE == isPeristalticPumpHome() ) && ( TRUE == pumpHomeLowSignalSeen ) ) + { + setPeristalticPumpSetSpeed( 0 ); + pumpHomeInProgress = FALSE; + } + else if ( isPeristalticPumpHome() != TRUE ) + { + pumpHomeLowSignalSeen = TRUE; + } } /*********************************************************************//** * @brief * The cmdPeristalticPumpHome function initiates a pump home operation. * @details \b Inputs: none - * @details \b Outputs: pumpHomeRequested, FPGA + * @details \b Outputs: pumpHomeInProgress, FPGA + * @return TRUE if home operation is initiated, FALSE if not + *************************************************************************/ +BOOL cmdPeristalticPumpHome( void ) +{ + BOOL result = FALSE; + + // Pump must be stopped before homing + if ( ( 0 == pumpSetSpeedRPM ) && ( pumpHomeInProgress != TRUE ) ) + { + result = TRUE; + pumpHomeInProgress = TRUE; + pumpHomeLowSignalSeen = ( isPeristalticPumpHome() != TRUE ? TRUE : FALSE ); + setPeristalticPumpSetSpeed( H4_HOME_SPEED_RPM ); + } + + return result; +} + +/*********************************************************************//** + * @brief + * The cancelPeristalticPumpHome function cancels a pump home operation. + * @details \b Inputs: none + * @details \b Outputs: pumpHomeInProgress * @return none *************************************************************************/ -void cmdPeristalticPumpHome( void ) +void cancelPeristalticPumpHome( void ) { - pumpHomeRequested = TRUE; - homeH4(); + pumpHomeInProgress = FALSE; } /*********************************************************************//** * @brief + * The isPeristalticPumpHome function determines whether the rotor is currently + * in the home position. + * @details \b Inputs: FPGA + * @details \b Outputs: none + * @return none + *************************************************************************/ +BOOL isPeristalticPumpHome( void ) +{ + U08 status = getH6RotorStatus() & H6_STATUS_HOME_BIT_POS; + BOOL result = ( status != 0 ? TRUE : FALSE ); + + return result; +} + +/*********************************************************************//** + * @brief + * The isPumpHomeInProgress function determines whether a home operation is + * currently in progress. + * @details \b Inputs: pumpHomeInProgress + * @details \b Outputs: none + * @return pumpHomeInProgress + *************************************************************************/ +BOOL isPumpHomeInProgress( void ) +{ + return pumpHomeInProgress; +} + +/*********************************************************************//** + * @brief * The setPeristalticPumpSetSpeed function sets the target pump speed (in RPM). * @note A negative set speed indicates reverse (CCW) direction. * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given set speed is out @@ -163,6 +236,19 @@ /*********************************************************************//** * @brief + * The setPeristalticPumpHardStop function stops the pump and holds the torque + * of the pump. + * @details \b Inputs: none + * @details \b Outputs: none + * @return none + *************************************************************************/ +void setPeristalticPumpHardStop( void ) +{ + setH4BrakeAndHoldTorque(); +} + +/*********************************************************************//** + * @brief * The getPeristalticPumpSetSpeed function gets the current pump set * speed in RPM. * @note A negative speed indicates reverse direction. @@ -186,14 +272,21 @@ *************************************************************************/ F32 getPeristalticPumpMeasSpeed( void ) { - F32 result = pumpMeasSpeedRPM.data; + return pumpMeasSpeedRPM; +} - if ( OVERRIDE_KEY == pumpMeasSpeedRPM.override ) - { - result = pumpMeasSpeedRPM.ovData; - } - - return result; +/*********************************************************************//** + * @brief + * The getPeristalticPumpMeasRotorSpeed function gets the latest measured pump + * rotor speed in RPM. + * @note A negative speed indicates reverse direction. + * @details \b Inputs: pumpMeasRotSpeedRPM + * @details \b Outputs: none + * @return Latest measured pump rotor speed in RPM. + *************************************************************************/ +F32 getPeristalticPumpMeasRotorSpeed( void ) +{ + return pumpMeasRotSpeedRPM; } /**@}*/