Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -rf8e78fc4b4f0506d7f5274f64294724b4cf44603 -rbfbe825bef1c8544c37c20bbeaf61c0c27356096 --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision f8e78fc4b4f0506d7f5274f64294724b4cf44603) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision bfbe825bef1c8544c37c20bbeaf61c0c27356096) @@ -7,8 +7,8 @@ * * @file BloodFlow.c * -* @author (last) Sean Nash -* @date (last) 01-May-2026 +* @author (last) Praneeth Bunne +* @date (last) 27-May-2026 * * @author (original) Sean Nash * @date (original) 24-Oct-2024 @@ -94,6 +94,9 @@ #define BP_TORQUE_PERIOD_RESOLUTION_US 1.0F ///< Blood pump torque period resolution in microseconds (1 us). #define BP_1KHZ_TO_TORQUE_CONVERSION_MNM 10.0F ///< Blood pump 1kHz to torque conversion in milli-newtonmeter +#define BP_ZERO_SPEED_THRESHOLD_RPM 0.01F ///< Blood pump threshold in rpm +#define BP_ZERO_FLOW_THRESHOLD_ML_MIN 0.01F ///< Blood pump zero flow threshold in ml. + /// Enumeration of blood pump controller states. typedef enum BloodPump_States { @@ -306,7 +309,13 @@ { resetBloodPumpRPMMovingAverage(); targetBloodFlowRate = dirFlowRate; - bloodPumpDirectionSet = dir; + + // This is for Preserving the current direction while ramping down to a stop. A zero request does not represent a new direction. + if ( flowRate != 0 ) + { + bloodPumpDirectionSet = dir; + } + bloodPumpControlMode = mode; bloodPumpRampToSpeedRPM = BP_RPM_FROM_RATE( flowRate ); @@ -513,8 +522,24 @@ filterBloodPumpRPMReadings( getPeristalticPumpMeasSpeed() ); bloodPumpSpeedRPM.data = filteredBloodPumpSpeed; + + // Remove small floating point residuals when the pump is stopped. + if ( fabs( bloodPumpSpeedRPM.data ) < BP_ZERO_SPEED_THRESHOLD_RPM ) + { + bloodPumpSpeedRPM.data = 0.0F; + } bloodPumpDirection = ( getMeasuredBloodPumpSpeed() < 0.0F ? MOTOR_DIR_REVERSE : MOTOR_DIR_FORWARD ); + + // Update calculated blood-flow rate. measuredBloodFlowRate.data = calcBloodFlow(); + + // Remove small floating-point residuals from calculated flow. + if ( fabs( measuredBloodFlowRate.data ) < BP_ZERO_FLOW_THRESHOLD_ML_MIN ) + { + measuredBloodFlowRate.data = 0.0F; + } + + // Update measured torque. bloodPumpTorquemNm.data = calcBloodPumpTorque(); // Do not start enforcing checks until out of init/POST mode TODO-add checks later @@ -533,6 +558,7 @@ // } } + /*********************************************************************//** * @brief * The execBloodFlowController function executes the blood flow controller. @@ -617,7 +643,7 @@ { isBloodPumpOn = FALSE; bloodPumpSetSpeedRPM = 0; - setPeristalticPumpSetSpeed( bloodPumpSetSpeedRPM ); + setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); } return result; @@ -626,8 +652,7 @@ /*********************************************************************//** * @brief * The handleBloodPumpHomingState function handles the blood pump homing state - * of the blood pump controller state machine. - * @details \b Inputs: bpHomeStartTime + * of the blood pump controller state machine. * @details \b Inputs: bpHomeStartTime * @details \b Outputs: Blood pump stopped if complete or timed out * @return next state *************************************************************************/ @@ -915,7 +940,7 @@ payload.h4MeasRotorSpd = getMeasuredBloodPumpRotorSpeed(); payload.h4MeasPumpSpd = getMeasuredBloodPumpSpeed(); payload.h4MeasTorquemNm = getMeasuredBloodPumpTorque(); - payload.h4SetRPM = bloodPumpSetSpeedRPM; + payload.h4SetRPM = BP_SIGNED_SET_SPEED; payload.h4RotorCount = getBloodPumpRotorCount(); if ( ( MODE_PRET == opMode ) || ( MODE_TREA == opMode ) || ( MODE_POST == opMode ) ) { // prescribed flow only available in treatment modes @@ -1027,12 +1052,28 @@ *************************************************************************/ static F32 calcBloodPumpTorque( void ) { - F32 torqueResolutionUS = (F32)getH4TorqueCount() * BP_TORQUE_PERIOD_RESOLUTION_US; - F32 torqueResolutionS = torqueResolutionUS / (F32)US_PER_SECOND; - F32 frequencyHz = 1.0F / torqueResolutionS; - F32 frequencyKHz = frequencyHz / (F32)HZ_PER_KHZ; - F32 torquemNm = frequencyKHz * BP_1KHZ_TO_TORQUE_CONVERSION_MNM; + S16 torqueCount = getH4TorqueCount(); + F32 torquemNm; + if ( torqueCount <= 0 ) + { + torquemNm = 0.0F; + } + else + { + F32 torqueResolutionUS = (F32)torqueCount * BP_TORQUE_PERIOD_RESOLUTION_US; + F32 torqueResolutionS = torqueResolutionUS / (F32)US_PER_SECOND; + F32 frequencyHz = 1.0F / torqueResolutionS; + F32 frequencyKHz = frequencyHz / (F32)HZ_PER_KHZ; + torquemNm = frequencyKHz * BP_1KHZ_TO_TORQUE_CONVERSION_MNM; + + // This FPGA torque count provides torque magnitude only and applies the measured H4 speed direction to the reported torque. + if( getPeristalticPumpMeasSpeed() < 0.0F ) + { + torquemNm *= -1.0F; + } + } + return torquemNm; }