Index: firmware/App/Drivers/PeristalticPump.c =================================================================== diff -u -r87d705fcf977af12b7b034735fa5867f2daea2b9 -r395522dffef1348e176564925656012f529c1910 --- firmware/App/Drivers/PeristalticPump.c (.../PeristalticPump.c) (revision 87d705fcf977af12b7b034735fa5867f2daea2b9) +++ firmware/App/Drivers/PeristalticPump.c (.../PeristalticPump.c) (revision 395522dffef1348e176564925656012f529c1910) @@ -1,17 +1,17 @@ /************************************************************************** * -* Copyright (c) 2024-2024 Diality Inc. - All Rights Reserved. +* Copyright (c) 2024-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 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,13 +27,30 @@ // ********** private definitions ********** -#define MAX_PUMP_SPEED_RPM 5000U ///< 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. +#define H4_PERIOD_SEC 0.00001F ///< Blood pump feedback period in seconds. +#define H4_HZ_TO_RPM_SCALAR 10.0F ///< Blood pump Hz to RPM scalar. +#define H4_PERIOD_TO_HZ(p) ( 1.0F / (F32)((S32)(p) * H4_PERIOD_SEC) ) ///< Blood pump period to Hz conversion macro. +#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_S32_T pumpMeasSpeedRPM; ///< Latest measured pump speed (in RPM). -static BOOL pumpHomeRequested; ///< Flag indicates a pump home operation has been requested. +static S32 pumpSetSpeedRPM; ///< Current set speed for the pump (in RPM). Negative indicates reverse direction. +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 ********** @@ -47,17 +64,15 @@ *************************************************************************/ void initPeristalticPumpDriver(void) { - pumpHomeRequested = FALSE; - pumpSetSpeedRPM = 0; - pumpMeasSpeedRPM.data = 0; - pumpMeasSpeedRPM.ovData = 0; - pumpMeasSpeedRPM.ovInitData = 0; - pumpMeasSpeedRPM.override = OVERRIDE_RESET; + pumpHomeInProgress = FALSE; + pumpHomeLowSignalSeen = FALSE; + pumpSetSpeedRPM = 0; + pumpMeasSpeedRPM = 0.0F; + pumpMeasRotSpeedRPM = 0.0F; - setBPDirection( MOTOR_DIR_FORWARD ); - resetBPHomeRequest(); - setBPSetSpeed( 0 ); - setBPEnabled( TRUE ); + setH4Direction( MOTOR_DIR_FORWARD ); + setH4SetSpeed( 0 ); + setH4Enabled( TRUE ); } /*********************************************************************//** @@ -66,45 +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 ) { - // update measured pump speed - if ( pumpSetSpeedRPM < 0 ) + U16 period = getH4Period(); + S16 rotPer = getH4RotorCount(); + + // update measured pump motor speed + if ( period != H4_ZERO_SPEED_PERIOD ) { - pumpMeasSpeedRPM.data = (S32)( (S16)getBPSpeed() * -1 ); + 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 = rpm * -1.0F; + } + else + { + pumpMeasSpeedRPM = rpm; + } } else { - pumpMeasSpeedRPM.data = (S32)( (S16)getBPSpeed() ); + pumpMeasSpeedRPM = 0.0F; } - // clear home command if previously requested - if ( TRUE == pumpHomeRequested ) + // update measured pump rotor speed + if ( rotPer != 0 ) { - pumpHomeRequested = FALSE; - resetBPHomeRequest(); + 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; - homeBP(); + 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 @@ -120,16 +213,17 @@ if ( abs( rpm ) < MAX_PUMP_SPEED_RPM ) { - U16 setRpm = (U16)( abs( rpm ) ); + F32 calRpm = H4_PUMP_CMD_CAL( (F32)rpm ); + U16 setRpm = (U16)( abs( (S32)calRpm ) ); MOTOR_DIR_T dir = MOTOR_DIR_FORWARD; if ( rpm < 0 ) { dir = MOTOR_DIR_REVERSE; } pumpSetSpeedRPM = rpm; - setBPDirection( dir ); - setBPSetSpeed( setRpm ); + setH4Direction( dir ); + setH4SetSpeed( setRpm ); result = TRUE; } else @@ -142,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. @@ -163,52 +270,23 @@ * @details \b Outputs: none * @return Latest measured pump speed in RPM. *************************************************************************/ -S32 getPeristalticPumpMeasSpeed( void ) +F32 getPeristalticPumpMeasSpeed( void ) { - S32 result = pumpMeasSpeedRPM.data; - - if ( OVERRIDE_KEY == pumpMeasSpeedRPM.override ) - { - result = pumpMeasSpeedRPM.ovData; - } - - return result; + return pumpMeasSpeedRPM; } - -/************************************************************************* - * TEST SUPPORT FUNCTIONS - *************************************************************************/ - - /*********************************************************************//** * @brief - * The testSetPeristalticPumpSetSpeed function sets the pump to a given speed - * (RPM). A negative speed indicates reverse (CCW) direction. - * @details \b Inputs: none - * @details \b Outputs: pumpSetSpeedRPM - * @param message set message from Dialin which includes the speed (RPM) to set - * the peristaltic pump to. - * @return TRUE if set request is successful, FALSE if not + * 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. *************************************************************************/ -BOOL testSetPeristalticPumpSetSpeed( MESSAGE_T *message ) +F32 getPeristalticPumpMeasRotorSpeed( void ) { - BOOL result = FALSE; - - // Verify tester has logged in with TD and override type is valid - if ( TRUE == isTestingActivated() ) - { - // Verify payload length is valid - if ( sizeof( S32 ) == message->hdr.payloadLen ) - { - S32 speed; - - memcpy( &speed, message->payload, sizeof(S32) ); - result = setPeristalticPumpSetSpeed( (S16)speed ); - } - } - - return result; + return pumpMeasRotSpeedRPM; } /**@}*/