Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -r5dea4d2934a06e63b87c74732d5d5d7756bdc62a -r289696b3b5e15120cfecbe447ff45ef093959056 --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 5dea4d2934a06e63b87c74732d5d5d7756bdc62a) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 289696b3b5e15120cfecbe447ff45ef093959056) @@ -23,9 +23,11 @@ #include "Messaging.h" #include "OperationModes.h" #include "PersistentAlarm.h" -//#include "PIControllers.h" +#include "PIControllers.h" +#include "Pressures.h" #include "TaskGeneral.h" #include "Timers.h" +#include "TxParams.h" #include "Utilities.h" /** @@ -40,22 +42,23 @@ #define MAX_SETTABLE_BLOOD_FLOW_RATE 700 ///< Maximum settable blood flow rate (in mL/min). -#define BP_CONTROL_INTERVAL_SEC 10 ///< Blood pump control interval (in seconds). +#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.0001F ///< P term for blood pump control -#define BP_I_COEFFICIENT 0.00075F ///< I term for blood pump control +#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 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). +#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_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. @@ -73,25 +76,30 @@ /// 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_RATE_FROM_RPM( rpm ) ( rpm / 5 ) ///< Macro to estimate a flow rate (mL/min) from a given speed (RPM). -#define BP_RPM_FROM_RATE( rate ) ( rate * 5 ) ///< Macro to estimate a pump speed (RPM) from a given flow rate (mL/min). +#define BP_RAMP_STEP_SPEED_RPM 50 ///< Blood pump ramp step size (in RPM). -#define BP_RAMP_STEP_SPEED_RPM 5 ///< Blood pump ramp step size (in RPM). - -#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_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 1.0F ///< Blood pump torque period resolution in microseconds (1 us). +#define BP_1KHZ_TO_TORQUE_CONVERSION_MNM 10.0F ///< Blood pump 1kHz to torque conversion in milli-newtonmeter + /// 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 @@ -118,11 +126,15 @@ 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. @@ -132,17 +144,19 @@ static U32 bpControlTimerCounter; ///< Determines when to perform control on blood flow. -static U32 bpRotorRevStartTime; ///< Blood pump rotor rotation start time (in ms). static OVERRIDE_U32_T bloodPumpRotorCounter; ///< Running counter for blood pump rotor revolutions. -static U32 bpRotorSpeedTooFastPulseCount; ///< Counter for rotor pulses indicating RPM > 100. -static BOOL bpStopAtHomePosition; ///< Stop blood pump at next home position. +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 ); @@ -152,8 +166,12 @@ 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 ); @@ -166,6 +184,8 @@ *************************************************************************/ void initBloodFlow( void ) { + U32 i; + // Initialize peristaltic pump driver initPeristalticPumpDriver(); @@ -180,9 +200,8 @@ bloodPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; bpControlTimerCounter = 0; bloodFlowDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; - bpRotorRevStartTime = 0; + bpHomeRequested = FALSE; bpHomeStartTime = 0; - bpStopAtHomePosition = FALSE; // Initialize overrides bloodFlowDataPublishInterval.data = BLOOD_FLOW_DATA_PUB_INTERVAL; @@ -205,22 +224,37 @@ 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 - memset( &rpmReadings[ 0 ], 0, sizeof( rpmReadings ) ); - rpmReadingsIdx = 0; - rpmReadingsTotal = 0.0; - rpmReadingsCount = 0; - filteredBloodPumpSpeed = 0.0; - - // Reset pump speed moving average and rotor count + 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, MIN_BLOOD_PUMP_PWM_DUTY_CYCLE, -// BP_P_COEFFICIENT, BP_I_COEFFICIENT, -// MIN_BLOOD_PUMP_PWM_DUTY_CYCLE, MAX_BLOOD_PUMP_PWM_DUTY_CYCLE ); + 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 ); @@ -262,7 +296,7 @@ // isFlowInrange = TRUE; // } #endif - // Verify flow rate of if the bypass flow limit has been enabled + // Verify flow rate valid if ( TRUE == isFlowInrange ) { resetBloodPumpRPMMovingAverage(); @@ -331,14 +365,14 @@ *************************************************************************/ 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 = 0.0F; // TODO alpha * BP_ML_PER_ROTOR_REV * rotSpd; + 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; } @@ -357,68 +391,29 @@ bloodPumpSetSpeedRPM = 0; isBloodPumpOn = FALSE; bpControlTimerCounter = 0; -// resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, MIN_BLOOD_PUMP_PWM_DUTY_CYCLE ); + setPeristalticPumpHardStop(); + resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, 0.0F, 0.0F ); } /*********************************************************************//** * @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 \b Inputs: bpRotorRevStartTime, bpStopAtHomePosition - * @details \b 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 \b Inputs: bloodPumpState - * @details \b Outputs: bpStopAtHomePosition, bpHomeStartTime, blood pump started (slow) - * @return none + * @details \b Outputs: bpHomeRequested + * @return TRUE if home request accepted, FALSE if not *************************************************************************/ BOOL homeBloodPump( void ) { - BOOL result = FALSE; - if ( BLOOD_PUMP_OFF_STATE == bloodPumpState ) { - bpStopAtHomePosition = TRUE; - bpHomeStartTime = getMSTimerCount(); - result = setBloodPumpTargetRPM( BP_HOME_SPEED, MOTOR_DIR_FORWARD ); + bpHomeRequested = TRUE; } + else + { + bpHomeRequested = FALSE; + } - return result; + return bpHomeRequested; } /*********************************************************************//** @@ -482,7 +477,7 @@ { bloodPumpRotorCounter.data = 0; - if ( TRUE == getTestConfigStatus( TEST_CONFIG_USE_WORN_CARTRIDGE ) ) +// if ( TRUE == getTestConfigStatus( TEST_CONFIG_USE_WORN_CARTRIDGE ) ) { bloodPumpRotorCounter.data = BP_MAX_ROTOR_COUNT_FOR_WEAR; } @@ -492,8 +487,8 @@ * @brief * The execBloodFlowMonitor function executes the blood flow monitor. * @details \b Inputs: none - * @details \b Outputs: measuredBloodFlowRate, adcBloodPumpMCSpeedRPM, - * adcBloodPumpMCCurrentmA + * @details \b Outputs: BP sensor readings updated, filteredBloodPumpSpeed, + * bloodPumpSpeedRPM, bloodPumpDirection, measuredBloodFlowRate * @return none *************************************************************************/ void execBloodFlowMonitor( void ) @@ -505,14 +500,17 @@ // 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( (F32)getPeristalticPumpMeasSpeed() ); + filterBloodPumpRPMReadings( getPeristalticPumpMeasSpeed() ); bloodPumpSpeedRPM.data = filteredBloodPumpSpeed; bloodPumpDirection = ( getMeasuredBloodPumpSpeed() < 0.0F ? MOTOR_DIR_REVERSE : MOTOR_DIR_FORWARD ); - //measuredBloodFlowRate.data = calcBloodFlow(); // TODO-restore when flow estimation function is implemented - measuredBloodFlowRate.data = BP_RATE_FROM_RPM( getMeasuredBloodPumpSpeed() ); // TODO-this is temporary hack-eventually need flow estimation + 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 ) @@ -533,6 +531,8 @@ /*********************************************************************//** * @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 @@ -546,6 +546,10 @@ bloodPumpState = handleBloodPumpOffState(); break; + case BLOOD_PUMP_HOMING_STATE: + bloodPumpState = handleBloodPumpHomingState(); + break; + case BLOOD_PUMP_RAMPING_UP_STATE: bloodPumpState = handleBloodPumpRampingUpState(); break; @@ -571,8 +575,9 @@ * @brief * The handleBloodPumpOffState function handles the blood pump off state * of the blood pump controller state machine. - * @details \b Inputs: targetBloodFlowRate, bloodPumpDirection - * @details \b Outputs: bloodPumpPWMDutyCyclePctSet, bloodPumpDirectionSet, isBloodPumpOn + * @details \b Inputs: targetBloodFlowRate, bpHomeRequested + * @details \b Outputs: bloodPumpSetSpeedRPM, isBloodPumpOn, bpHomeRequested, + * bpHomeStartTime * @return next state *************************************************************************/ static BLOOD_PUMP_STATE_T handleBloodPumpOffState( void ) @@ -584,22 +589,64 @@ { // Start ramp up to target flow rate bloodPumpSetSpeedRPM = BP_RAMP_STEP_SPEED_RPM; - setBPDirection( bloodPumpDirectionSet ); - setBPSetSpeed( bloodPumpSetSpeedRPM ); + 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 @@ -615,16 +662,16 @@ { // Start ramp down to stop bloodPumpSetSpeedRPM -= BP_RAMP_STEP_SPEED_RPM; - setBPSetSpeed( bloodPumpSetSpeedRPM ); + 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; - setBPSetSpeed( bloodPumpSetSpeedRPM ); -// resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, bloodPumpSetSpeedRPM ); + setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); + resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, (F32)bloodPumpSetSpeedRPM, 0.0F ); bpControlTimerCounter = 0; result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; } @@ -633,7 +680,7 @@ { bloodPumpSetSpeedRPM += BP_RAMP_STEP_SPEED_RPM; bloodPumpSetSpeedRPM = MIN( bloodPumpSetSpeedRPM, bloodPumpRampToSpeedRPM ); - setBPSetSpeed( bloodPumpSetSpeedRPM ); + setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); } return result; @@ -661,9 +708,8 @@ else if ( bloodPumpSetSpeedRPM <= bloodPumpRampToSpeedRPM ) { resetBloodPumpRPMMovingAverage(); - bloodPumpSetSpeedRPM = 0; -// resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, bloodPumpSetSpeedRPM ); - setBPSetSpeed( bloodPumpSetSpeedRPM ); + resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, (F32)bloodPumpSetSpeedRPM, 0.0F ); + setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); bpControlTimerCounter = 0; result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; } @@ -672,7 +718,7 @@ { bloodPumpSetSpeedRPM -= BP_RAMP_STEP_SPEED_RPM; bloodPumpSetSpeedRPM = MAX( bloodPumpSetSpeedRPM, bloodPumpRampToSpeedRPM ); - setBPSetSpeed( bloodPumpSetSpeedRPM ); + setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); } return result; @@ -697,12 +743,10 @@ { F32 tgtFlow = (F32)targetBloodFlowRate; F32 actFlow = getMeasuredBloodFlowRate(); - F32 newRPM = bloodPumpSetSpeedRPM; // TODO - don't set speed here when control is implemented below. + F32 newRPM= runPIController( PI_CONTROLLER_ID_BLOOD_FLOW, tgtFlow, actFlow ); -// newRPM = runPIController( PI_CONTROLLER_ID_BLOOD_FLOW, tgtFlow, actFlow ); -// newRPM = ( newRPM < 0.0F ? 0.0F : newRPM ); bloodPumpSetSpeedRPM = (U32)((S32)(newRPM)); - setBPSetSpeed( bloodPumpSetSpeedRPM ); + setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); } bpControlTimerCounter = 0; } @@ -732,13 +776,8 @@ *************************************************************************/ F32 getMeasuredBloodFlowRate( void ) { - F32 result = measuredBloodFlowRate.data; + F32 result = getF32OverrideValue( &measuredBloodFlowRate ); - if ( OVERRIDE_KEY == measuredBloodFlowRate.override ) - { - result = measuredBloodFlowRate.ovData; - } - return result; } @@ -752,13 +791,8 @@ *************************************************************************/ F32 getMeasuredBloodPumpRotorSpeed( void ) { - F32 result = bloodPumpRotorSpeedRPM.data; + F32 result = getF32OverrideValue( &bloodPumpRotorSpeedRPM ); - if ( OVERRIDE_KEY == bloodPumpRotorSpeedRPM.override ) - { - result = bloodPumpRotorSpeedRPM.ovData; - } - return result; } @@ -772,18 +806,73 @@ *************************************************************************/ F32 getMeasuredBloodPumpSpeed( void ) { - F32 result = bloodPumpSpeedRPM.data; + F32 result = getF32OverrideValue( &bloodPumpSpeedRPM ); - if ( OVERRIDE_KEY == bloodPumpSpeedRPM.override ) - { - result = bloodPumpSpeedRPM.ovData; - } + return result; +} +/*********************************************************************//** + * @brief + * The getMeasuredBloodPumpTorque function gets the measured blood pump + * torque in mN.m. + * @details \b Inputs: bloodPumpTorquemNm + * @details \b Outputs: none + * @return the current blood pump torque (in mN.m). + *************************************************************************/ +F32 getMeasuredBloodPumpTorque( void ) +{ + F32 result = getF32OverrideValue( &bloodPumpTorquemNm ); + 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. @@ -801,22 +890,24 @@ TD_OP_MODE_T opMode = getCurrentOperationMode(); U32 hallSensor = 0; // TODO gioGetBit( hetPORT1, BP_ROTOR_HALL_SENSOR_NHET_ID ); - payload.setFlowRate = targetBloodFlowRate; - payload.measFlow = getMeasuredBloodFlowRate(); - payload.measRotorSpd = getMeasuredBloodPumpRotorSpeed(); - payload.measPumpSpd = getMeasuredBloodPumpSpeed(); -// payload.measCurr = getMeasuredBloodPumpMCCurrent(); - payload.setRPM = bloodPumpSetSpeedRPM; - payload.rotorCount = getBloodPumpRotorCount(); + payload.h4SetFlowRate = targetBloodFlowRate; + payload.h4MeasFlow = getMeasuredBloodFlowRate(); + payload.h4MeasRotorSpd = getMeasuredBloodPumpRotorSpeed(); + payload.h4MeasPumpSpd = getMeasuredBloodPumpSpeed(); + payload.h4MeasTorquemNm = getMeasuredBloodPumpTorque(); + payload.h4SetRPM = bloodPumpSetSpeedRPM; + payload.h4RotorCount = 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 ); // TODO - restore when Tx Param Mode implemented -// } -// else -// { - payload.presFlow = 0; + payload.h4PresFlow = getTreatmentParameterU32( TREATMENT_PARAM_BLOOD_FLOW ); } - payload.rotorHall = ( hallSensor > 0 ? 0 : 1 ); // 1=home, 0=not home + else + { + payload.h4PresFlow = 0; + } + payload.h6RotorHallState = ( hallSensor > 0 ? 0 : 1 ); // 1=home, 0=not home + //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; } @@ -834,8 +925,8 @@ { rpmReadingsIdx = 0; rpmReadingsCount = 0; - rpmReadingsTotal = 0.0; - filteredBloodPumpSpeed = 0.0; + rpmReadingsTotal = 0.0F; + filteredBloodPumpSpeed = 0.0F; } /*********************************************************************//** @@ -862,9 +953,27 @@ /*********************************************************************//** * @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)getH4TorqueCount() * BP_TORQUE_PERIOD_RESOLUTION_US; + F32 torqueResolutionS = torqueResolutionUS / (F32)US_PER_SECOND; + F32 frequencyHz = 1.0F / torqueResolutionS; + F32 frequencyKHz = frequencyHz / (F32)HZ_PER_KHZ; + F32 torquemNm = frequencyKHz * BP_1KHZ_TO_TORQUE_CONVERSION_MNM; + + return torquemNm; +} + +/*********************************************************************//** + * @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. + * 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, @@ -877,13 +986,6 @@ { // 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 ) ) // { @@ -1239,4 +1341,96 @@ 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; +} + /**@}*/