/************************************************************************** * * Copyright (c) 2024-2024 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 BloodFlow.c * * @author (last) Sean * @date (last) 25-Oct-2024 * * @author (original) Sean * @date (original) 25-Oct-2024 * ***************************************************************************/ #include // Used for fabs() functions #include "BloodFlow.h" #include "CpldInterface.h" #include "FpgaTD.h" #include "Messaging.h" #include "ModeTxParams.h" #include "OperationModes.h" #include "PersistentAlarm.h" #include "PIControllers.h" #include "Pressures.h" #include "TaskGeneral.h" #include "Timers.h" #include "Utilities.h" /** * @addtogroup BloodFlow * @{ */ // ********** private definitions ********** /// Interval (ms/task time) at which the blood flow data is published on the CAN bus. #define BLOOD_FLOW_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) #define MAX_SETTABLE_BLOOD_FLOW_RATE 700 ///< Maximum settable blood flow rate (in mL/min). #define BP_CONTROL_INTERVAL_SEC 2 ///< Blood pump control interval (in seconds). /// Interval (ms/task time) at which the blood pump is controlled. static const U32 BP_CONTROL_INTERVAL = ( BP_CONTROL_INTERVAL_SEC * MS_PER_SECOND / TASK_GENERAL_INTERVAL ); #define BP_P_COEFFICIENT 0.5F ///< P term for blood pump control #define BP_I_COEFFICIENT 1.5F ///< I term for blood pump control #define BP_HOME_SPEED 400 ///< Target pump speed (in RPM) for homing. #define BP_HOME_TIMEOUT_MS ( 12 * MS_PER_SECOND ) ///< Maximum time (in ms) allowed for homing to complete. #define BP_MAX_ROTOR_HALL_INTERVAL_MS ( 20 * MS_PER_SECOND ) ///< Maximum time (in ms) allowed between rotor hall sensor detects (50 mL/min worst case). #define BP_MAX_ROTOR_SPEED_RPM 100.0F ///< Maximum rotor speed allowed for blood pump. #define BP_GEAR_RATIO 43.5F ///< BP motor to rotor gear ratio. #define BP_MAX_FLOW_RATE 1320.0F ///< Maximum measured BP flow rate alarm threshold. #define BP_MIN_FLOW_RATE -1320.0F ///< Minimum measured BP flow rate alarm threshold. #define BP_MAX_MOTOR_SPEED_WHILE_OFF_RPM 100.0F ///< Maximum motor speed (RPM) while motor is commanded off. #define BP_MAX_ROTOR_VS_MOTOR_DIFF_RPM 5.0F ///< Maximum difference in speed between motor and rotor (in rotor RPM). #define BP_MAX_MOTOR_SPEED_ERROR_RPM 300.0F ///< Maximum difference in speed between measured and commanded RPM. #define BP_MAX_MOTOR_SPEED_VS_TRGT_DIFF_PCT 0.15F ///< Maximum motor speed vs target difference in percent. /// Persist time (in ms) for motor off error condition. static const U32 BP_OFF_ERROR_PERSIST = ( 5 * MS_PER_SECOND ); /// Persist time (in ms) motor speed error condition. static const U32 BP_MOTOR_SPEED_ERROR_PERSIST = ( 5 * MS_PER_SECOND ); /// Persist time (in ms) pump direction error condition. static const U32 BP_DIRECTION_ERROR_PERSIST = ( 250 ); /// Persist time period (in ms) blood pump rotor speed too fast error condition. static const U32 BP_MAX_ROTOR_SPEED_ERROR_PERSIST = ( 10 * MS_PER_SECOND ); /// Measured blood pump speed is filtered w/ 1 second moving average. #define SIZE_OF_BP_ROLLING_AVG ( ( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) * 1 ) #define BP_RAMP_STEP_SPEED_RPM 50 ///< Blood pump ramp step size (in RPM). #define BP_ML_PER_ROTOR_REV 13.75F ///< Blood pump volume (mL) per rotor revolution. #define BP_FLOW_ALPHA_Y_INTERCEPT 1.00F ///< Y intercept used for alpha flow coefficient calculation. #define BP_FLOW_WEAR_A_TERM 0.000000F ///< A term used for wear portion of alpha flow coefficient. #define BP_FLOW_WEAR_B_TERM 0.000000F ///< B term used for wear portion of alpha flow coefficient. #define BP_MAX_ROTOR_COUNT_FOR_WEAR 25000 ///< Maximum rotor count for determining wear of the cartridge (negligible affect beyond this threshold). #define BP_MIN_ART_PRESSURE_MMHG -200.0F ///< Minimum arterial pressure factored into blood flow calculation. #define BP_RATE_FROM_RPM( rpm ) ( rpm / ( BP_GEAR_RATIO / BP_ML_PER_ROTOR_REV ) ) ///< Macro to estimate a flow rate (mL/min) from a given speed (RPM). #define BP_RPM_FROM_RATE( rate ) ( rate * ( BP_GEAR_RATIO / BP_ML_PER_ROTOR_REV ) ) ///< Macro to estimate a pump speed (RPM) from a given flow rate (mL/min). #define DATA_PUBLISH_COUNTER_START_COUNT 20 ///< Data publish counter start count. #define SIZE_OF_ROLLING_AVG 20 ///< Number of pump speed samples in rolling average. #define BP_TORQUE_PERIOD_RESOLUTION_US 10.0F ///< Blood pump torque period resolution in microseconds (10 us). #define BP_1KHZ_TO_TORQUE_CONVERSION_MNM 10.0F ///< Blood pump 1kHz to torque conversion in milli-newtonmeter /// Enumeration of blood pump controller states. typedef enum BloodPump_States { BLOOD_PUMP_OFF_STATE = 0, ///< Blood pump off state BLOOD_PUMP_HOMING_STATE, ///< Blood pump homing state BLOOD_PUMP_RAMPING_UP_STATE, ///< Blood pump ramping up state BLOOD_PUMP_RAMPING_DN_STATE, ///< Blood pump ramping down state BLOOD_PUMP_CONTROL_TO_TARGET_STATE, ///< Blood pump controlling to target state NUM_OF_BLOOD_PUMP_STATES ///< Number of blood pump states } BLOOD_PUMP_STATE_T; /// Enumeration of blood pump self-test states. typedef enum BloodFlow_Self_Test_States { BLOOD_FLOW_SELF_TEST_STATE_START = 0, ///< Blood pump self-test start state BLOOD_FLOW_TEST_STATE_IN_PROGRESS, ///< Blood pump self-test in progress state BLOOD_FLOW_TEST_STATE_COMPLETE, ///< Blood pump self-test completed state NUM_OF_BLOOD_FLOW_SELF_TEST_STATES ///< Number of blood pump self-test states } BLOOD_FLOW_SELF_TEST_STATE_T; // ********** private data ********** static BLOOD_PUMP_STATE_T bloodPumpState; ///< Current state of blood pump controller state machine static U32 bloodFlowDataPublicationTimerCounter; ///< Used to schedule blood flow data publication to CAN bus static BOOL isBloodPumpOn; ///< Blood pump is currently running static MOTOR_DIR_T bloodPumpDirection; ///< Measured blood flow direction static MOTOR_DIR_T bloodPumpDirectionSet; ///< Set blood flow direction static PUMP_CONTROL_MODE_T bloodPumpControlMode; ///< Requested blood pump control mode. static U16 bloodPumpRampToSpeedRPM; ///< Estimated target speed that we want to ramp to. static U16 bloodPumpSetSpeedRPM; ///< Current set speed for the blood pump. /// Macro for determining the signed set speed for the blood pump based on currently set direction and unsigned set speed. #define BP_SIGNED_SET_SPEED ( MOTOR_DIR_REVERSE == bloodPumpDirectionSet ? (S32)bloodPumpSetSpeedRPM * -1 : (S32)bloodPumpSetSpeedRPM ) static OVERRIDE_U32_T bloodFlowDataPublishInterval; ///< Interval (in task intervals) at which to publish blood flow data to CAN bus. static S32 targetBloodFlowRate; ///< Requested blood flow rate. static OVERRIDE_F32_T measuredBloodFlowRate; ///< Measured (calculated now) blood flow rate. static OVERRIDE_F32_T bloodPumpRotorSpeedRPM; ///< Measured blood pump rotor speed. static OVERRIDE_F32_T bloodPumpSpeedRPM; ///< Measured blood pump motor speed. static OVERRIDE_F32_T bloodPumpTorquemNm; ///< Measured blood pump torque in mNm. static F32 rpmReadings[ SIZE_OF_ROLLING_AVG ]; ///< Holds RPM samples for a rolling average. static U32 rpmReadingsIdx; ///< Index for next sample in rolling average array. static F32 rpmReadingsTotal; ///< Rolling total - used to calc average. static U32 rpmReadingsCount; ///< Number of samples in RPM rolling average buffer. static F32 filteredBloodPumpSpeed; ///< Filtered blood pump speed used in blood flow estimation. static U32 bpControlTimerCounter; ///< Determines when to perform control on blood flow. static OVERRIDE_U32_T bloodPumpRotorCounter; ///< Running counter for blood pump rotor revolutions. static BOOL bpHomeRequested; ///< Flag indicating a home operation has been requested. static U32 bpHomeStartTime; ///< When did blood pump home command begin? (in ms). static OVERRIDE_F32_T bpFlowAlphaYIntercept; ///< Blood flow estimation term for alpha Y intercept. static OVERRIDE_F32_T bpFlowWearATerm; ///< Blood flow estimation term for wear slope. static OVERRIDE_F32_T bpFlowWearBTerm; ///< Blood flow estimation term for wear offset. //static TD_PUMPS_CAL_RECORD_T bloodPumpCalRecord; ///< Blood pump calibration record. // ********** private function prototypes ********** static BLOOD_PUMP_STATE_T handleBloodPumpOffState( void ); static BLOOD_PUMP_STATE_T handleBloodPumpHomingState( void ); static BLOOD_PUMP_STATE_T handleBloodPumpRampingUpState( void ); static BLOOD_PUMP_STATE_T handleBloodPumpRampingDownState( void ); static BLOOD_PUMP_STATE_T handleBloodPumpControlToTargetState( void ); static U32 getBloodPumpRotorCount( void ); static void checkBloodPumpRotor( void ); static void checkBloodPumpDirection( void ); static void checkBloodPumpSpeeds( void ); static F32 calcBloodFlow( void ); static F32 getBPFlowAlphaYIntercept( void ); static F32 getBPFlowWearATerm( void ); static F32 getBPFlowWearBTerm( void ); static void resetBloodPumpRPMMovingAverage( void ); static void filterBloodPumpRPMReadings( F32 rpm ); static F32 calcBloodPumpTorque( void ); static void publishBloodFlowData( void ); /*********************************************************************//** * @brief * The initBloodFlow function initializes the BloodFlow unit. * @details \b Inputs: none * @details \b Outputs: Blood flow unit initialized * @return none *************************************************************************/ void initBloodFlow( void ) { U32 i; // Initialize peristaltic pump driver initPeristalticPumpDriver(); // Initialize core variables bloodPumpState = BLOOD_PUMP_OFF_STATE; targetBloodFlowRate = 0; bloodPumpSetSpeedRPM = 0; bloodPumpRampToSpeedRPM = 0; isBloodPumpOn = FALSE; bloodPumpDirection = MOTOR_DIR_FORWARD; bloodPumpDirectionSet = MOTOR_DIR_FORWARD; bloodPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; bpControlTimerCounter = 0; bloodFlowDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; bpHomeRequested = FALSE; bpHomeStartTime = 0; // Initialize overrides bloodFlowDataPublishInterval.data = BLOOD_FLOW_DATA_PUB_INTERVAL; bloodFlowDataPublishInterval.ovData = BLOOD_FLOW_DATA_PUB_INTERVAL; bloodFlowDataPublishInterval.ovInitData = BLOOD_FLOW_DATA_PUB_INTERVAL; bloodFlowDataPublishInterval.override = OVERRIDE_RESET; measuredBloodFlowRate.data = 0.0F; measuredBloodFlowRate.ovData = 0.0F; measuredBloodFlowRate.ovInitData = 0.0F; measuredBloodFlowRate.override = OVERRIDE_RESET; bloodPumpRotorSpeedRPM.data = 0.0F; bloodPumpRotorSpeedRPM.ovData = 0.0F; bloodPumpRotorSpeedRPM.ovInitData = 0.0F; bloodPumpRotorSpeedRPM.override = OVERRIDE_RESET; bloodPumpSpeedRPM.data = 0.0F; bloodPumpSpeedRPM.ovData = 0.0F; bloodPumpSpeedRPM.ovInitData = 0.0F; bloodPumpSpeedRPM.override = OVERRIDE_RESET; bloodPumpRotorCounter.data = 0; bloodPumpRotorCounter.ovData = 0; bloodPumpRotorCounter.ovInitData = 0; bloodPumpRotorCounter.override = OVERRIDE_RESET; bloodPumpTorquemNm.data = 0.0F; bloodPumpTorquemNm.ovData = 0.0F; bloodPumpTorquemNm.ovInitData = 0.0F; bloodPumpTorquemNm.override = OVERRIDE_RESET; bpFlowAlphaYIntercept.data = BP_FLOW_ALPHA_Y_INTERCEPT; bpFlowAlphaYIntercept.ovData = BP_FLOW_ALPHA_Y_INTERCEPT; bpFlowAlphaYIntercept.ovInitData = BP_FLOW_ALPHA_Y_INTERCEPT; bpFlowAlphaYIntercept.override = OVERRIDE_RESET; bpFlowWearATerm.data = BP_FLOW_WEAR_A_TERM; bpFlowWearATerm.ovData = BP_FLOW_WEAR_A_TERM; bpFlowWearATerm.ovInitData = BP_FLOW_WEAR_A_TERM; bpFlowWearATerm.override = OVERRIDE_RESET; bpFlowWearBTerm.data = BP_FLOW_WEAR_B_TERM; bpFlowWearBTerm.ovData = BP_FLOW_WEAR_B_TERM; bpFlowWearBTerm.ovInitData = BP_FLOW_WEAR_B_TERM; bpFlowWearBTerm.override = OVERRIDE_RESET; // Initialize pump speed filter for ( i = 0; i < SIZE_OF_ROLLING_AVG; i++ ) { rpmReadings[ i ] = 0.0F; } resetBloodPumpRPMMovingAverage(); // Reset pump rotor count resetBloodPumpRotorCount(); // Initialize blood flow PI controller initializePIController( PI_CONTROLLER_ID_BLOOD_FLOW, 0.0F, BP_P_COEFFICIENT, BP_I_COEFFICIENT, 0.0F, (F32)MAX_PUMP_SPEED_RPM, FALSE, 0.0F); // Initialize persistent alarm for flow sensor // initPersistentAlarm( ALARM_ID_HD_BLOOD_PUMP_OFF_CHECK, 0, BP_OFF_ERROR_PERSIST ); // initPersistentAlarm( ALARM_ID_HD_BLOOD_PUMP_MOTOR_SPEED_CHECK, 0, BP_MOTOR_SPEED_ERROR_PERSIST ); // initPersistentAlarm( ALARM_ID_HD_BLOOD_PUMP_MC_DIRECTION_CHECK, 0, BP_DIRECTION_ERROR_PERSIST ); // initPersistentAlarm( ALARM_ID_HD_BLOOD_PUMP_ROTOR_SPEED_TOO_HIGH, 0, BP_MAX_ROTOR_SPEED_ERROR_PERSIST ); } /*********************************************************************//** * @brief * The setBloodPumpTargetFlowRate function sets a new target flow rate and * pump direction. * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if requested flow rate is too high. * @details \b Inputs: isBloodPumpOn, bloodPumpDirectionSet * @details \b Outputs: targetBloodFlowRate, bloodPumpdirection, bloodPumpPWMDutyCyclePct * @param flowRate new target blood flow rate * @param dir new blood flow direction * @param mode new control mode * @return TRUE if new flow rate & dir are set, FALSE if not *************************************************************************/ BOOL setBloodPumpTargetFlowRate( U32 flowRate, MOTOR_DIR_T dir, PUMP_CONTROL_MODE_T mode ) { BOOL result = FALSE; // Direction change while pump is running is not allowed if ( ( FALSE == isBloodPumpOn ) || ( 0 == flowRate ) || ( dir == bloodPumpDirectionSet ) ) { S32 prevFlowRate = targetBloodFlowRate; S32 dirFlowRate = ( dir == MOTOR_DIR_FORWARD ? (S32)flowRate : (S32)flowRate * -1 ); // Don't interrupt pump control unless rate or mode is changing if ( ( dirFlowRate != targetBloodFlowRate ) || ( mode != bloodPumpControlMode ) ) { BOOL isFlowInrange = ( flowRate <= MAX_SETTABLE_BLOOD_FLOW_RATE ? TRUE : FALSE ); #ifndef _RELEASE_ // if ( SW_CONFIG_ENABLE_VALUE == getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMPS_FLOW_LIMITS ) ) // { // isFlowInrange = TRUE; // } #endif // Verify flow rate valid if ( TRUE == isFlowInrange ) { resetBloodPumpRPMMovingAverage(); targetBloodFlowRate = dirFlowRate; bloodPumpDirectionSet = dir; bloodPumpControlMode = mode; bloodPumpRampToSpeedRPM = BP_RPM_FROM_RATE( flowRate ); if ( BLOOD_PUMP_CONTROL_TO_TARGET_STATE == bloodPumpState ) { // Start ramp to new target in appropriate direction if ( abs( targetBloodFlowRate ) > abs( prevFlowRate ) ) { bloodPumpState = BLOOD_PUMP_RAMPING_UP_STATE; } else { bloodPumpState = BLOOD_PUMP_RAMPING_DN_STATE; } } result = TRUE; } else // Requested flow rate too high { #ifndef _RELEASE_ // if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMPS_FLOW_LIMITS ) != SW_CONFIG_ENABLE_VALUE ) #endif { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_BLOOD_FLOW_SET_TOO_HIGH, flowRate ) } } } } return result; } /*********************************************************************//** * @brief * The setBloodPumpTargetRPM function sets a new target pump speed and pump * direction. Pump is set to open loop control. * @details \b Inputs: none * @details \b Outputs: none * @param rpm new target blood pump speed (in RPM) * @param dir new blood flow direction * @return TRUE if new flow rate & direction are set, FALSE if not *************************************************************************/ BOOL setBloodPumpTargetRPM( U32 rpm, MOTOR_DIR_T dir ) { BOOL result = FALSE; result = setBloodPumpTargetFlowRate( (U32)BP_RATE_FROM_RPM( rpm ), dir, PUMP_CONTROL_MODE_OPEN_LOOP ); return result; } /*********************************************************************//** * @brief * The calcBloodFlow function calculates an estimated blood flow rate from * blood pump speed, arterial pressure and tubing wear (measured from count * of rotor revolutions since cartridge install). * @details \b Inputs: bloodPumpRotorCounter, arterial pressure * @details \b Outputs: none * @return calculated blood flow rate (mL/min) *************************************************************************/ static F32 calcBloodFlow( void ) { F32 artPres = getLongFilteredArterialPressure(); F32 artPresL= ( artPres > BP_MIN_ART_PRESSURE_MMHG ? artPres : BP_MIN_ART_PRESSURE_MMHG ); F32 rotSpd = filteredBloodPumpSpeed / BP_GEAR_RATIO; U32 r = getBloodPumpRotorCount(); U32 rotCnt = CAP( r, BP_MAX_ROTOR_COUNT_FOR_WEAR ); F32 wear = getBPFlowWearATerm() * (F32)rotCnt + getBPFlowWearBTerm(); F32 alpha = wear * artPresL + getBPFlowAlphaYIntercept(); F32 flow = BP_ML_PER_ROTOR_REV * rotSpd * alpha; return flow; } /*********************************************************************//** * @brief * The signalBloodPumpHardStop function stops the blood pump immediately. * @details \b Inputs: none * @details \b Outputs: Blood pump stopped, set point reset, state changed to off * @return none *************************************************************************/ void signalBloodPumpHardStop( void ) { targetBloodFlowRate = 0; bloodPumpState = BLOOD_PUMP_OFF_STATE; bloodPumpSetSpeedRPM = 0; isBloodPumpOn = FALSE; bpControlTimerCounter = 0; setPeristalticPumpHardStop(); resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, 0.0F, 0.0F ); } /*********************************************************************//** * @brief * The homeBloodPump function initiates a blood pump home operation. * @details \b Inputs: bloodPumpState * @details \b Outputs: bpHomeRequested * @return TRUE if home request accepted, FALSE if not *************************************************************************/ BOOL homeBloodPump( void ) { if ( BLOOD_PUMP_OFF_STATE == bloodPumpState ) { bpHomeRequested = TRUE; } else { bpHomeRequested = FALSE; } return bpHomeRequested; } /*********************************************************************//** * @brief * The getBloodPumpRotorCount function returns the current count for the * blood pump rotor revolution counter. * @details \b Inputs: bloodPumpRotorCounter * @details \b Outputs: none * @return bloodPumpRotorCounter *************************************************************************/ static U32 getBloodPumpRotorCount( void ) { U32 result = bloodPumpRotorCounter.data; if ( OVERRIDE_KEY == bloodPumpRotorCounter.override ) { result = bloodPumpRotorCounter.ovData; } return result; } /*********************************************************************//** * @brief * The isBloodPumpRunning function returns whether the blood pump is currently * running or not. * @details \b Inputs: isBloodPumpOn * @details \b Outputs: none * @return isBloodPumpOn *************************************************************************/ BOOL isBloodPumpRunning( void ) { return isBloodPumpOn; } /*********************************************************************//** * @brief * The isBloodPumpRampComplete function returns whether the blood pump has * completed its ramp up and entered control state (closed or open loop). * @details \b Inputs: bloodPumpState * @details \b Outputs: none * @return TRUE if pump is in control state, FALSE if not *************************************************************************/ BOOL isBloodPumpRampComplete( void ) { BOOL result = ( BLOOD_PUMP_CONTROL_TO_TARGET_STATE == bloodPumpState ? TRUE : FALSE ); return result; } /*********************************************************************//** * @brief * The resetBloodPumpRotorCount function resets the blood pump rotor counter * that is a proxy for cartridge wear. Call this function after a new cartridge * has been installed. * @details \b Inputs: none * @details \b Outputs: bloodPumpRotorCounter * @return none *************************************************************************/ void resetBloodPumpRotorCount( void ) { bloodPumpRotorCounter.data = 0; // if ( TRUE == getTestConfigStatus( TEST_CONFIG_USE_WORN_CARTRIDGE ) ) { bloodPumpRotorCounter.data = BP_MAX_ROTOR_COUNT_FOR_WEAR; } } /*********************************************************************//** * @brief * The execBloodFlowMonitor function executes the blood flow monitor. * @details \b Inputs: none * @details \b Outputs: BP sensor readings updated, filteredBloodPumpSpeed, * bloodPumpSpeedRPM, bloodPumpDirection, measuredBloodFlowRate * @return none *************************************************************************/ void execBloodFlowMonitor( void ) { TD_OP_MODE_T opMode = getCurrentOperationMode(); // U16 bpRPM = getIntADCReading( INT_ADC_BLOOD_PUMP_SPEED ); // TODO - bring in current monitoring later // U16 bpmA = getIntADCReading( INT_ADC_BLOOD_PUMP_MOTOR_CURRENT ); // Update pump feedback from FPGA readPeristalticPumps(); // Update rotor RPM bloodPumpRotorSpeedRPM.data = getPeristalticPumpMeasRotorSpeed(); // adcBloodPumpMCSpeedRPM.data = (F32)(SIGN_FROM_12_BIT_VALUE(bpRPM)) * BP_SPEED_ADC_TO_RPM_FACTOR; // adcBloodPumpMCCurrentmA.data = (F32)(SIGN_FROM_12_BIT_VALUE(bpmA)) * BP_CURRENT_ADC_TO_MA_FACTOR; filterBloodPumpRPMReadings( getPeristalticPumpMeasSpeed() ); bloodPumpSpeedRPM.data = filteredBloodPumpSpeed; bloodPumpDirection = ( getMeasuredBloodPumpSpeed() < 0.0F ? MOTOR_DIR_REVERSE : MOTOR_DIR_FORWARD ); measuredBloodFlowRate.data = calcBloodFlow(); bloodPumpTorquemNm.data = calcBloodPumpTorque(); // Do not start enforcing checks until out of init/POST mode TODO-add checks later // if ( opMode != MODE_INIT ) // { // // Check pump direction // checkBloodPumpDirection(); // // Check pump speeds // checkBloodPumpSpeeds(); // // Check for home position, zero/low speed // checkBloodPumpRotor(); // } // else // { // lastBloodPumpDirectionCount = getFPGABloodPumpHallSensorStatus() & PUMP_DIR_ERROR_COUNT_MASK; // } } /*********************************************************************//** * @brief * The execBloodFlowController function executes the blood flow controller. * @details \b Alarms: ALARM_ID_TD_SOFTWARE_FAULT if current blood pump state * is invalid. * @details \b Inputs: bloodPumpState * @details \b Outputs: bloodPumpState * @return none *************************************************************************/ void execBloodFlowController( void ) { // Control blood pump switch ( bloodPumpState ) { case BLOOD_PUMP_OFF_STATE: bloodPumpState = handleBloodPumpOffState(); break; case BLOOD_PUMP_HOMING_STATE: bloodPumpState = handleBloodPumpHomingState(); break; case BLOOD_PUMP_RAMPING_UP_STATE: bloodPumpState = handleBloodPumpRampingUpState(); break; case BLOOD_PUMP_RAMPING_DN_STATE: bloodPumpState = handleBloodPumpRampingDownState(); break; case BLOOD_PUMP_CONTROL_TO_TARGET_STATE: bloodPumpState = handleBloodPumpControlToTargetState(); break; default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_BLOOD_FLOW_INVALID_BLOOD_PUMP_STATE, bloodPumpState ) break; } // Publish blood flow data on interval publishBloodFlowData(); } /*********************************************************************//** * @brief * The handleBloodPumpOffState function handles the blood pump off state * of the blood pump controller state machine. * @details \b Inputs: targetBloodFlowRate, bpHomeRequested * @details \b Outputs: bloodPumpSetSpeedRPM, isBloodPumpOn, bpHomeRequested, * bpHomeStartTime * @return next state *************************************************************************/ static BLOOD_PUMP_STATE_T handleBloodPumpOffState( void ) { BLOOD_PUMP_STATE_T result = BLOOD_PUMP_OFF_STATE; // If we have been given a flow rate, setup ramp up and transition to ramp up state if ( targetBloodFlowRate != 0 ) { // Start ramp up to target flow rate bloodPumpSetSpeedRPM = BP_RAMP_STEP_SPEED_RPM; setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); isBloodPumpOn = TRUE; result = BLOOD_PUMP_RAMPING_UP_STATE; } // If home requested, initiate homing operation else if ( TRUE == bpHomeRequested ) { BOOL homeOK = FALSE; bpHomeRequested = FALSE; bpHomeStartTime = getMSTimerCount(); homeOK = cmdPeristalticPumpHome(); if ( TRUE == homeOK ) { isBloodPumpOn = TRUE; bloodPumpSetSpeedRPM = H4_HOME_SPEED_RPM; result = BLOOD_PUMP_HOMING_STATE; } } else { isBloodPumpOn = FALSE; bloodPumpSetSpeedRPM = 0; setPeristalticPumpSetSpeed( bloodPumpSetSpeedRPM ); } return result; } /*********************************************************************//** * @brief * The handleBloodPumpHomingState function handles the blood pump homing state * 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 *************************************************************************/ static BLOOD_PUMP_STATE_T handleBloodPumpHomingState( void ) { BLOOD_PUMP_STATE_T result = BLOOD_PUMP_HOMING_STATE; if ( isPumpHomeInProgress() != TRUE ) { signalBloodPumpHardStop(); result = BLOOD_PUMP_OFF_STATE; } else if ( TRUE == didTimeout( bpHomeStartTime, BP_HOME_TIMEOUT_MS ) ) { cancelPeristalticPumpHome(); signalBloodPumpHardStop(); result = BLOOD_PUMP_OFF_STATE; } return result; } /*********************************************************************//** * @brief * The handleBloodPumpRampingUpState function handles the ramp up state * of the blood pump controller state machine. * @details \b Inputs: bloodPumpRampToSpeedRPM, bloodPumpSetSpeedRPM * @details \b Outputs: bloodPumpSetSpeedRPM * @return next state *************************************************************************/ static BLOOD_PUMP_STATE_T handleBloodPumpRampingUpState( void ) { BLOOD_PUMP_STATE_T result = BLOOD_PUMP_RAMPING_UP_STATE; // Have we been asked to stop the blood pump? if ( 0 == targetBloodFlowRate ) { // Start ramp down to stop bloodPumpSetSpeedRPM -= BP_RAMP_STEP_SPEED_RPM; setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); result = BLOOD_PUMP_RAMPING_DN_STATE; } // Have we reached end of ramp up? else if ( bloodPumpSetSpeedRPM >= bloodPumpRampToSpeedRPM ) { resetBloodPumpRPMMovingAverage(); bloodPumpSetSpeedRPM = bloodPumpRampToSpeedRPM; setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, (F32)bloodPumpSetSpeedRPM, 0.0F ); bpControlTimerCounter = 0; result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; } // Continue ramp up else { bloodPumpSetSpeedRPM += BP_RAMP_STEP_SPEED_RPM; bloodPumpSetSpeedRPM = MIN( bloodPumpSetSpeedRPM, bloodPumpRampToSpeedRPM ); setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); } return result; } /*********************************************************************//** * @brief * The handleBloodPumpRampingDownState function handles the ramp down state * of the blood pump controller state machine. * @details \b Inputs: bloodPumpRampToSpeedRPM, bloodPumpSetSpeedRPM * @details \b Outputs: bloodPumpSetSpeedRPM * @return next state *************************************************************************/ static BLOOD_PUMP_STATE_T handleBloodPumpRampingDownState( void ) { BLOOD_PUMP_STATE_T result = BLOOD_PUMP_RAMPING_DN_STATE; // Have we essentially reached zero speed? if ( bloodPumpSetSpeedRPM < BP_RAMP_STEP_SPEED_RPM ) { signalBloodPumpHardStop(); result = BLOOD_PUMP_OFF_STATE; } // Have we reached end of ramp down? else if ( bloodPumpSetSpeedRPM <= bloodPumpRampToSpeedRPM ) { resetBloodPumpRPMMovingAverage(); resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, (F32)bloodPumpSetSpeedRPM, 0.0F ); setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); bpControlTimerCounter = 0; result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; } // Continue ramp down else { bloodPumpSetSpeedRPM -= BP_RAMP_STEP_SPEED_RPM; bloodPumpSetSpeedRPM = MAX( bloodPumpSetSpeedRPM, bloodPumpRampToSpeedRPM ); setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); } return result; } /*********************************************************************//** * @brief * The handleBloodPumpControlToTargetState function handles the "control to * target" state of the blood pump controller state machine. * @details \b Inputs: bloodPumpControlMode * @details \b Outputs: bloodPumpState * @return next state *************************************************************************/ static BLOOD_PUMP_STATE_T handleBloodPumpControlToTargetState( void ) { BLOOD_PUMP_STATE_T result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; // Control at set interval if ( ++bpControlTimerCounter >= BP_CONTROL_INTERVAL ) { if ( bloodPumpControlMode == PUMP_CONTROL_MODE_CLOSED_LOOP ) { F32 tgtFlow = (F32)targetBloodFlowRate; F32 actFlow = getMeasuredBloodFlowRate(); F32 newRPM= runPIController( PI_CONTROLLER_ID_BLOOD_FLOW, tgtFlow, actFlow ); bloodPumpSetSpeedRPM = (U32)((S32)(newRPM)); setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); } bpControlTimerCounter = 0; } return result; } /*********************************************************************//** * @brief * The getTargetBloodFlowRate function gets the target blood flow rate. * @details \b Inputs: targetBloodFlowRate * @details \b Outputs: none * @return the current target blood flow rate (in mL/min). *************************************************************************/ S32 getTargetBloodFlowRate( void ) { return targetBloodFlowRate; } /*********************************************************************//** * @brief * The getMeasuredBloodFlowRate function gets the measured blood flow * rate. * @details \b Inputs: measuredBloodFlowRate * @details \b Outputs: none * @return the current blood flow rate (in mL/min). *************************************************************************/ F32 getMeasuredBloodFlowRate( void ) { F32 result = getF32OverrideValue( &measuredBloodFlowRate ); return result; } /*********************************************************************//** * @brief * The getMeasuredBloodPumpRotorSpeed function gets the measured blood flow * rate. * @details \b Inputs: bloodPumpRotorSpeedRPM * @details \b Outputs: none * @return the current blood flow rate (in mL/min). *************************************************************************/ F32 getMeasuredBloodPumpRotorSpeed( void ) { F32 result = getF32OverrideValue( &bloodPumpRotorSpeedRPM ); return result; } /*********************************************************************//** * @brief * The getMeasuredBloodPumpSpeed function gets the measured blood flow * rate. * @details \b Inputs: bloodPumpSpeedRPM * @details \b Outputs: none * @return the current blood flow rate (in mL/min). *************************************************************************/ F32 getMeasuredBloodPumpSpeed( void ) { F32 result = getF32OverrideValue( &bloodPumpSpeedRPM ); return result; } /*********************************************************************//** * @brief * The getBPFlowAlphaYIntercept function gets the blood pump flow alpha * intercept for blood flow estimation. * @details \b Inputs: bpFlowAlphaYIntercept * @details \b Outputs: none * @return the current blood flow alpha intercept term. *************************************************************************/ static F32 getBPFlowAlphaYIntercept( void ) { F32 result = getF32OverrideValue( &bpFlowAlphaYIntercept ); return result; } /*********************************************************************//** * @brief * The getBPFlowWearATerm function gets the blood pump flow estimation * A term for wear. * @details \b Inputs: bpFlowWearATerm * @details \b Outputs: none * @return the current blood flow A term for wear. *************************************************************************/ static F32 getBPFlowWearATerm( void ) { F32 result = getF32OverrideValue( &bpFlowWearATerm ); return result; } /*********************************************************************//** * @brief * The getBPFlowWearBTerm function gets the blood pump flow estimation * B term for wear. * @details \b Inputs: bpFlowWearBTerm * @details \b Outputs: none * @return the current blood flow B term for wear. *************************************************************************/ static F32 getBPFlowWearBTerm( void ) { F32 result = getF32OverrideValue( &bpFlowWearBTerm ); return result; } /*********************************************************************//** * @brief * The publishBloodFlowData function publishes blood flow data at the set * interval. * @details \b Message \b Sent: MSG_ID_TD_BLOOD_PUMP_DATA at set interval. * @details \b Inputs: target flow rate, measured flow rate, measured MC speed, * measured MC current * @details \b Outputs: Blood flow data is published to CAN bus. * @return none *************************************************************************/ static void publishBloodFlowData( void ) { // Publish blood flow data on interval if ( ++bloodFlowDataPublicationTimerCounter >= getU32OverrideValue( &bloodFlowDataPublishInterval ) ) { BLOOD_PUMP_STATUS_PAYLOAD_T payload; TD_OP_MODE_T opMode = getCurrentOperationMode(); U32 hallSensor = 0; // TODO gioGetBit( hetPORT1, BP_ROTOR_HALL_SENSOR_NHET_ID ); payload.h4SetFlowRate = targetBloodFlowRate; payload.h4MeasFlow = getMeasuredBloodFlowRate(); payload.h4MeasRotorSpd = getMeasuredBloodPumpRotorSpeed(); payload.h4MeasPumpSpd = getMeasuredBloodPumpSpeed(); payload.h4MeasCurr = 0.0F; // TODO getMeasuredBloodPumpMCCurrent(); payload.h4SetRPM = bloodPumpSetSpeedRPM; payload.h4RotorCount = getBloodPumpRotorCount(); if ( ( MODE_PRET == opMode ) || ( MODE_TREA == opMode ) || ( MODE_POST == opMode ) ) { // prescribed flow only available in treatment modes payload.h4PresFlow = getTreatmentParameterU32( TREATMENT_PARAM_BLOOD_FLOW ); } else { payload.h4PresFlow = 0; } payload.h6RotorHallState = ( hallSensor > 0 ? 0 : 1 ); // 1=home, 0=not home payload.h4MeasTorquemNm = getF32OverrideValue( &bloodPumpTorquemNm ); //payload.bPstate = bloodPumpState; //payload.bpRotorStatus = (U32)getH6RotorStatus(); broadcastData( MSG_ID_TD_BLOOD_PUMP_DATA, COMM_BUFFER_OUT_CAN_TD_BROADCAST, (U08*)&payload, sizeof( BLOOD_PUMP_STATUS_PAYLOAD_T ) ); bloodFlowDataPublicationTimerCounter = 0; } } /*********************************************************************//** * @brief * The resetBloodPumpRPMMovingAverage function re-initializes the pump speed * moving average sample buffer. * @details \b Inputs: none * @details \b Outputs: rpmReadingsTotal, rpmReadingsIdx, rpmReadingsCount all set to zero. * @return none *************************************************************************/ static void resetBloodPumpRPMMovingAverage( void ) { rpmReadingsIdx = 0; rpmReadingsCount = 0; rpmReadingsTotal = 0.0F; filteredBloodPumpSpeed = 0.0F; } /*********************************************************************//** * @brief * The filterBloodPumpRPMReadings function adds a new pump speed sample to * the filter. * @details \b Inputs: none * @details \b Outputs: rpmReadings[], rpmReadingsIdx, rpmReadingsCount, rpmReadingsTotal * @param rpm newest blood pump speed (in RPM) sample to add to filter * @return none *************************************************************************/ static void filterBloodPumpRPMReadings( F32 rpm ) { if ( rpmReadingsCount >= SIZE_OF_ROLLING_AVG ) { rpmReadingsTotal -= rpmReadings[ rpmReadingsIdx ]; } rpmReadings[ rpmReadingsIdx ] = rpm; rpmReadingsTotal += rpm; rpmReadingsIdx = INC_WRAP( rpmReadingsIdx, 0, SIZE_OF_ROLLING_AVG - 1 ); rpmReadingsCount = INC_CAP( rpmReadingsCount, SIZE_OF_ROLLING_AVG ); filteredBloodPumpSpeed = rpmReadingsTotal / (F32)rpmReadingsCount; } /*********************************************************************//** * @brief * The calcBloodPumpTorque function calculates the blood pump's torque from * the resolution count. * @details \b Inputs: none * @details \b Outputs: none * @return calculated blood pump torque (mN.m) *************************************************************************/ static F32 calcBloodPumpTorque( void ) { F32 torqueResolutionUS = (F32)getH4Period() * BP_TORQUE_PERIOD_RESOLUTION_US; F32 frequencyHz = 1.0F / ( (F32)US_PER_SECOND / torqueResolutionUS ); F32 frequencyKHz = (F32)HZ_PER_KHZ / frequencyHz; F32 torquemNm = frequencyKHz * BP_1KHZ_TO_TORQUE_CONVERSION_MNM; return torquemNm; } /*********************************************************************//** * @brief * The checkBloodPumpRotor function checks the rotor for the blood * pump. If pump is off or running very slowly, rotor speed will be set to zero. * @details \b Alarm: ALARM_ID_HD_BLOOD_PUMP_ROTOR_SPEED_TOO_HIGH if rotor speed * is too high. * @details \b Inputs: bpStopAtHomePosition, bpHomeStartTime, bpRotorRevStartTime, * bpRotorSpeedTooFastPulseCount * @details \b Outputs: pump may be stopped if homing, bloodPumpRotorSpeedRPM may be * set to zero if too long since last pulse. * @return none *************************************************************************/ static void checkBloodPumpRotor( void ) { // F32 rotorSpeed = getMeasuredBloodPumpRotorSpeed(); // // // Ensure rotor speed below maximum // if ( TRUE == isPersistentAlarmTriggered( ALARM_ID_HD_BLOOD_PUMP_ROTOR_SPEED_TOO_HIGH, bpRotorSpeedTooFastPulseCount >= BP_MIN_ROTOR_PULSES_FOR_MAX_SPEED ) ) // { // SET_ALARM_WITH_1_F32_DATA( ALARM_ID_HD_BLOOD_PUMP_ROTOR_SPEED_TOO_HIGH, rotorSpeed ) // } // // // If pump is stopped or running very slowly, set rotor speed to zero // if ( TRUE == didTimeout( bpRotorRevStartTime, BP_MAX_ROTOR_HALL_INTERVAL_MS ) ) // { // bloodPumpRotorSpeedRPM.data = 0.0; // } } /*********************************************************************//** * @brief * The checkBloodPumpDirection function checks the set direction vs. * the direction implied by the sign of the measured MC speed. * @details \b Alarm: * @details \b Inputs: * @details \b Outputs: * @return none *************************************************************************/ static void checkBloodPumpDirection( void ) { // if ( BLOOD_PUMP_CONTROL_TO_TARGET_STATE == bloodPumpState ) // { // MOTOR_DIR_T bpMCDir, bpDir; // BOOL isDirIncorrect; // U08 dirErrorCnt = getFPGABloodPumpHallSensorStatus() & PUMP_DIR_ERROR_COUNT_MASK; // F32 measMCSpeed = getMeasuredBloodPumpMCSpeed(); // BOOL minDirSpeed = ( fabs( measMCSpeed ) >= BP_MIN_DIR_CHECK_SPEED_RPM ? TRUE : FALSE ); // BOOL isHallSensorFailed = ( TRUE == minDirSpeed && lastBloodPumpDirectionCount != dirErrorCnt ? TRUE : FALSE ); // // // Check pump direction error count // if ( ( TRUE == isHallSensorFailed ) && ( TRUE == incTimeWindowedCount( TIME_WINDOWED_COUNT_BP_COMMUTATION_ERROR ) ) ) // { //#ifndef _RELEASE_ // if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMP_DIRECTION_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) //#endif // { // SET_ALARM_WITH_1_U32_DATA( ALARM_ID_TD_PUMP_DIRECTION_STATUS_ERROR, (U32)HD_PUMP_BLOOD_PUMP ) // } // } // lastBloodPumpDirectionCount = dirErrorCnt; // // bpMCDir = ( getMeasuredBloodPumpMCSpeed() >= 0.0 ? MOTOR_DIR_FORWARD : MOTOR_DIR_REVERSE ); // bpDir = ( getMeasuredBloodPumpSpeed() >= 0.0 ? MOTOR_DIR_FORWARD : MOTOR_DIR_REVERSE ); // // // Check set direction vs. direction from hall sensors vs. direction from sign of motor controller speed // isDirIncorrect = ( ( ( bloodPumpDirectionSet != bpDir ) || ( bloodPumpDirectionSet != bpMCDir ) ) && ( TRUE == minDirSpeed ) ); // if ( TRUE == isPersistentAlarmTriggered( ALARM_ID_HD_BLOOD_PUMP_MC_DIRECTION_CHECK, isDirIncorrect ) ) // { //#ifndef _RELEASE_ // if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMP_DIRECTION_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) //#endif // { // if ( bloodPumpDirectionSet != bpDir ) // { // SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_BLOOD_PUMP_MC_DIRECTION_CHECK, (U32)bloodPumpDirectionSet, (U32)bpDir ) // } // else // { // SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_BLOOD_PUMP_MC_DIRECTION_CHECK, (U32)bloodPumpDirectionSet, (U32)bpMCDir ) // } // } // } // } // else // { // resetPersistentAlarmTimer( ALARM_ID_HD_BLOOD_PUMP_MC_DIRECTION_CHECK ); // } } /*********************************************************************//** * @brief * The checkBloodPumpSpeeds function checks several aspects of the blood pump * speed. * 1. while pump is commanded off, measured motor speed should be < limit. * 2. while pump is controlling, measured motor speed should be within allowed range of commanded speed. * 3. measured motor speed should be within allowed range of measured rotor speed. * All 3 checks have a persistence time that must be met before an alarm is triggered. * @details \b Alarm: * @details \b Inputs: bloodPumpState, targetBloodFlowRate, bloodPumpPWMDutyCyclePctSet, bloodPumpDirectionSet * @details \b Outputs: errorBloodRotorSpeedPersistTimerCtr, alarm(s) may be triggered * @return none *************************************************************************/ static void checkBloodPumpSpeeds( void ) { // F32 measMotorSpeed = getMeasuredBloodPumpSpeed(); // F32 measMCMotorSpeed = getMeasuredBloodPumpMCSpeed(); // // // Check for pump running while commanded off and not trigger alarm when AC power lost condition // if ( ( TRUE == isPersistentAlarmTriggered( ALARM_ID_HD_BLOOD_PUMP_OFF_CHECK, // ( 0 == targetBloodFlowRate ) && ( fabs( measMotorSpeed ) > BP_MAX_MOTOR_SPEED_WHILE_OFF_RPM ) && ( TRUE != getCPLDACPowerLossDetected() ) ) ) ) // { //#ifndef _RELEASE_ // if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMP_SPEED_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) //#endif // { // SET_ALARM_WITH_1_F32_DATA( ALARM_ID_HD_BLOOD_PUMP_OFF_CHECK, measMotorSpeed ); // activateSafetyShutdown(); // } // } // // // Checks that only occur when pump is running (and beyond ramp). // if ( BLOOD_PUMP_CONTROL_TO_TARGET_STATE == bloodPumpState ) // { // F32 cmdMotorSpeed = BP_PWM_TO_MOTOR_SPEED_RPM( bloodPumpPWMDutyCyclePctSet, bloodPumpDirectionSet ); // F32 deltaMotorSpeed = fabs( measMotorSpeed - cmdMotorSpeed ); // F32 deltaMCMotorSpeed = fabs( measMCMotorSpeed - cmdMotorSpeed ); // F32 measRotorSpeed = fabs( getMeasuredBloodPumpRotorSpeed() ); // F32 measMotorSpeedInRotorRPM = fabs( measMotorSpeed / BP_GEAR_RATIO ); // F32 deltaRotorSpeed = fabs( measRotorSpeed - measMotorSpeedInRotorRPM ); // F32 measMotorSpeedDeltaPct = fabs( deltaRotorSpeed / measMotorSpeedInRotorRPM ); // // // Check measured motor speed vs. commanded motor speed while controlling to target // if ( ( TRUE == isPersistentAlarmTriggered( ALARM_ID_HD_BLOOD_PUMP_MOTOR_SPEED_CHECK, // ( deltaMotorSpeed > BP_MAX_MOTOR_SPEED_ERROR_RPM ) || ( deltaMCMotorSpeed > BP_MAX_MOTOR_SPEED_ERROR_RPM ) ) ) ) // { //#ifndef _RELEASE_ // if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMP_SPEED_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) //#endif // { // SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_BLOOD_PUMP_MOTOR_SPEED_CHECK, cmdMotorSpeed, measMotorSpeed ); // } // } // // // Check measured rotor speed vs. measured motor speed while controlling to target // if ( ( deltaRotorSpeed > BP_MAX_ROTOR_VS_MOTOR_DIFF_RPM ) && ( measMotorSpeedDeltaPct > BP_MAX_MOTOR_SPEED_VS_TRGT_DIFF_PCT ) ) // { // if ( ++errorBloodRotorSpeedPersistTimerCtr >= ( getPumpRotorErrorPersistTime( measMotorSpeed, BP_GEAR_RATIO ) / TASK_PRIORITY_INTERVAL ) ) // { //#ifndef _RELEASE_ // if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMP_SPEED_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) //#endif // { // SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_BLOOD_PUMP_ROTOR_SPEED_CHECK, measRotorSpeed, measMotorSpeed ); // } // } // } // else // { // errorBloodRotorSpeedPersistTimerCtr = 0; // } // } // else // { // resetPersistentAlarmTimer( ALARM_ID_HD_BLOOD_PUMP_MOTOR_SPEED_CHECK ); // errorBloodRotorSpeedPersistTimerCtr = 0; // // } } /*********************************************************************//** * @brief * The execBloodFlowTest function executes the state machine for the * BloodFlow self-test. * @details \b Inputs: bloodPumpCalRecord * @details \b Outputs: none * @return the current state of the BloodFlow self-test. *************************************************************************/ SELF_TEST_STATUS_T execBloodFlowTest( void ) { SELF_TEST_STATUS_T result = SELF_TEST_STATUS_PASSED; // BOOL calStatus = getNVRecord2Driver( GET_CAL_PUMPS, (U08*)&bloodPumpCalRecord, sizeof( HD_PUMPS_CAL_RECORD_T ), // NUM_OF_CAL_DATA_HD_PUMPS, ALARM_ID_NO_ALARM ); // // if ( TRUE == calStatus ) // { // result = SELF_TEST_STATUS_PASSED; // } // else // { // result = SELF_TEST_STATUS_FAILED; // } return result; } /************************************************************************* * TEST SUPPORT FUNCTIONS *************************************************************************/ /*********************************************************************//** * @brief * The testBloodFlowDataPublishIntervalOverride function overrides the * blood flow data publish interval. * @details \b Inputs: none * @details \b Outputs: bloodFlowDataPublishInterval * @param message Override message from Dialin which includes the interval * (in ms) to override the blood pump data broadcast interval to. * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testBloodFlowDataPublishIntervalOverride( MESSAGE_T *message ) { BOOL result = u32BroadcastIntervalOverride( message, &bloodFlowDataPublishInterval, TASK_GENERAL_INTERVAL ); return result; } /*********************************************************************//** * @brief * The testSetTargetBloodFlowRateOverride function commands the blood pump to * run at a given flow rate (mL/min) and in a given control mode. * @details \b Inputs: none * @details \b Outputs: Blood pump commanded to given rate w/ given control mode. * @param message set message from Dialin which includes the flow rate (mL/min) * and control mode to command the blood pump to. * @return TRUE if set command successful, FALSE if not *************************************************************************/ BOOL testSetTargetBloodFlowRateOverride( MESSAGE_T *message ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { if ( ( sizeof( S32 ) + sizeof( U32 ) ) == message->hdr.payloadLen ) { MOTOR_DIR_T dir; U08 *msgPayload = &message->payload[0]; S32 flow; U32 ctrlMode; memcpy( &flow, msgPayload, sizeof( S32 ) ); msgPayload += sizeof( S32 ); memcpy( &ctrlMode, msgPayload, sizeof( U32 ) ); if ( flow < 0 ) { dir = MOTOR_DIR_REVERSE; } else { dir = MOTOR_DIR_FORWARD; } if ( ctrlMode < NUM_OF_PUMP_CONTROL_MODES ) { result = setBloodPumpTargetFlowRate( abs(flow), dir, (PUMP_CONTROL_MODE_T)ctrlMode ); } } } return result; } /*********************************************************************//** * @brief * The testSetBloodPumpSpeedOverride function sets the blood pump to run * open loop at a given speed (in RPM). * @details \b Inputs: none * @details \b Outputs: Blood pump commanded to given rate w/ given control mode. * @param message set message from Dialin which includes the pump speed (RPM) * to command the blood pump to. * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetBloodPumpSpeedOverride( MESSAGE_T *message ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { if ( ( sizeof( S32 ) ) == message->hdr.payloadLen ) { MOTOR_DIR_T dir; U08 *msgPayload = &message->payload[0]; S32 rpm; memcpy( &rpm, msgPayload, sizeof( S32 ) ); if ( rpm < 0 ) { dir = MOTOR_DIR_REVERSE; } else { dir = MOTOR_DIR_FORWARD; } result = setBloodPumpTargetFlowRate( BP_RATE_FROM_RPM( abs(rpm) ), dir, PUMP_CONTROL_MODE_OPEN_LOOP ); } } return result; } /*********************************************************************//** * @brief * The testMeasuredBloodFlowRateOverride function overrides the measured * blood flow rate. * @details \b Inputs: none * @details \b Outputs: measuredBloodFlowRate * @param message Override message from Dialin which includes the flow rate * (in mL/min) to override the measured flow rate to. * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testMeasuredBloodFlowRateOverride( MESSAGE_T *message ) { BOOL result = f32Override( message, &measuredBloodFlowRate ); return result; } /*********************************************************************//** * @brief * The testMeasuredBloodPumpRotorSpeedOverride function overrides the measured * blood pump rotor speed. * @details \b Inputs: none * @details \b Outputs: bloodPumpRotorSpeedRPM * @param message Override message from Dialin which includes the pump rotor * speed (in RPM) to override the measured rotor speed to. * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testMeasuredBloodPumpRotorSpeedOverride( MESSAGE_T *message ) { BOOL result = f32Override( message, &bloodPumpRotorSpeedRPM ); return result; } /*********************************************************************//** * @brief * The testMeasuredBloodPumpSpeedOverride function overrides the measured * blood pump motor speed. * @details \b Inputs: none * @details \b Outputs: bloodPumpSpeedRPM * @param message Override message from Dialin which includes the pump motor * speed (in RPM) to override the measured pump speed to. * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testMeasuredBloodPumpSpeedOverride( MESSAGE_T *message ) { BOOL result = f32Override( message, &bloodPumpSpeedRPM ); return result; } /*********************************************************************//** * @brief * The testBloodPumpRotorCountOverride function overrides the blood pump * rotor counter value. * @details \b Inputs: none * @details \b Outputs: bloodPumpRotorCounter * @param message Override message from Dialin which includes the count * to override the blood pump rotor count to. * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testBloodPumpRotorCountOverride( MESSAGE_T *message ) { BOOL result = u32Override( message, &bloodPumpRotorCounter, 0, HEX_32_BIT_FULL_SCALE ); return result; } /*********************************************************************//** * @brief * The testHomeBloodPump function initiates a blood pump home operation. * @details \b Inputs: none * @details \b Outputs: none * @param message BP home command message from Dialin. * @return TRUE if command successful, FALSE if not *************************************************************************/ BOOL testHomeBloodPump( MESSAGE_T *message ) { BOOL result = FALSE; if ( 0 == message->hdr.payloadLen ) { result = homeBloodPump(); } return result; } /*********************************************************************//** * @brief * The testHardStopBloodPump function hard stops the blood pump. * @details \b Inputs: none * @details \b Outputs: none * @param message BP home command message from Dialin. * @return TRUE if command successful, FALSE if not *************************************************************************/ BOOL testHardStopBloodPump( MESSAGE_T *message ) { BOOL result = FALSE; if ( 0 == message->hdr.payloadLen ) { result = TRUE; signalBloodPumpHardStop(); } return result; } /*********************************************************************//** * @brief * The testBPFlowAlphaYInterceptOverride function overrides the Alpha Y * intercept of the blood flow estimation equation. * @details \b Inputs: none * @details \b Outputs: bpFlowAlphaYIntercept * @param message Override message from Dialin which includes the Alpha Y * intercept value to override with. * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testBPFlowAlphaYInterceptOverride( MESSAGE_T *message ) { BOOL result = f32Override( message, &bpFlowAlphaYIntercept ); return result; } /*********************************************************************//** * @brief * The testBPFlowWearATermOverride function overrides the wear A term of the * blood flow estimation equation. * @details \b Inputs: none * @details \b Outputs: bpFlowWearATerm * @param message Override message from Dialin which includes the wear A term * value to override with. * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testBPFlowWearATermOverride( MESSAGE_T *message ) { BOOL result = f32Override( message, &bpFlowWearATerm ); return result; } /*********************************************************************//** * @brief * The testBPFlowWearBTermOverride function overrides the wear B term of the * blood flow estimation equation. * @details \b Inputs: none * @details \b Outputs: bpFlowWearBTerm * @param message Override message from Dialin which includes the wear B term * value to override with. * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testBPFlowWearBTermOverride( MESSAGE_T *message ) { BOOL result = f32Override( message, &bpFlowWearBTerm ); return result; } /**@}*/