/************************************************************************** * * Copyright (c) 2019-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) Vinayakam Mani * @date (last) 22-Apr-2024 * * @author (original) Sean Nash * @date (original) 07-Nov-2019 * ***************************************************************************/ #include // Used for fabs() functions #include "can.h" #include "etpwm.h" #include "gio.h" #include "reg_het.h" #include "BloodFlow.h" #include "CPLD.h" #include "FPGA.h" #include "InternalADC.h" #include "NVDataMgmt.h" #include "OperationModes.h" #include "PersistentAlarm.h" #include "PIControllers.h" #include "SafetyShutdown.h" #include "SystemCommMessages.h" #include "TaskGeneral.h" #include "TaskPriority.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_PRIORITY_INTERVAL ) #define MAX_SETTABLE_BLOOD_FLOW_RATE 700 ///< Maximum settable blood flow rate (in mL/min). #define MAX_BLOOD_PUMP_PWM_STEP_UP_CHANGE 0.008F ///< Max duty cycle change when ramping up ~ 100 mL/min/s. #define MAX_BLOOD_PUMP_PWM_STEP_DN_CHANGE 0.016F ///< Max duty cycle change when ramping down ~ 200 mL/min/s. #define MAX_BLOOD_PUMP_PWM_DUTY_CYCLE 0.90F ///< Controller will error if PWM duty cycle > 90%, so set max to 89% #define MIN_BLOOD_PUMP_PWM_DUTY_CYCLE 0.10F ///< Controller will error if PWM duty cycle < 10%, so set min to 10% #define BP_CONTROL_INTERVAL_SEC 10 ///< 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.0001F ///< P term for blood pump control #define BP_I_COEFFICIENT 0.00075F ///< I term for blood pump control #define BP_HOME_SPEED 400 ///< Target pump speed (in RPM) for homing. #define BP_HOME_TIMEOUT_MS 10000 ///< Maximum time (in ms) allowed for homing to complete. #define BP_MAX_ROTOR_HALL_INTERVAL_MS 20000 ///< Maximum time (in ms) allowed between rotor hall sensor detects (50 mL/min worst case). /// Interval (ms/task time) at which the blood pump speed is calculated (every 40 ms). #define BP_SPEED_CALC_INTERVAL ( 40 / TASK_PRIORITY_INTERVAL ) /// Number of hall sensor counts kept in buffer to hold last 1 second of count data. #define BP_SPEED_CALC_BUFFER_LEN ( MS_PER_SECOND / BP_SPEED_CALC_INTERVAL / TASK_PRIORITY_INTERVAL ) #define BP_MAX_ROTOR_SPEED_RPM 100.0F ///< Maximum rotor speed allowed for blood pump. #define BP_MIN_ROTOR_PULSES_FOR_MAX_SPEED 2 ///< Minimum rotor pulses indicating excessive speed required for alarm. #define BP_MAX_FLOW_RATE 1320.0F ///< Maximum measured BP flow rate allowed. #define BP_MIN_FLOW_RATE -1320.0F ///< Minimum measured BP flow rate allowed. #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 ); #define BP_MAX_CURR_WHEN_STOPPED_MA 150.0F ///< Motor controller current should not exceed this when pump should be stopped. #define BP_MAX_CURR_WHEN_RUNNING_MA 2000.0F ///< Motor controller current should not exceed this when pump should be running. #define BP_MAX_CURR_ERROR_DURATION_MS 5000 ///< Motor controller current errors persisting beyond this duration will trigger an alarm. #define BLOODPUMP_ADC_FULL_SCALE_V 3.0F ///< BP analog signals are 0-3V (while int. ADC ref may be different). #define BLOODPUMP_ADC_ZERO 1998 ///< Blood pump ADC channel zero offset. /// Macro converts 12 bit ADC value to signed 16-bit value. #define SIGN_FROM_12_BIT_VALUE(v) ( (S16)(v) - (S16)BLOODPUMP_ADC_ZERO ) #define BP_SPEED_ADC_TO_RPM_FACTOR 1.751752F ///< Conversion factor from ADC counts to RPM for blood pump motor (3500 RPM/1998 counts). #define BP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.000238F ///< ~42 BP motor RPM = 1% PWM duty cycle. #define BP_CURRENT_ADC_TO_MA_FACTOR 3.002F ///< Conversion factor from ADC counts to mA for blood pump motor. #define BP_REV_PER_LITER 146.84F ///< Rotor revolutions per liter. #define BP_ML_PER_ROTOR_REV 6.81F ///< Milliliters per rotor revolusion. #define BP_ML_PER_MIN_TO_PUMP_RPM_FACTOR ( BP_REV_PER_LITER / ML_PER_LITER ) ///< Conversion factor from mL/min to motor RPM. #define BP_GEAR_RATIO 32.0F ///< Blood pump motor to blood pump gear ratio. #define BP_PWM_ZERO_OFFSET 0.1F ///< 10 pct PWM duty cycle = zero speed. #define BP_100_PCT_PWM_RPM_RANGE 4000.0F ///< 10-90% PWM range yields 0-3,200 RPM range. Full 100% PWM range would yield 4,000 RPM range. /// Conversion macro from mL/min to estimated PWM duty cycle %. #define BP_PWM_FROM_ML_PER_MIN(rate) ( (rate) * BP_ML_PER_MIN_TO_PUMP_RPM_FACTOR * BP_GEAR_RATIO * BP_MOTOR_RPM_TO_PWM_DC_FACTOR + BP_PWM_ZERO_OFFSET ) /// Conversion from PWM duty cycle % to commanded pump motor speed. #define BP_PWM_TO_MOTOR_SPEED_RPM(pwm,dir) ( ( ((pwm) - BP_PWM_ZERO_OFFSET) * BP_100_PCT_PWM_RPM_RANGE ) * ( dir == MOTOR_DIR_FORWARD ? 1.0F : -1.0F ) ) /// Conversion from RPM to PWM duty cycle %. #define BP_MOTOR_SPEED_RPM_TO_PWM(rpm) ( ( (F32)(rpm) / BP_100_PCT_PWM_RPM_RANGE ) + BP_PWM_ZERO_OFFSET ) /// Conversion macro from mL/min to estimated PWM duty cycle %. #define BP_ML_PER_MIN_FROM_PWM(pwm) ( ( ( pwm - BP_PWM_ZERO_OFFSET ) / ( BP_ML_PER_MIN_TO_PUMP_RPM_FACTOR * BP_GEAR_RATIO * BP_MOTOR_RPM_TO_PWM_DC_FACTOR ) ) ) /// Measured blood flow is filtered w/ moving average. #define SIZE_OF_ROLLING_AVG ( ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) * 1 ) #define PUMP_DIR_ERROR_COUNT_MASK 0x3F ///< Bit mask for pump direction error counter. #define BP_MIN_DIR_CHECK_SPEED_RPM 10.0F ///< Minimum motor speed before we check pump direction. #define BP_COMMUTATION_ERROR_MAX_CNT 3 ///< Maximum number of commutation errors within time window before alarm triggered. #define BP_COMMUTATION_ERROR_TIME_WIN_MS (15 * MS_PER_SECOND) ///< Time window for BP commutation error. #define BP_FLOW_ALPHA_Y_INTERCEPT 1.11F ///< Y intercept used for alpha flow coefficient calculation. #define BP_FLOW_WEAR_A_TERM 0.00000000896F ///< A term used for wear portion of alpha flow coefficient. #define BP_FLOW_WEAR_B_TERM 0.000550F ///< 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 DATA_PUBLISH_COUNTER_START_COUNT 20 ///< Data publish counter start count. /// Enumeration of blood pump controller states. typedef enum BloodPump_States { BLOOD_PUMP_OFF_STATE = 0, ///< Blood pump off state BLOOD_PUMP_RAMPING_UP_STATE, ///< Blood pump ramping up state BLOOD_PUMP_RAMPING_DOWN_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; // Pin assignments for pump stop and direction outputs and rotor hall sensor input #define STOP_CAN3_PORT_MASK 0x00000002 ///< (Tx - re-purposed as output GPIO for blood pump stop signal) #define DIR_CAN3_PORT_MASK 0x00000002 ///< (Rx - re-purposed as output GPIO for blood pump direction signal) #define BP_ROTOR_HALL_SENSOR_NHET_ID 0x0000000C ///< NHET pin number associated with BP rotor hall sensor input // Blood pump stop and direction macros #define SET_BP_DIR() {canREG3->RIOC |= DIR_CAN3_PORT_MASK;} ///< Macro to set blood pump direction signal high (forward). #define CLR_BP_DIR() {canREG3->RIOC &= ~DIR_CAN3_PORT_MASK;} ///< Macro to set blood pump direction signal low (reverse). #define SET_BP_STOP() {canREG3->TIOC &= ~STOP_CAN3_PORT_MASK;} ///< Macro to set blood pump disable signal low (active low). #define CLR_BP_STOP() {canREG3->TIOC |= STOP_CAN3_PORT_MASK;} ///< Macro to set blood pump disable signal high (active low). // ********** private data ********** static BLOOD_PUMP_STATE_T bloodPumpState = BLOOD_PUMP_OFF_STATE; ///< Current state of blood flow controller state machine static U32 bloodFlowDataPublicationTimerCounter; ///< Used to schedule blood flow data publication to CAN bus static BOOL isBloodPumpOn = FALSE; ///< Blood pump is currently running static F32 bloodPumpPWMDutyCyclePct = 0.0; ///< Initial blood pump PWM duty cycle static F32 bloodPumpPWMDutyCyclePctSet = 0.0; ///< Currently set blood pump PWM duty cycle static MOTOR_DIR_T bloodPumpDirection = MOTOR_DIR_FORWARD; ///< Requested blood flow direction static MOTOR_DIR_T bloodPumpDirectionSet = MOTOR_DIR_FORWARD; ///< Currently set blood flow direction static PUMP_CONTROL_MODE_T bloodPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; ///< Requested blood pump control mode. static PUMP_CONTROL_MODE_T bloodPumpControlModeSet = PUMP_CONTROL_MODE_CLOSED_LOOP; ///< Currently set blood pump control mode. static U32 errorBloodRotorSpeedPersistTimerCtr = 0; ///< Persistence timer counter for rotor speed error condition. /// Interval (in task intervals) at which to publish blood flow data to CAN bus. static OVERRIDE_U32_T bloodFlowDataPublishInterval = { BLOOD_FLOW_DATA_PUB_INTERVAL, BLOOD_FLOW_DATA_PUB_INTERVAL, BLOOD_FLOW_DATA_PUB_INTERVAL, 0 }; static S32 targetBloodFlowRate = 0; ///< Requested blood flow rate. static OVERRIDE_F32_T measuredBloodFlowRate = { 0.0, 0.0, 0.0, 0 }; ///< Measured (calculated now) blood flow rate. static OVERRIDE_F32_T bloodPumpRotorSpeedRPM = { 0.0, 0.0, 0.0, 0 }; ///< Measured blood pump rotor speed. static OVERRIDE_F32_T bloodPumpSpeedRPM = { 0.0, 0.0, 0.0, 0 }; ///< Measured blood pump motor speed. static OVERRIDE_F32_T adcBloodPumpMCSpeedRPM = { 0.0, 0.0, 0.0, 0 }; ///< Measured blood pump motor controller speed. static OVERRIDE_F32_T adcBloodPumpMCCurrentmA = { 0.0, 0.0, 0.0, 0 }; ///< Measured blood pump motor controller current. static U08 lastBloodPumpDirectionCount = 0; ///< Previous pump direction error count reported by FPGA. static F32 rpmReadings[ SIZE_OF_ROLLING_AVG ]; ///< Holds RPM samples for a rolling average. static U32 rpmReadingsIdx = 0; ///< Index for next sample in rolling average array. static F32 rpmReadingsTotal = 0.0; ///< Rolling total - used to calc average. static U32 rpmReadingsCount = 0; ///< Number of samples in RPM rolling average buffer. static F32 filteredBloodPumpSpeed = 0.0; ///< Filtered blood pump speed used in blood flow estimation. static U32 bpControlTimerCounter = 0; ///< Determines when to perform control on blood flow. static U32 bpRotorRevStartTime = 0; ///< Blood pump rotor rotation start time (in ms). static OVERRIDE_U32_T bloodPumpRotorCounter = { 0, 0, 0, 0 }; ///< Running counter for blood pump rotor revolutions. static U32 bpRotorSpeedTooFastPulseCount; ///< Counter for rotor pulses indicating RPM > 100. static BOOL bpStopAtHomePosition = FALSE; ///< Stop blood pump at next home position. static U32 bpHomeStartTime = 0; ///< When did blood pump home command begin? (in ms). static U32 bloodPumpMotorEdgeCount = 0; ///< Running counter for blood pump motor revolutions. static U16 bpLastMotorHallSensorCounts[ BP_SPEED_CALC_BUFFER_LEN ]; ///< Last hall sensor readings for the blood pump motor. static U32 bpMotorSpeedCalcIdx = 0; ///< Index into 1 second buffer of motor speed hall sensor counts. static U32 bpMotorSpeedCalcTimerCtr = 0; ///< Counter determines interval for calculating blood pump motor speed from hall sensor count. static HD_PUMPS_CAL_RECORD_T bloodPumpCalRecord; ///< Blood pump calibration record. // ********** private function prototypes ********** static BLOOD_PUMP_STATE_T handleBloodPumpOffState( void ); static BLOOD_PUMP_STATE_T handleBloodPumpRampingUpState( void ); static BLOOD_PUMP_STATE_T handleBloodPumpRampingDownState( void ); static BLOOD_PUMP_STATE_T handleBloodPumpControlToTargetState( void ); static void setBloodPumpControlSignalPWM( F32 newPWM ); static void stopBloodPump( void ); static void releaseBloodPumpStop( void ); static void setBloodPumpDirection( MOTOR_DIR_T dir ); static void publishBloodFlowData( void ); static void updateBloodPumpSpeedAndDirectionFromHallSensors( void ); static void checkBloodPumpRotor( void ); static void checkBloodPumpDirection( void ); static void checkBloodPumpSpeeds( void ); static void checkBloodPumpMCCurrent( void ); static F32 calcBloodFlow( void ); static void resetBloodPumpRPMMovingAverage( void ); static void filterBloodPumpRPMReadings( F32 rpm ); /*********************************************************************//** * @brief * The initBloodFlow function initializes the BloodFlow module. * @details Inputs: none * @details Outputs: bloodFlowDataPublicationTimerCounter * @return none *************************************************************************/ void initBloodFlow( void ) { U32 i; bloodFlowDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; bpRotorSpeedTooFastPulseCount = 0; signalBloodPumpHardStop(); setBloodPumpDirection( MOTOR_DIR_FORWARD ); // Zero rolling pump speed average buffer resetBloodPumpRPMMovingAverage(); // Zero motor hall sensors counts buffer bpMotorSpeedCalcIdx = 0; for ( i = 0; i < BP_SPEED_CALC_BUFFER_LEN; i++ ) { bpLastMotorHallSensorCounts[ i ] = getFPGABloodPumpHallSensorCount(); } resetBloodPumpRotorCount(); // Initialize blood flow PI controller initializePIController( PI_CONTROLLER_ID_BLOOD_FLOW, MIN_BLOOD_PUMP_PWM_DUTY_CYCLE, BP_P_COEFFICIENT, BP_I_COEFFICIENT, MIN_BLOOD_PUMP_PWM_DUTY_CYCLE, MAX_BLOOD_PUMP_PWM_DUTY_CYCLE ); // 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 ); initTimeWindowedCount( TIME_WINDOWED_COUNT_BP_COMMUTATION_ERROR, BP_COMMUTATION_ERROR_MAX_CNT, BP_COMMUTATION_ERROR_TIME_WIN_MS ); initPersistentAlarm( ALARM_ID_HD_BLOOD_PUMP_ROTOR_SPEED_TOO_HIGH, 0, BP_MAX_ROTOR_SPEED_ERROR_PERSIST ); initPersistentAlarm( ALARM_ID_HD_BLOOD_PUMP_MC_CURRENT_CHECK, 0, BP_MAX_CURR_ERROR_DURATION_MS ); } /*********************************************************************//** * @brief * The setBloodPumpTargetFlowRate function sets a new target flow rate and * pump direction. * @details Inputs: isBloodPumpOn, bloodPumpDirectionSet * @details 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 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 of if the bypass flow limit has been enabled if ( TRUE == isFlowInrange ) { resetBloodPumpRPMMovingAverage(); targetBloodFlowRate = dirFlowRate; bloodPumpDirection = dir; bloodPumpControlMode = mode; // Set PWM duty cycle target to an estimated initial target to ramp to based on target flow rate - then we will control to flow when ramp completed bloodPumpPWMDutyCyclePct = BP_PWM_FROM_ML_PER_MIN((F32)flowRate); // ~ 8% per 100 mL/min with a 10% zero offset added in (e.g. 100 mL/min = 8+10 = 18%) #ifndef _VECTORCAST_ /* * This range check was disabled in VectorCAST since all of its branches cannot be covered. * This if statement is only executed if the flow rate is less than 700 mL/min and the required duty cycle for that will be 88% and not 90% and above. * Also the blood flow to PWM conversion macro will have a minimum of 10% duty cycle and therefore, duty cycles below 10% cannot be achieved. * This range check will stay in the actual code in case there was a memory stomp or other catastrophic cases */ bloodPumpPWMDutyCyclePct = RANGE( bloodPumpPWMDutyCyclePct, MIN_BLOOD_PUMP_PWM_DUTY_CYCLE, MAX_BLOOD_PUMP_PWM_DUTY_CYCLE ); #endif switch ( bloodPumpState ) { case BLOOD_PUMP_RAMPING_UP_STATE: // See if we need to reverse direction of ramp if ( bloodPumpPWMDutyCyclePct < bloodPumpPWMDutyCyclePctSet ) { bloodPumpState = BLOOD_PUMP_RAMPING_DOWN_STATE; } break; case BLOOD_PUMP_RAMPING_DOWN_STATE: // See if we need to reverse direction of ramp if ( bloodPumpPWMDutyCyclePct > bloodPumpPWMDutyCyclePctSet ) { bloodPumpState = BLOOD_PUMP_RAMPING_UP_STATE; } break; case BLOOD_PUMP_CONTROL_TO_TARGET_STATE: // Start ramp to new target in appropriate direction if ( bloodPumpPWMDutyCyclePctSet > bloodPumpPWMDutyCyclePct ) { bloodPumpState = BLOOD_PUMP_RAMPING_DOWN_STATE; } else { bloodPumpState = BLOOD_PUMP_RAMPING_UP_STATE; } break; default: // Ok - not all states need to be handled here break; } 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_HD_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 Inputs: none * @details 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; F32 pwm = BP_MOTOR_SPEED_RPM_TO_PWM( rpm ); result = setBloodPumpTargetFlowRate( (U32)BP_ML_PER_MIN_FROM_PWM( pwm ), 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 Inputs: bloodPumpRotorCounter, arterial pressure * @details 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 = BP_FLOW_WEAR_A_TERM * (F32)rotCnt + BP_FLOW_WEAR_B_TERM; F32 alpha = wear * artPresL + BP_FLOW_ALPHA_Y_INTERCEPT; F32 flow = alpha * BP_ML_PER_ROTOR_REV * rotSpd; return flow; } /*********************************************************************//** * @brief * The signalBloodPumpHardStop function stops the blood pump immediately. * @details Inputs: none * @details Outputs: Blood pump stopped, set point reset, state changed to off * @return none *************************************************************************/ void signalBloodPumpHardStop( void ) { targetBloodFlowRate = 0; stopBloodPump(); bloodPumpState = BLOOD_PUMP_OFF_STATE; bloodPumpPWMDutyCyclePct = BP_PWM_ZERO_OFFSET; bpControlTimerCounter = 0; resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, MIN_BLOOD_PUMP_PWM_DUTY_CYCLE ); } /*********************************************************************//** * @brief * The signalBloodPumpRotorHallSensor function handles the blood pump rotor * hall sensor detection. Calculates rotor speed (in RPM). Stops pump if * there is a pending request to home the pump. * @details Inputs: bpRotorRevStartTime, bpStopAtHomePosition * @details Outputs: bpRotorRevStartTime, bloodPumpRotorSpeedRPM, bloodPumpRotorCounter * @return none *************************************************************************/ void signalBloodPumpRotorHallSensor( void ) { U32 rotTime = getMSTimerCount(); U32 deltaTime = calcTimeBetween( bpRotorRevStartTime, rotTime ); F32 rotSpeed = ( 1.0F / (F32)deltaTime ) * (F32)MS_PER_SECOND * (F32)SEC_PER_MIN; // calculate rotor speed indicating by time between this and previous pulse // Increment rotor counter bloodPumpRotorCounter.data++; // Count pulses indicating rotor speed too fast if ( getMeasuredBloodPumpRotorSpeed() > BP_MAX_ROTOR_SPEED_RPM ) { bpRotorSpeedTooFastPulseCount++; } else { bpRotorSpeedTooFastPulseCount = 0; } // Calculate rotor speed (in RPM) bloodPumpRotorSpeedRPM.data = rotSpeed; bpRotorRevStartTime = rotTime; // If we are supposed to stop pump at home position, stop pump now. if ( TRUE == bpStopAtHomePosition ) { signalBloodPumpHardStop(); bpStopAtHomePosition = FALSE; } } /*********************************************************************//** * @brief * The homeBloodPump function initiates a blood pump home operation. * @details Inputs: bloodPumpState * @details Outputs: bpStopAtHomePosition, bpHomeStartTime, blood pump started (slow) * @return none *************************************************************************/ BOOL homeBloodPump( void ) { BOOL result = FALSE; if ( BLOOD_PUMP_OFF_STATE == bloodPumpState ) { bpStopAtHomePosition = TRUE; bpHomeStartTime = getMSTimerCount(); result = setBloodPumpTargetRPM( BP_HOME_SPEED, MOTOR_DIR_FORWARD ); } return result; } /*********************************************************************//** * @brief * The getBloodPumpMotorCount function returns the current count for the * blood pump motor revolution counter. * @details Inputs: bloodPumpMotorCount * @details Outputs: none * @return bloodPumpMotorCount *************************************************************************/ U32 getBloodPumpMotorCount( void ) { return bloodPumpMotorEdgeCount; } /*********************************************************************//** * @brief * The getBloodPumpRotorCount function returns the current count for the * blood pump rotor revolution counter. * @details Inputs: bloodPumpRotorCounter * @details Outputs: none * @return bloodPumpRotorCounter *************************************************************************/ 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 Inputs: isBloodPumpOn * @details 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 Inputs: bloodPumpState * @details 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 Inputs: none * @details 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 Inputs: none * @details Outputs: measuredBloodFlowRate, adcBloodPumpMCSpeedRPM, * adcBloodPumpMCCurrentmA * @return none *************************************************************************/ void execBloodFlowMonitor( void ) { HD_OP_MODE_T opMode = getCurrentOperationMode(); U16 bpRPM = getIntADCReading( INT_ADC_BLOOD_PUMP_SPEED ); U16 bpmA = getIntADCReading( INT_ADC_BLOOD_PUMP_MOTOR_CURRENT ); 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( getMeasuredBloodPumpMCSpeed() ); measuredBloodFlowRate.data = calcBloodFlow(); // Pressure and rotor speed already filtered as inputs to calc, so no need to filter flow any further // Calculate blood pump motor speed/direction from hall sensor count updateBloodPumpSpeedAndDirectionFromHallSensors(); // Do not start enforcing checks until out of init/POST mode if ( opMode != MODE_INIT ) { // Check pump direction checkBloodPumpDirection(); // Check pump controller current checkBloodPumpMCCurrent(); // Check pump speeds checkBloodPumpSpeeds(); // Check for home position, zero/low speed checkBloodPumpRotor(); } else { lastBloodPumpDirectionCount = getFPGABloodPumpHallSensorStatus() & PUMP_DIR_ERROR_COUNT_MASK; } // Publish blood flow data on interval publishBloodFlowData(); } /*********************************************************************//** * @brief * The execBloodFlowController function executes the blood flow controller. * @details Inputs: bloodPumpState * @details Outputs: bloodPumpState * @return none *************************************************************************/ void execBloodFlowController( void ) { switch ( bloodPumpState ) { case BLOOD_PUMP_OFF_STATE: bloodPumpState = handleBloodPumpOffState(); break; case BLOOD_PUMP_RAMPING_UP_STATE: bloodPumpState = handleBloodPumpRampingUpState(); break; case BLOOD_PUMP_RAMPING_DOWN_STATE: bloodPumpState = handleBloodPumpRampingDownState(); break; case BLOOD_PUMP_CONTROL_TO_TARGET_STATE: bloodPumpState = handleBloodPumpControlToTargetState(); break; default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_BLOOD_FLOW_INVALID_BLOOD_PUMP_STATE, bloodPumpState ) break; } } /*********************************************************************//** * @brief * The handleBloodPumpOffState function handles the blood pump off state * of the blood pump controller state machine. * @details Inputs: targetBloodFlowRate, bloodPumpDirection * @details Outputs: bloodPumpPWMDutyCyclePctSet, bloodPumpDirectionSet, isBloodPumpOn * @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 ) { // Set initial PWM duty cycle bloodPumpPWMDutyCyclePctSet = BP_PWM_ZERO_OFFSET + MAX_BLOOD_PUMP_PWM_STEP_UP_CHANGE; setBloodPumpControlSignalPWM( bloodPumpPWMDutyCyclePctSet ); // Allow blood pump to run in requested direction setBloodPumpDirection( bloodPumpDirection ); releaseBloodPumpStop(); isBloodPumpOn = TRUE; result = BLOOD_PUMP_RAMPING_UP_STATE; } else { isBloodPumpOn = FALSE; } return result; } /*********************************************************************//** * @brief * The handleBloodPumpRampingUpState function handles the ramp up state * of the blood pump controller state machine. * @details Inputs: bloodPumpPWMDutyCyclePctSet * @details Outputs: bloodPumpPWMDutyCyclePctSet * @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 bloodPumpPWMDutyCyclePctSet -= MAX_BLOOD_PUMP_PWM_STEP_DN_CHANGE; setBloodPumpControlSignalPWM( bloodPumpPWMDutyCyclePctSet ); result = BLOOD_PUMP_RAMPING_DOWN_STATE; } // Have we reached end of ramp up? else if ( bloodPumpPWMDutyCyclePctSet >= bloodPumpPWMDutyCyclePct ) { resetBloodPumpRPMMovingAverage(); bloodPumpPWMDutyCyclePctSet = bloodPumpPWMDutyCyclePct; resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, bloodPumpPWMDutyCyclePctSet ); bloodPumpControlModeSet = bloodPumpControlMode; setBloodPumpControlSignalPWM( bloodPumpPWMDutyCyclePctSet ); bpControlTimerCounter = 0; result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; } // Continue ramp up else { bloodPumpPWMDutyCyclePctSet += MAX_BLOOD_PUMP_PWM_STEP_UP_CHANGE; setBloodPumpControlSignalPWM( bloodPumpPWMDutyCyclePctSet ); } return result; } /*********************************************************************//** * @brief * The handleBloodPumpRampingDownState function handles the ramp down state * of the blood pump controller state machine. * @details Inputs: bloodPumpPWMDutyCyclePctSet * @details Outputs: bloodPumpPWMDutyCyclePctSet * @return next state *************************************************************************/ static BLOOD_PUMP_STATE_T handleBloodPumpRampingDownState( void ) { BLOOD_PUMP_STATE_T result = BLOOD_PUMP_RAMPING_DOWN_STATE; // Have we essentially reached zero speed? if ( bloodPumpPWMDutyCyclePctSet < (MAX_BLOOD_PUMP_PWM_STEP_DN_CHANGE + BP_PWM_ZERO_OFFSET) ) { signalBloodPumpHardStop(); result = BLOOD_PUMP_OFF_STATE; } // Have we reached end of ramp down? else if ( bloodPumpPWMDutyCyclePctSet <= bloodPumpPWMDutyCyclePct ) { resetBloodPumpRPMMovingAverage(); bloodPumpPWMDutyCyclePctSet = bloodPumpPWMDutyCyclePct; resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, bloodPumpPWMDutyCyclePctSet ); bloodPumpControlModeSet = bloodPumpControlMode; setBloodPumpControlSignalPWM( bloodPumpPWMDutyCyclePctSet ); bpControlTimerCounter = 0; result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; } // Continue ramp down else { bloodPumpPWMDutyCyclePctSet -= MAX_BLOOD_PUMP_PWM_STEP_DN_CHANGE; setBloodPumpControlSignalPWM( bloodPumpPWMDutyCyclePctSet ); } return result; } /*********************************************************************//** * @brief * The handleBloodPumpControlToTargetState function handles the "control to * target" state of the blood pump controller state machine. * @details Inputs: none * @details 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 ( bloodPumpControlModeSet == PUMP_CONTROL_MODE_CLOSED_LOOP ) { F32 tgtFlow = (F32)targetBloodFlowRate; F32 actFlow = getMeasuredBloodFlowRate(); F32 newPWM; newPWM = runPIController( PI_CONTROLLER_ID_BLOOD_FLOW, tgtFlow, actFlow ); bloodPumpPWMDutyCyclePctSet = newPWM; setBloodPumpControlSignalPWM( newPWM ); } bpControlTimerCounter = 0; } return result; } /*********************************************************************//** * @brief * The setBloodPumpControlSignalPWM function sets the PWM duty cycle for * the blood pump to a given %. * @details Inputs: none * @details Outputs: blood pump stop signal activated, PWM duty cycle zeroed * @param newPWM new duty cycle % to apply to PWM * @return none *************************************************************************/ static void setBloodPumpControlSignalPWM( F32 newPWM ) { etpwmSetCmpA( etpwmREG1, (U32)( (S32)( ( newPWM * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) ); } /*********************************************************************//** * @brief * The stopBloodPump function sets the blood pump stop signal. * @details Inputs: none * @details Outputs: blood pump stop signal activated, PWM duty cycle zeroed * @return none *************************************************************************/ static void stopBloodPump( void ) { isBloodPumpOn = FALSE; bloodPumpPWMDutyCyclePctSet = BP_PWM_ZERO_OFFSET; etpwmSetCmpA( etpwmREG1, 0 ); SET_BP_STOP(); } /*********************************************************************//** * @brief * The releaseBloodPumpStop function clears the blood pump stop signal. * @details Inputs: none * @details Outputs: blood pump stop signal * @return none *************************************************************************/ static void releaseBloodPumpStop( void ) { CLR_BP_STOP(); } /*********************************************************************//** * @brief * The setBloodPumpDirection function sets the set blood pump direction to * the given direction. * @details Inputs: bloodPumpState * @details Outputs: bloodPumpState * @param dir blood pump direction to set * @return none *************************************************************************/ static void setBloodPumpDirection( MOTOR_DIR_T dir ) { switch ( dir ) { case MOTOR_DIR_FORWARD: bloodPumpDirectionSet = dir; SET_BP_DIR(); break; case MOTOR_DIR_REVERSE: bloodPumpDirectionSet = dir; CLR_BP_DIR(); break; default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_BLOOD_FLOW_INVALID_BLOOD_PUMP_DIRECTION, dir ) break; } } /*********************************************************************//** * @brief * The getTargetBloodFlowRate function gets the target blood flow rate. * @details Inputs: targetBloodFlowRate * @details 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 Inputs: measuredBloodFlowRate * @details Outputs: none * @return the current blood flow rate (in mL/min). *************************************************************************/ F32 getMeasuredBloodFlowRate( void ) { F32 result = measuredBloodFlowRate.data; if ( OVERRIDE_KEY == measuredBloodFlowRate.override ) { result = measuredBloodFlowRate.ovData; } return result; } /*********************************************************************//** * @brief * The getMeasuredBloodPumpRotorSpeed function gets the measured blood flow * rate. * @details Inputs: bloodPumpRotorSpeedRPM * @details Outputs: none * @return the current blood flow rate (in mL/min). *************************************************************************/ F32 getMeasuredBloodPumpRotorSpeed( void ) { F32 result = bloodPumpRotorSpeedRPM.data; if ( OVERRIDE_KEY == bloodPumpRotorSpeedRPM.override ) { result = bloodPumpRotorSpeedRPM.ovData; } return result; } /*********************************************************************//** * @brief * The getMeasuredBloodPumpSpeed function gets the measured blood flow * rate. * @details Inputs: bloodPumpSpeedRPM * @details Outputs: none * @return the current blood flow rate (in mL/min). *************************************************************************/ F32 getMeasuredBloodPumpSpeed( void ) { F32 result = bloodPumpSpeedRPM.data; if ( OVERRIDE_KEY == bloodPumpSpeedRPM.override ) { result = bloodPumpSpeedRPM.ovData; } return result; } /*********************************************************************//** * @brief * The getMeasuredBloodPumpMCSpeed function gets the measured blood pump * speed. * @details Inputs: adcBloodPumpMCSpeedRPM * @details Outputs: none * @return the current blood pump speed (in RPM). *************************************************************************/ F32 getMeasuredBloodPumpMCSpeed( void ) { F32 result = adcBloodPumpMCSpeedRPM.data; if ( OVERRIDE_KEY == adcBloodPumpMCSpeedRPM.override ) { result = adcBloodPumpMCSpeedRPM.ovData; } return result; } /*********************************************************************//** * @brief * The getMeasuredBloodPumpMCCurrent function gets the measured blood pump * current. * @details Inputs: adcBloodPumpMCCurrentmA * @details Outputs: none * @return the current blood pump current (in mA). *************************************************************************/ F32 getMeasuredBloodPumpMCCurrent( void ) { F32 result = adcBloodPumpMCCurrentmA.data; if ( OVERRIDE_KEY == adcBloodPumpMCCurrentmA.override ) { result = adcBloodPumpMCCurrentmA.ovData; } return result; } /*********************************************************************//** * @brief * The publishBloodFlowData function publishes blood flow data at the set * interval. * @details Inputs: target flow rate, measured flow rate, measured MC speed, * measured MC current * @details 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; HD_OP_MODE_T opMode = getCurrentOperationMode(); U32 hallSensor = gioGetBit( hetPORT1, BP_ROTOR_HALL_SENSOR_NHET_ID ); payload.setPoint = targetBloodFlowRate; payload.measFlow = getMeasuredBloodFlowRate(); payload.measRotorSpd = getMeasuredBloodPumpRotorSpeed(); payload.measPumpSpd = getMeasuredBloodPumpSpeed(); payload.measMCSpd = getMeasuredBloodPumpMCSpeed(); payload.measMCCurr = getMeasuredBloodPumpMCCurrent(); payload.pwmDC = bloodPumpPWMDutyCyclePctSet * FRACTION_TO_PERCENT_FACTOR; payload.rotorCount = getBloodPumpRotorCount(); if ( ( MODE_PRET == opMode ) || ( MODE_TREA == opMode ) || ( MODE_POST == opMode ) ) { // prescribed flow only available in treatment modes payload.presFlow = getTreatmentParameterU32( TREATMENT_PARAM_BLOOD_FLOW ); } else { payload.presFlow = 0; } payload.rotorHall = ( hallSensor > 0 ? 0 : 1 ); // 1=home, 0=not home broadcastData( MSG_ID_BLOOD_FLOW_DATA, COMM_BUFFER_OUT_CAN_HD_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 Inputs: none * @details Outputs: rpmReadingsTotal, rpmReadingsIdx, rpmReadingsCount all set to zero. * @return none *************************************************************************/ static void resetBloodPumpRPMMovingAverage( void ) { rpmReadingsIdx = 0; rpmReadingsCount = 0; rpmReadingsTotal = 0.0; filteredBloodPumpSpeed = 0.0; } /*********************************************************************//** * @brief * The filterBloodPumpRPMReadings function adds a new pump speed sample to * the filter. * @details Inputs: none * @details 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 updateBloodPumpSpeedAndDirectionFromHallSensors function calculates * the blood pump motor speed and direction from hall sensor counter on * a 1 second interval. * @details Inputs: bpLastMotorHallSensorCount, bpMotorSpeedCalcTimerCtr, current count from FPGA * @details Outputs: bpMotorDirectionFromHallSensors, bloodPumpSpeedRPM * @return none *************************************************************************/ static void updateBloodPumpSpeedAndDirectionFromHallSensors( void ) { if ( ++bpMotorSpeedCalcTimerCtr >= BP_SPEED_CALC_INTERVAL ) { U16 bpMotorHallSensorCount = getFPGABloodPumpHallSensorCount(); U16 last = bpLastMotorHallSensorCounts[ bpMotorSpeedCalcIdx ]; U32 nextIdx = INC_WRAP( bpMotorSpeedCalcIdx, 0, BP_SPEED_CALC_BUFFER_LEN - 1 ); U16 incDelta = u16DiffWithWrap( bpLastMotorHallSensorCounts[ nextIdx ], bpMotorHallSensorCount ); U16 decDelta = ( 0 == incDelta ? 0xFFFF : HEX_64_K - incDelta ); U16 spdDelta; S16 delta; // Determine blood pump speed/direction from delta hall sensor count since last interval if ( incDelta < decDelta ) { spdDelta = incDelta; bloodPumpSpeedRPM.data = ( (F32)spdDelta / (F32)BP_HALL_EDGE_COUNTS_PER_REV ) * (F32)SEC_PER_MIN; } else { spdDelta = decDelta; bloodPumpSpeedRPM.data = ( (F32)spdDelta / (F32)BP_HALL_EDGE_COUNTS_PER_REV ) * (F32)SEC_PER_MIN * -1.0; } // Keep a running 32-bit edge count used for safety check on volume in some functions delta = u16BiDiffWithWrap( last, bpMotorHallSensorCount ); bloodPumpMotorEdgeCount += ( delta >= 0 ? (U16)delta : 0 ); // Update last count for next time bpLastMotorHallSensorCounts[ nextIdx ] = bpMotorHallSensorCount; bpMotorSpeedCalcIdx = nextIdx; bpMotorSpeedCalcTimerCtr = 0; } } /*********************************************************************//** * @brief * The checkBloodPumpRotor function checks the rotor for the blood * pump. If homing, this function will stop when hall sensor detected. If pump * is off or running very slowly, rotor speed will be set to zero. * @details Inputs: bpStopAtHomePosition, bpHomeStartTime, bpRotorRevStartTime, * bpRotorSpeedTooFastPulseCount * @details 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(); // If homing, check timeout if ( ( TRUE == bpStopAtHomePosition ) && ( TRUE == didTimeout( bpHomeStartTime, BP_HOME_TIMEOUT_MS ) ) ) { signalBloodPumpHardStop(); bpStopAtHomePosition = FALSE; } // 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 Inputs: * @details 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_HD_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 Inputs: bloodPumpState, targetBloodFlowRate, bloodPumpPWMDutyCyclePctSet, bloodPumpDirectionSet * @details 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 checkBloodPumpMCCurrent function checks the measured MC current vs. * the set state of the blood pump (stopped or running). * @details Inputs: BP motor controller measured current. * @details Outputs: Alarm triggered it current too high. * @return none *************************************************************************/ static void checkBloodPumpMCCurrent( void ) { F32 const bpCurr = fabs( getMeasuredBloodPumpMCCurrent() ); // Check blood pump current during off state BOOL const isOffMCCurrentBad = ( ( BLOOD_PUMP_OFF_STATE == bloodPumpState ) && ( bpCurr > BP_MAX_CURR_WHEN_STOPPED_MA ) ? TRUE : FALSE ); // Check blood pump current during running state BOOL const isRunningMCCurrentBad = ( ( BLOOD_PUMP_OFF_STATE != bloodPumpState ) && ( bpCurr > BP_MAX_CURR_WHEN_RUNNING_MA ) ? TRUE : FALSE ); if ( ( TRUE == isPersistentAlarmTriggered( ALARM_ID_HD_BLOOD_PUMP_MC_CURRENT_CHECK, isOffMCCurrentBad || isRunningMCCurrentBad ) ) && ( getCPLDACPowerLossDetected() != TRUE ) ) { #ifndef _RELEASE_ if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_MOTOR_CURRNT_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) #endif { SET_ALARM_WITH_1_F32_DATA( ALARM_ID_HD_BLOOD_PUMP_MC_CURRENT_CHECK, bpCurr ); } } } /*********************************************************************//** * @brief * The execBloodFlowTest function executes the state machine for the * BloodFlow self-test. * @details Inputs: bloodPumpCalRecord * @details 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 testSetBloodFlowDataPublishIntervalOverride function overrides the * blood flow data publish interval. * @details Inputs: none * @details Outputs: bloodFlowDataPublishInterval * @param value override blood flow data publish interval with (in ms) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetBloodFlowDataPublishIntervalOverride( U32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { U32 intvl = value / TASK_PRIORITY_INTERVAL; result = TRUE; bloodFlowDataPublishInterval.ovData = intvl; bloodFlowDataPublishInterval.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetBloodFlowDataPublishIntervalOverride function resets the override * of the blood flow data publish interval. * @details Inputs: none * @details Outputs: bloodFlowDataPublishInterval * @return TRUE if override reset successful, FALSE if not *************************************************************************/ BOOL testResetBloodFlowDataPublishIntervalOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; bloodFlowDataPublishInterval.override = OVERRIDE_RESET; bloodFlowDataPublishInterval.ovData = bloodFlowDataPublishInterval.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetTargetBloodFlowRateOverride function overrides the target * blood flow rate. * @details Inputs: none * @details Outputs: targetBloodFlowRate * @param value override target blood flow rate (in mL/min) * @param ctrlMode override pump control mode to this mode (0 = closed loop, 1 = open loop) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetTargetBloodFlowRateOverride( S32 value, U32 ctrlMode ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { MOTOR_DIR_T dir; if ( value < 0 ) { dir = MOTOR_DIR_REVERSE; } else { dir = MOTOR_DIR_FORWARD; } if ( ctrlMode < NUM_OF_PUMP_CONTROL_MODES ) { result = setBloodPumpTargetFlowRate( abs(value), dir, (PUMP_CONTROL_MODE_T)ctrlMode ); } } return result; } /*********************************************************************//** * @brief * The testResetMeasuredBloodFlowRateOverride function overrides the measured * blood flow rate. * @details Inputs: none * @details Outputs: measuredBloodFlowRate * @param value override measured blood flow rate (in mL/min) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetMeasuredBloodFlowRateOverride( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; measuredBloodFlowRate.ovData = value; measuredBloodFlowRate.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetMeasuredBloodFlowRateOverride function resets the override of the * measured blood flow rate. * @details Inputs: none * @details Outputs: measuredBloodFlowRate * @return TRUE if reset successful, FALSE if not *************************************************************************/ BOOL testResetMeasuredBloodFlowRateOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; measuredBloodFlowRate.override = OVERRIDE_RESET; measuredBloodFlowRate.ovData = measuredBloodFlowRate.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetMeasuredBloodPumpRotorSpeedOverride function overrides the measured * blood pump rotor speed. * @details Inputs: none * @details Outputs: bloodPumpRotorSpeedRPM * @param value override measured blood pump rotor speed (in RPM) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetMeasuredBloodPumpRotorSpeedOverride( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; bloodPumpRotorSpeedRPM.ovData = value; bloodPumpRotorSpeedRPM.override = OVERRIDE_KEY; // since override may occur while pump is actually stopped, may not get pulses to reach min pulse count for alarm if ( value > BP_MAX_ROTOR_SPEED_RPM ) { bpRotorSpeedTooFastPulseCount = BP_MIN_ROTOR_PULSES_FOR_MAX_SPEED; } } return result; } /*********************************************************************//** * @brief * The testResetMeasuredBloodPumpRotorSpeedOverride function resets the override of the * measured blood pump rotor speed. * @details Inputs: none * @details Outputs: bloodPumpRotorSpeedRPM * @return TRUE if reset successful, FALSE if not *************************************************************************/ BOOL testResetMeasuredBloodPumpRotorSpeedOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; bloodPumpRotorSpeedRPM.override = OVERRIDE_RESET; bloodPumpRotorSpeedRPM.ovData = bloodPumpRotorSpeedRPM.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetMeasuredBloodPumpSpeedOverride function overrides the measured * blood pump motor speed. * @details Inputs: none * @details Outputs: bloodPumpSpeedRPM * @param value override measured blood pump motor speed (in RPM) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetMeasuredBloodPumpSpeedOverride( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; bloodPumpSpeedRPM.ovData = value; bloodPumpSpeedRPM.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetMeasuredBloodPumpSpeedOverride function resets the override of the * measured blood pump motor speed. * @details Inputs: none * @details Outputs: bloodPumpSpeedRPM * @return TRUE if reset successful, FALSE if not *************************************************************************/ BOOL testResetMeasuredBloodPumpSpeedOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; bloodPumpSpeedRPM.override = OVERRIDE_RESET; bloodPumpSpeedRPM.ovData = bloodPumpSpeedRPM.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetMeasuredBloodPumpMCSpeedOverride function overrides the measured * blood pump motor speed. * @details Inputs: none * @details Outputs: adcBloodPumpMCSpeedRPM * @param value override measured blood pump speed (in RPM) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetMeasuredBloodPumpMCSpeedOverride( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; adcBloodPumpMCSpeedRPM.ovData = value; adcBloodPumpMCSpeedRPM.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetMeasuredBloodPumpMCSpeedOverride function resets the override of the * measured blood pump motor speed. * @details Inputs: none * @details Outputs: adcBloodPumpMCSpeedRPM * @return TRUE if reset successful, FALSE if not *************************************************************************/ BOOL testResetMeasuredBloodPumpMCSpeedOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; adcBloodPumpMCSpeedRPM.override = OVERRIDE_RESET; adcBloodPumpMCSpeedRPM.ovData = adcBloodPumpMCSpeedRPM.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetMeasuredBloodPumpMCCurrentOverride function overrides the measured * blood pump motor current. * @details Inputs: none * @details Outputs: adcBloodPumpMCCurrentmA * @param value override measured blood pump current (in mA) * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetMeasuredBloodPumpMCCurrentOverride( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; adcBloodPumpMCCurrentmA.ovData = value; adcBloodPumpMCCurrentmA.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetMeasuredBloodPumpMCCurrentOverride function resets the override of the * measured blood pump motor current. * @details Inputs: none * @details Outputs: adcBloodPumpMCCurrentmA * @return TRUE if reset successful, FALSE if not *************************************************************************/ BOOL testResetMeasuredBloodPumpMCCurrentOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; adcBloodPumpMCCurrentmA.override = OVERRIDE_RESET; adcBloodPumpMCCurrentmA.ovData = adcBloodPumpMCCurrentmA.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetBloodPumpRotorCountOverride function overrides the blood pump * rotor counter value. * @details Inputs: none * @details Outputs: bloodPumpRotorCounter * @param value override blood pump rotor counter value * @return TRUE if override successful, FALSE if not *************************************************************************/ BOOL testSetBloodPumpRotorCountOverride( U32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; bloodPumpRotorCounter.ovData = value; bloodPumpRotorCounter.override = OVERRIDE_KEY; } return result; } /*********************************************************************//** * @brief * The testResetBloodPumpRotorCountOverride function resets the override * of the blood pump rotor counter. * @details Inputs: none * @details Outputs: bloodPumpRotorCounter * @return TRUE if reset successful, FALSE if not *************************************************************************/ BOOL testResetBloodPumpRotorCountOverride( void ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { result = TRUE; bloodPumpRotorCounter.override = OVERRIDE_RESET; bloodPumpRotorCounter.ovData = bloodPumpRotorCounter.ovInitData; } return result; } /*********************************************************************//** * @brief * The testSetBloodPumpTargetDutyCycle function sets the duty cycle of the * blood pump by calling setBloodPumpTargetFlowRate. * @details Inputs: none * @details Outputs: none * @param value duty cycle of the blood pump (as a percentage). * @return TRUE if reset successful, FALSE if not *************************************************************************/ BOOL testSetBloodPumpTargetDutyCycle( F32 value ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { setBloodPumpTargetFlowRate( (U32)BP_ML_PER_MIN_FROM_PWM( value ), MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_OPEN_LOOP ); result = TRUE; } return result; } /**@}*/