Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -rf05acb2d013aa3dec1b59d793b8876aed928b5aa -r051326e2671e8d5b3e99eaa109ea549e94a929f3 --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision f05acb2d013aa3dec1b59d793b8876aed928b5aa) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 051326e2671e8d5b3e99eaa109ea549e94a929f3) @@ -22,8 +22,9 @@ #include "FpgaTD.h" #include "Messaging.h" #include "OperationModes.h" +#include "PeristalticPump.h" #include "PersistentAlarm.h" -#include "PIControllers.h" +//#include "PIControllers.h" #include "TaskGeneral.h" #include "Timers.h" #include "Utilities.h" @@ -36,43 +37,31 @@ // ********** 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 BLOOD_FLOW_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) -#define MAX_SETTABLE_BLOOD_FLOW_RATE 700 ///< Maximum settable blood flow rate (in mL/min). +#define 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). -#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_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). +#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 ) +#define BP_MAX_ROTOR_SPEED_RPM 100.0F ///< Maximum rotor speed allowed for blood pump. -/// 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_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. -#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. @@ -82,186 +71,163 @@ /// 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. +/// 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 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. +#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). -/// 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_RAMP_STEP_SPEED_RPM 5 ///< Blood pump ramp step size (in RPM). -#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_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 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. +#define DATA_PUBLISH_COUNTER_START_COUNT 20 ///< Data publish counter start count. -/// 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 ) ) ) +#define SIZE_OF_ROLLING_AVG 20 ///< Number of pump speed samples in rolling average. -/// 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_OFF_STATE = 0, ///< Blood pump off state + BLOOD_PUMP_RAMPING_UP_STATE, ///< Blood pump ramping up state + BLOOD_PUMP_RAMPING_DN_STATE, ///< Blood pump ramping down state + BLOOD_PUMP_CONTROL_TO_TARGET_STATE, ///< Blood pump controlling to target state + NUM_OF_BLOOD_PUMP_STATES ///< Number of blood pump states } BLOOD_PUMP_STATE_T; /// Enumeration of blood pump self-test states. typedef enum BloodFlow_Self_Test_States { - BLOOD_FLOW_SELF_TEST_STATE_START = 0, ///< Blood pump self-test start state - BLOOD_FLOW_TEST_STATE_IN_PROGRESS, ///< Blood pump self-test in progress state - BLOOD_FLOW_TEST_STATE_COMPLETE, ///< Blood pump self-test completed state - NUM_OF_BLOOD_FLOW_SELF_TEST_STATES ///< Number of blood pump self-test states + BLOOD_FLOW_SELF_TEST_STATE_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 BLOOD_PUMP_STATE_T bloodPumpState; ///< Current state of blood pump controller state machine static U32 bloodFlowDataPublicationTimerCounter; ///< Used to schedule blood flow data publication to CAN bus -static BOOL isBloodPumpOn = 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. +static BOOL isBloodPumpOn; ///< Blood pump is currently running +static MOTOR_DIR_T bloodPumpDirection; ///< Measured blood flow direction +static MOTOR_DIR_T bloodPumpDirectionSet; ///< Set blood flow direction +static PUMP_CONTROL_MODE_T bloodPumpControlMode; ///< Requested blood pump control mode. +static U16 bloodPumpRampToSpeedRPM; ///< Estimated target speed that we want to ramp to. +static U16 bloodPumpSetSpeedRPM; ///< Current set speed for the blood pump. -/// 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 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 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; ///< Index for next sample in rolling average array. +static F32 rpmReadingsTotal; ///< Rolling total - used to calc average. +static U32 rpmReadingsCount; ///< Number of samples in RPM rolling average buffer. +static F32 filteredBloodPumpSpeed; ///< Filtered blood pump speed used in blood flow estimation. -static 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; ///< Determines when to perform control on blood flow. -static U32 bpControlTimerCounter = 0; ///< 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 U32 bpHomeStartTime; ///< When did blood pump home command begin? (in ms). -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 TD_PUMPS_CAL_RECORD_T bloodPumpCalRecord; ///< Blood pump calibration record. -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 U32 getBloodPumpRotorCount( 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 ); +static void publishBloodFlowData( void ); + /*********************************************************************//** * @brief - * The initBloodFlow function initializes the BloodFlow module. + * The initBloodFlow function initializes the BloodFlow unit. * @details Inputs: none - * @details Outputs: bloodFlowDataPublicationTimerCounter + * @details Outputs: Blood flow unit initialized * @return none *************************************************************************/ void initBloodFlow( void ) { - U32 i; + // Initialize peristaltic pump driver + initPeristalticPumpDriver(); + // Initialize core variables + bloodPumpState = BLOOD_PUMP_OFF_STATE; + targetBloodFlowRate = 0; + bloodPumpSetSpeedRPM = 0; + bloodPumpRampToSpeedRPM = 0; + isBloodPumpOn = FALSE; + bloodPumpDirection = MOTOR_DIR_FORWARD; + bloodPumpDirectionSet = MOTOR_DIR_FORWARD; + bloodPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; + bpControlTimerCounter = 0; bloodFlowDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; - bpRotorSpeedTooFastPulseCount = 0; + bpRotorRevStartTime = 0; + bpHomeStartTime = 0; + bpStopAtHomePosition = FALSE; - signalBloodPumpHardStop(); - setBloodPumpDirection( MOTOR_DIR_FORWARD ); + // Initialize overrides + bloodFlowDataPublishInterval.data = BLOOD_FLOW_DATA_PUB_INTERVAL; + bloodFlowDataPublishInterval.ovData = BLOOD_FLOW_DATA_PUB_INTERVAL; + bloodFlowDataPublishInterval.ovInitData = BLOOD_FLOW_DATA_PUB_INTERVAL; + bloodFlowDataPublishInterval.override = OVERRIDE_RESET; + measuredBloodFlowRate.data = 0.0F; + measuredBloodFlowRate.ovData = 0.0F; + measuredBloodFlowRate.ovInitData = 0.0F; + measuredBloodFlowRate.override = OVERRIDE_RESET; + bloodPumpRotorSpeedRPM.data = 0.0F; + bloodPumpRotorSpeedRPM.ovData = 0.0F; + bloodPumpRotorSpeedRPM.ovInitData = 0.0F; + bloodPumpRotorSpeedRPM.override = OVERRIDE_RESET; + bloodPumpSpeedRPM.data = 0.0F; + bloodPumpSpeedRPM.ovData = 0.0F; + bloodPumpSpeedRPM.ovInitData = 0.0F; + bloodPumpSpeedRPM.override = OVERRIDE_RESET; + bloodPumpRotorCounter.data = 0; + bloodPumpRotorCounter.ovData = 0; + bloodPumpRotorCounter.ovInitData = 0; + bloodPumpRotorCounter.override = OVERRIDE_RESET; - // Zero rolling pump speed average buffer - resetBloodPumpRPMMovingAverage(); + // Initialize pump speed filter + memset( &rpmReadings[ 0 ], 0, sizeof( rpmReadings ) ); + rpmReadingsIdx = 0; + rpmReadingsTotal = 0.0; + rpmReadingsCount = 0; + filteredBloodPumpSpeed = 0.0; - // Zero motor hall sensors counts buffer - bpMotorSpeedCalcIdx = 0; - for ( i = 0; i < BP_SPEED_CALC_BUFFER_LEN; i++ ) - { - bpLastMotorHallSensorCounts[ i ] = getFPGABloodPumpHallSensorCount(); - } + // Reset pump speed moving average and rotor count + resetBloodPumpRPMMovingAverage(); 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, 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 ); +// initPersistentAlarm( ALARM_ID_HD_BLOOD_PUMP_OFF_CHECK, 0, BP_OFF_ERROR_PERSIST ); +// initPersistentAlarm( ALARM_ID_HD_BLOOD_PUMP_MOTOR_SPEED_CHECK, 0, BP_MOTOR_SPEED_ERROR_PERSIST ); +// initPersistentAlarm( ALARM_ID_HD_BLOOD_PUMP_MC_DIRECTION_CHECK, 0, BP_DIRECTION_ERROR_PERSIST ); +// initPersistentAlarm( ALARM_ID_HD_BLOOD_PUMP_ROTOR_SPEED_TOO_HIGH, 0, BP_MAX_ROTOR_SPEED_ERROR_PERSIST ); } /*********************************************************************//** @@ -282,6 +248,7 @@ // Direction change while pump is running is not allowed if ( ( FALSE == isBloodPumpOn ) || ( 0 == flowRate ) || ( dir == bloodPumpDirectionSet ) ) { + S32 prevFlowRate = targetBloodFlowRate; S32 dirFlowRate = ( dir == MOTOR_DIR_FORWARD ? (S32)flowRate : (S32)flowRate * -1 ); // Don't interrupt pump control unless rate or mode is changing @@ -290,71 +257,42 @@ 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; - } +// 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; + bloodPumpDirectionSet = 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 + bloodPumpRampToSpeedRPM = BP_RPM_FROM_RATE( flowRate ); - switch ( bloodPumpState ) + if ( BLOOD_PUMP_CONTROL_TO_TARGET_STATE == 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; + // Start ramp to new target in appropriate direction + if ( abs( targetBloodFlowRate ) > abs( prevFlowRate ) ) + { + bloodPumpState = BLOOD_PUMP_RAMPING_UP_STATE; + } + else + { + bloodPumpState = BLOOD_PUMP_RAMPING_DN_STATE; + } } result = TRUE; } else // Requested flow rate too high { #ifndef _RELEASE_ - if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMPS_FLOW_LIMITS ) != SW_CONFIG_ENABLE_VALUE ) +// 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 ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_BLOOD_FLOW_SET_TOO_HIGH, flowRate ) } } } @@ -376,9 +314,8 @@ 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 ); + result = setBloodPumpTargetFlowRate( (U32)BP_RATE_FROM_RPM( rpm ), dir, PUMP_CONTROL_MODE_OPEN_LOOP ); return result; } @@ -394,14 +331,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 = 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 = 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; return flow; } @@ -416,11 +353,11 @@ void signalBloodPumpHardStop( void ) { targetBloodFlowRate = 0; - stopBloodPump(); bloodPumpState = BLOOD_PUMP_OFF_STATE; - bloodPumpPWMDutyCyclePct = BP_PWM_ZERO_OFFSET; + bloodPumpSetSpeedRPM = 0; + isBloodPumpOn = FALSE; bpControlTimerCounter = 0; - resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, MIN_BLOOD_PUMP_PWM_DUTY_CYCLE ); +// resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, MIN_BLOOD_PUMP_PWM_DUTY_CYCLE ); } /*********************************************************************//** @@ -486,26 +423,13 @@ /*********************************************************************//** * @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 ) +static U32 getBloodPumpRotorCount( void ) { U32 result = bloodPumpRotorCounter.data; @@ -574,38 +498,36 @@ *************************************************************************/ 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 ); + TD_OP_MODE_T opMode = getCurrentOperationMode(); +// U16 bpRPM = getIntADCReading( INT_ADC_BLOOD_PUMP_SPEED ); // TODO - bring in current monitoring later +// U16 bpmA = getIntADCReading( INT_ADC_BLOOD_PUMP_MOTOR_CURRENT ); - 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; + // Update pump feedback from FPGA + readPeristalticPumps(); - filterBloodPumpRPMReadings( getMeasuredBloodPumpMCSpeed() ); - measuredBloodFlowRate.data = calcBloodFlow(); // Pressure and rotor speed already filtered as inputs to calc, so no need to filter flow any further +// 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; - // Calculate blood pump motor speed/direction from hall sensor count - updateBloodPumpSpeedAndDirectionFromHallSensors(); + filterBloodPumpRPMReadings( (F32)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 - // 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(); + // Do not start enforcing checks until out of init/POST mode TODO-add checks later +// if ( opMode != MODE_INIT ) +// { +// // Check pump direction +// checkBloodPumpDirection(); +// // Check pump speeds +// checkBloodPumpSpeeds(); +// // Check for home position, zero/low speed +// checkBloodPumpRotor(); +// } +// else +// { +// lastBloodPumpDirectionCount = getFPGABloodPumpHallSensorStatus() & PUMP_DIR_ERROR_COUNT_MASK; +// } } /*********************************************************************//** @@ -617,6 +539,7 @@ *************************************************************************/ void execBloodFlowController( void ) { + // Control blood pump switch ( bloodPumpState ) { case BLOOD_PUMP_OFF_STATE: @@ -627,7 +550,7 @@ bloodPumpState = handleBloodPumpRampingUpState(); break; - case BLOOD_PUMP_RAMPING_DOWN_STATE: + case BLOOD_PUMP_RAMPING_DN_STATE: bloodPumpState = handleBloodPumpRampingDownState(); break; @@ -636,9 +559,12 @@ break; default: - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_BLOOD_FLOW_INVALID_BLOOD_PUMP_STATE, bloodPumpState ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_BLOOD_FLOW_INVALID_BLOOD_PUMP_STATE, bloodPumpState ) break; } + + // Publish blood flow data on interval + publishBloodFlowData(); } /*********************************************************************//** @@ -656,18 +582,17 @@ // 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(); + // Start ramp up to target flow rate + bloodPumpSetSpeedRPM = BP_RAMP_STEP_SPEED_RPM; + setBPDirection( bloodPumpDirectionSet ); + setBPSetSpeed( bloodPumpSetSpeedRPM ); isBloodPumpOn = TRUE; result = BLOOD_PUMP_RAMPING_UP_STATE; } else { isBloodPumpOn = FALSE; + bloodPumpSetSpeedRPM = 0; } return result; @@ -677,8 +602,8 @@ * @brief * The handleBloodPumpRampingUpState function handles the ramp up state * of the blood pump controller state machine. - * @details Inputs: bloodPumpPWMDutyCyclePctSet - * @details Outputs: bloodPumpPWMDutyCyclePctSet + * @details Inputs: bloodPumpRampToSpeedRPM, bloodPumpSetSpeedRPM + * @details Outputs: bloodPumpSetSpeedRPM * @return next state *************************************************************************/ static BLOOD_PUMP_STATE_T handleBloodPumpRampingUpState( void ) @@ -689,26 +614,26 @@ if ( 0 == targetBloodFlowRate ) { // Start ramp down to stop - bloodPumpPWMDutyCyclePctSet -= MAX_BLOOD_PUMP_PWM_STEP_DN_CHANGE; - setBloodPumpControlSignalPWM( bloodPumpPWMDutyCyclePctSet ); - result = BLOOD_PUMP_RAMPING_DOWN_STATE; + bloodPumpSetSpeedRPM -= BP_RAMP_STEP_SPEED_RPM; + setBPSetSpeed( bloodPumpSetSpeedRPM ); + result = BLOOD_PUMP_RAMPING_DN_STATE; } // Have we reached end of ramp up? - else if ( bloodPumpPWMDutyCyclePctSet >= bloodPumpPWMDutyCyclePct ) + else if ( bloodPumpSetSpeedRPM >= bloodPumpRampToSpeedRPM ) { resetBloodPumpRPMMovingAverage(); - bloodPumpPWMDutyCyclePctSet = bloodPumpPWMDutyCyclePct; - resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, bloodPumpPWMDutyCyclePctSet ); - bloodPumpControlModeSet = bloodPumpControlMode; - setBloodPumpControlSignalPWM( bloodPumpPWMDutyCyclePctSet ); + bloodPumpSetSpeedRPM = bloodPumpRampToSpeedRPM; + setBPSetSpeed( bloodPumpSetSpeedRPM ); +// resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, bloodPumpSetSpeedRPM ); bpControlTimerCounter = 0; result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; } // Continue ramp up else { - bloodPumpPWMDutyCyclePctSet += MAX_BLOOD_PUMP_PWM_STEP_UP_CHANGE; - setBloodPumpControlSignalPWM( bloodPumpPWMDutyCyclePctSet ); + bloodPumpSetSpeedRPM += BP_RAMP_STEP_SPEED_RPM; + bloodPumpSetSpeedRPM = MIN( bloodPumpSetSpeedRPM, bloodPumpRampToSpeedRPM ); + setBPSetSpeed( bloodPumpSetSpeedRPM ); } return result; @@ -718,36 +643,36 @@ * @brief * The handleBloodPumpRampingDownState function handles the ramp down state * of the blood pump controller state machine. - * @details Inputs: bloodPumpPWMDutyCyclePctSet - * @details Outputs: bloodPumpPWMDutyCyclePctSet + * @details Inputs: bloodPumpRampToSpeedRPM, bloodPumpSetSpeedRPM + * @details Outputs: bloodPumpSetSpeedRPM * @return next state *************************************************************************/ static BLOOD_PUMP_STATE_T handleBloodPumpRampingDownState( void ) { - BLOOD_PUMP_STATE_T result = BLOOD_PUMP_RAMPING_DOWN_STATE; + BLOOD_PUMP_STATE_T result = BLOOD_PUMP_RAMPING_DN_STATE; // Have we essentially reached zero speed? - if ( bloodPumpPWMDutyCyclePctSet < (MAX_BLOOD_PUMP_PWM_STEP_DN_CHANGE + BP_PWM_ZERO_OFFSET) ) + if ( bloodPumpSetSpeedRPM < BP_RAMP_STEP_SPEED_RPM ) { signalBloodPumpHardStop(); result = BLOOD_PUMP_OFF_STATE; } // Have we reached end of ramp down? - else if ( bloodPumpPWMDutyCyclePctSet <= bloodPumpPWMDutyCyclePct ) + else if ( bloodPumpSetSpeedRPM <= bloodPumpRampToSpeedRPM ) { resetBloodPumpRPMMovingAverage(); - bloodPumpPWMDutyCyclePctSet = bloodPumpPWMDutyCyclePct; - resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, bloodPumpPWMDutyCyclePctSet ); - bloodPumpControlModeSet = bloodPumpControlMode; - setBloodPumpControlSignalPWM( bloodPumpPWMDutyCyclePctSet ); + bloodPumpSetSpeedRPM = 0; +// resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, bloodPumpSetSpeedRPM ); + setBPSetSpeed( bloodPumpSetSpeedRPM ); bpControlTimerCounter = 0; result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; } // Continue ramp down else { - bloodPumpPWMDutyCyclePctSet -= MAX_BLOOD_PUMP_PWM_STEP_DN_CHANGE; - setBloodPumpControlSignalPWM( bloodPumpPWMDutyCyclePctSet ); + bloodPumpSetSpeedRPM -= BP_RAMP_STEP_SPEED_RPM; + bloodPumpSetSpeedRPM = MAX( bloodPumpSetSpeedRPM, bloodPumpRampToSpeedRPM ); + setBPSetSpeed( bloodPumpSetSpeedRPM ); } return result; @@ -757,7 +682,7 @@ * @brief * The handleBloodPumpControlToTargetState function handles the "control to * target" state of the blood pump controller state machine. - * @details Inputs: none + * @details Inputs: bloodPumpControlMode * @details Outputs: bloodPumpState * @return next state *************************************************************************/ @@ -768,15 +693,16 @@ // Control at set interval if ( ++bpControlTimerCounter >= BP_CONTROL_INTERVAL ) { - if ( bloodPumpControlModeSet == PUMP_CONTROL_MODE_CLOSED_LOOP ) + if ( bloodPumpControlMode == PUMP_CONTROL_MODE_CLOSED_LOOP ) { F32 tgtFlow = (F32)targetBloodFlowRate; F32 actFlow = getMeasuredBloodFlowRate(); - F32 newPWM; + F32 newRPM = bloodPumpSetSpeedRPM; // TODO - don't set speed here when control is implemented below. - newPWM = runPIController( PI_CONTROLLER_ID_BLOOD_FLOW, tgtFlow, actFlow ); - bloodPumpPWMDutyCyclePctSet = newPWM; - setBloodPumpControlSignalPWM( newPWM ); +// newRPM = runPIController( PI_CONTROLLER_ID_BLOOD_FLOW, tgtFlow, actFlow ); +// newRPM = ( newRPM < 0.0F ? 0.0F : newRPM ); + bloodPumpSetSpeedRPM = (U32)((S32)(newRPM)); + setBPSetSpeed( bloodPumpSetSpeedRPM ); } bpControlTimerCounter = 0; } @@ -786,76 +712,6 @@ /*********************************************************************//** * @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 @@ -928,46 +784,6 @@ /*********************************************************************//** * @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, @@ -981,27 +797,26 @@ 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 ); + TD_OP_MODE_T opMode = getCurrentOperationMode(); + U32 hallSensor = 0; // TODO gioGetBit( hetPORT1, BP_ROTOR_HALL_SENSOR_NHET_ID ); - payload.setPoint = targetBloodFlowRate; + payload.setFlowRate = targetBloodFlowRate; payload.measFlow = getMeasuredBloodFlowRate(); payload.measRotorSpd = getMeasuredBloodPumpRotorSpeed(); payload.measPumpSpd = getMeasuredBloodPumpSpeed(); - payload.measMCSpd = getMeasuredBloodPumpMCSpeed(); - payload.measMCCurr = getMeasuredBloodPumpMCCurrent(); - payload.pwmDC = bloodPumpPWMDutyCyclePctSet * FRACTION_TO_PERCENT_FACTOR; +// payload.measCurr = getMeasuredBloodPumpMCCurrent(); + payload.setRPM = bloodPumpSetSpeedRPM; 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 = getTreatmentParameterU32( TREATMENT_PARAM_BLOOD_FLOW ); // TODO - restore when Tx Param Mode implemented +// } +// 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 ) ); + broadcastData( MSG_ID_TD_BLOOD_PUMP_DATA, COMM_BUFFER_OUT_CAN_TD_BROADCAST, (U08*)&payload, sizeof( BLOOD_PUMP_STATUS_PAYLOAD_T ) ); bloodFlowDataPublicationTimerCounter = 0; } } @@ -1046,50 +861,6 @@ /*********************************************************************//** * @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. @@ -1101,26 +872,26 @@ *************************************************************************/ 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; - } +// 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; +// } } /*********************************************************************//** @@ -1133,53 +904,53 @@ *************************************************************************/ 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 ); - } +// 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 ); +// } } /*********************************************************************//** @@ -1196,101 +967,73 @@ *************************************************************************/ 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; - - } +// 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 @@ -1301,18 +1044,18 @@ { 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 ); +// 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; +// } - if ( TRUE == calStatus ) - { - result = SELF_TEST_STATUS_PASSED; - } - else - { - result = SELF_TEST_STATUS_FAILED; - } - return result; } @@ -1324,381 +1067,171 @@ /*********************************************************************//** * @brief - * The testSetBloodFlowDataPublishIntervalOverride function overrides the + * The testBloodFlowDataPublishIntervalOverride 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) + * @param message Override message from Dialin which includes the interval + * (in ms) to override the blood pump data broadcast interval to. * @return TRUE if override successful, FALSE if not *************************************************************************/ -BOOL testSetBloodFlowDataPublishIntervalOverride( U32 value ) +BOOL testBloodFlowDataPublishIntervalOverride( MESSAGE_T *message ) { - BOOL result = FALSE; + BOOL result = u32BroadcastIntervalOverride( message, &bloodFlowDataPublishInterval, TASK_GENERAL_INTERVAL ); - 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. + * The testSetTargetBloodFlowRateOverride function commands the blood pump to + * run at a given flow rate (mL/min) and in a given control mode. * @details Inputs: none - * @details Outputs: bloodFlowDataPublishInterval - * @return TRUE if override reset successful, FALSE if not + * @details Outputs: Blood pump commanded to given rate w/ given control mode. + * @param message set message from Dialin which includes the flow rate (mL/min) + * and control mode to command the blood pump to. + * @return TRUE if set command successful, FALSE if not *************************************************************************/ -BOOL testResetBloodFlowDataPublishIntervalOverride( void ) +BOOL testSetTargetBloodFlowRateOverride( MESSAGE_T *message ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { - result = TRUE; - bloodFlowDataPublishInterval.override = OVERRIDE_RESET; - bloodFlowDataPublishInterval.ovData = bloodFlowDataPublishInterval.ovInitData; + if ( ( sizeof( S32 ) + sizeof( U32 ) ) == message->hdr.payloadLen ) + { + MOTOR_DIR_T dir; + U08 *msgPayload = &message->payload[0]; + S32 flow; + U32 ctrlMode; + + memcpy( &flow, msgPayload, sizeof( S32 ) ); + msgPayload += sizeof( S32 ); + memcpy( &ctrlMode, msgPayload, sizeof( U32 ) ); + + if ( flow < 0 ) + { + dir = MOTOR_DIR_REVERSE; + } + else + { + dir = MOTOR_DIR_FORWARD; + } + if ( ctrlMode < NUM_OF_PUMP_CONTROL_MODES ) + { + result = setBloodPumpTargetFlowRate( abs(flow), dir, (PUMP_CONTROL_MODE_T)ctrlMode ); + } + } } return result; } /*********************************************************************//** * @brief - * The testSetTargetBloodFlowRateOverride function overrides the target - * blood flow rate. + * The testSetBloodPumpSpeedOverride function sets the blood pump to run + * open loop at a given speed (in RPM). * @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) + * @details Outputs: Blood pump commanded to given rate w/ given control mode. + * @param message set message from Dialin which includes the pump speed (RPM) + * to command the blood pump to. * @return TRUE if override successful, FALSE if not *************************************************************************/ -BOOL testSetTargetBloodFlowRateOverride( S32 value, U32 ctrlMode ) +BOOL testSetBloodPumpSpeedOverride( MESSAGE_T *message ) { BOOL result = FALSE; if ( TRUE == isTestingActivated() ) { - MOTOR_DIR_T dir; - - if ( value < 0 ) + if ( ( sizeof( S32 ) ) == message->hdr.payloadLen ) { - dir = MOTOR_DIR_REVERSE; + MOTOR_DIR_T dir; + U08 *msgPayload = &message->payload[0]; + S32 rpm; + + memcpy( &rpm, msgPayload, sizeof( S32 ) ); + + if ( rpm < 0 ) + { + dir = MOTOR_DIR_REVERSE; + } + else + { + dir = MOTOR_DIR_FORWARD; + } + result = setBloodPumpTargetFlowRate( BP_RATE_FROM_RPM( abs(rpm) ), dir, PUMP_CONTROL_MODE_OPEN_LOOP ); } - 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 + * The testMeasuredBloodFlowRateOverride function overrides the measured * blood flow rate. * @details Inputs: none * @details Outputs: measuredBloodFlowRate - * @param value override measured blood flow rate (in mL/min) + * @param message Override message from Dialin which includes the flow rate + * (in mL/min) to override the measured flow rate to. * @return TRUE if override successful, FALSE if not *************************************************************************/ -BOOL testSetMeasuredBloodFlowRateOverride( F32 value ) +BOOL testMeasuredBloodFlowRateOverride( MESSAGE_T *message ) { - BOOL result = FALSE; + BOOL result = f32Override( message, &measuredBloodFlowRate ); - 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 + * The testMeasuredBloodPumpRotorSpeedOverride function overrides the measured * blood pump rotor speed. * @details Inputs: none * @details Outputs: bloodPumpRotorSpeedRPM - * @param value override measured blood pump rotor speed (in RPM) + * @param message Override message from Dialin which includes the pump rotor + * speed (in RPM) to override the measured rotor speed to. * @return TRUE if override successful, FALSE if not *************************************************************************/ -BOOL testSetMeasuredBloodPumpRotorSpeedOverride( F32 value ) +BOOL testMeasuredBloodPumpRotorSpeedOverride( MESSAGE_T *message ) { - BOOL result = FALSE; + BOOL result = f32Override( message, &bloodPumpRotorSpeedRPM ); - 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 + * The testMeasuredBloodPumpSpeedOverride function overrides the measured * blood pump motor speed. * @details Inputs: none * @details Outputs: bloodPumpSpeedRPM - * @param value override measured blood pump motor speed (in RPM) + * @param message Override message from Dialin which includes the pump motor + * speed (in RPM) to override the measured pump speed to. * @return TRUE if override successful, FALSE if not *************************************************************************/ -BOOL testSetMeasuredBloodPumpSpeedOverride( F32 value ) +BOOL testMeasuredBloodPumpSpeedOverride( MESSAGE_T *message ) { - BOOL result = FALSE; + BOOL result = f32Override( message, &bloodPumpSpeedRPM ); - 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 + * The testBloodPumpRotorCountOverride function overrides the blood pump * rotor counter value. * @details Inputs: none * @details Outputs: bloodPumpRotorCounter - * @param value override blood pump rotor counter value + * @param message Override message from Dialin which includes the count + * to override the blood pump rotor count to. * @return TRUE if override successful, FALSE if not *************************************************************************/ -BOOL testSetBloodPumpRotorCountOverride( U32 value ) +BOOL testBloodPumpRotorCountOverride( MESSAGE_T *message ) { - BOOL result = FALSE; + BOOL result = u32Override( message, &bloodPumpRotorCounter, 0, HEX_32_BIT_FULL_SCALE ); - 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; -} - /**@}*/ Index: firmware/App/Controllers/BloodFlow.h =================================================================== diff -u -rf05acb2d013aa3dec1b59d793b8876aed928b5aa -r051326e2671e8d5b3e99eaa109ea549e94a929f3 --- firmware/App/Controllers/BloodFlow.h (.../BloodFlow.h) (revision f05acb2d013aa3dec1b59d793b8876aed928b5aa) +++ firmware/App/Controllers/BloodFlow.h (.../BloodFlow.h) (revision 051326e2671e8d5b3e99eaa109ea549e94a929f3) @@ -22,8 +22,8 @@ /** * @defgroup BloodFlow BloodFlow - * @brief Blood Pump & Blood Flow controller/monitor module. Monitors the - * blood flow rate and controls the blood pump. + * @brief Blood Pump & Blood Flow controller/monitor module. Monitors and + * controls the blood pump. * * @addtogroup BloodFlow * @{ @@ -33,39 +33,32 @@ #define MAX_SET_BLOOD_FLOW_RATE 500 ///< Maximum blood flow rate (in mL/min). #define MIN_SET_BLOOD_FLOW_RATE 100 ///< Minimum blood flow rate (in mL/min). -#define SALINE_BOLUS_FLOW_RATE 150 ///< Saline bolus flow rate (in mL/min). -#define VOLUME_PER_BP_MOTOR_REV_ML 0.216F ///< Theoretical volume (mL) of blood/saline volume per motor revolution. -#define BP_HALL_EDGE_COUNTS_PER_REV 48 ///< Number of hall sensor edge counts per motor revolution. - /// Payload record structure for a blood pump data message. typedef struct { - S32 setPoint; ///< Set point. - F32 measFlow; ///< Measured flow in mL/min. + S32 setFlowRate; ///< Set flow rate in mL/min. + F32 measFlow; ///< Measured flow rate in mL/min. F32 measRotorSpd; ///< Measured rotor speed in RPM. F32 measPumpSpd; ///< Measured pump speed in RPM. - F32 measMCSpd; ///< Measured motor speed in RPM. - F32 measMCCurr; ///< Measure motor current in Amps. - F32 pwmDC; ///< Duty cycle. + F32 measCurr; ///< Measure motor current in Amps. + F32 setRPM; ///< Set motor speed in RPM. U32 rotorCount; ///< Rotor count. - U32 presFlow; ///< Blood flow in mL/min. - U32 rotorHall; ///< Rotor hall in counts. + U32 presFlow; ///< Prescribed blood flow in mL/min. + U32 rotorHall; ///< Rotor hall state (1=home, 0=not home). } BLOOD_PUMP_STATUS_PAYLOAD_T; // ********** public function prototypes ********** void initBloodFlow( void ); -void execBloodFlowMonitor( void ); +void execBloodFlowMonitor( void ); void execBloodFlowController( void ); BOOL setBloodPumpTargetFlowRate( U32 flowRate, MOTOR_DIR_T dir, PUMP_CONTROL_MODE_T mode ); BOOL setBloodPumpTargetRPM( U32 rpm, MOTOR_DIR_T dir ); void signalBloodPumpHardStop( void ); void signalBloodPumpRotorHallSensor( void ); BOOL homeBloodPump( void ); -U32 getBloodPumpMotorCount( void ); -U32 getBloodPumpRotorCount( void ); BOOL isBloodPumpRunning( void ); BOOL isBloodPumpRampComplete( void ); void resetBloodPumpRotorCount( void ); @@ -76,25 +69,14 @@ F32 getMeasuredBloodFlowRate( void ); F32 getMeasuredBloodPumpRotorSpeed( void ); F32 getMeasuredBloodPumpSpeed( void ); -F32 getMeasuredBloodPumpMCSpeed( void ); -F32 getMeasuredBloodPumpMCCurrent( void ); -BOOL testSetBloodFlowDataPublishIntervalOverride( U32 value ); -BOOL testResetBloodFlowDataPublishIntervalOverride( void ); -BOOL testSetTargetBloodFlowRateOverride( S32 value, U32 bloodPumpControlMode ); -BOOL testSetMeasuredBloodFlowRateOverride( F32 value ); -BOOL testResetMeasuredBloodFlowRateOverride( void ); -BOOL testSetMeasuredBloodPumpRotorSpeedOverride( F32 value ); -BOOL testResetMeasuredBloodPumpRotorSpeedOverride( void ); -BOOL testSetMeasuredBloodPumpSpeedOverride( F32 value ); -BOOL testResetMeasuredBloodPumpSpeedOverride( void ); -BOOL testSetMeasuredBloodPumpMCSpeedOverride( F32 value ); -BOOL testResetMeasuredBloodPumpMCSpeedOverride( void ); -BOOL testSetMeasuredBloodPumpMCCurrentOverride( F32 value ); -BOOL testResetMeasuredBloodPumpMCCurrentOverride( void ); -BOOL testSetBloodPumpRotorCountOverride( U32 value ); -BOOL testResetBloodPumpRotorCountOverride( void ); -BOOL testSetBloodPumpTargetDutyCycle( F32 value ); +BOOL testBloodFlowDataPublishIntervalOverride( MESSAGE_T *message ); +BOOL testSetTargetBloodFlowRateOverride( MESSAGE_T *message ); +BOOL testSetBloodPumpSpeedOverride( MESSAGE_T *message ); +BOOL testMeasuredBloodFlowRateOverride( MESSAGE_T *message ); +BOOL testMeasuredBloodPumpRotorSpeedOverride( MESSAGE_T *message ); +BOOL testMeasuredBloodPumpSpeedOverride( MESSAGE_T *message ); +BOOL testBloodPumpRotorCountOverride( MESSAGE_T *message ); /**@}*/ Index: firmware/App/Controllers/Valves.c =================================================================== diff -u -rb692af0986725e87431ca5e23c5721f1b242baac -r051326e2671e8d5b3e99eaa109ea549e94a929f3 --- firmware/App/Controllers/Valves.c (.../Valves.c) (revision b692af0986725e87431ca5e23c5721f1b242baac) +++ firmware/App/Controllers/Valves.c (.../Valves.c) (revision 051326e2671e8d5b3e99eaa109ea549e94a929f3) @@ -40,6 +40,8 @@ #define MAX_HOME_FULL_TRAVEL_DIFF 100U ///< Maximum allowed difference in full travel encoder counts between expected and measured during home operation. #define VALVE_TRANSITION_TIMEOUT_MS 3000U ///< Valves transition time out in ms +#define VALVE_FORCE_HOME TRUE ///< Force valve to home even if already homed + /// Valve controller states typedef enum Valve_Control_States { @@ -132,6 +134,48 @@ /*********************************************************************//** * @brief + * The homeValve function requests a valve homing operation on a given valve. + * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if given valve is invalid. + * @details \b Inputs: currentValveStates[] + * @details \b Outputs: currentValveStates[] + * @param valve ID of valve that is set to be homed + * @param force flag indicating whether homing should be forced even if already homed + * @param cartridge flag indicating whether a cartridge may be present requiring a more lax homing + * @return TRUE if the homing command accepted otherwise, FALSE + *************************************************************************/ +BOOL homeValve( VALVE_T valve, BOOL force, BOOL cartridge ) +{ + BOOL result = FALSE; + + if ( valve < NUM_OF_VALVES ) + { + VALVE_STATE_T currState = currentValveStates[ valve ].controlState; + + if ( ( VALVE_STATE_HOMING_NOT_STARTED == currState ) || ( VALVE_STATE_IDLE == currState ) ) + { + // If haven't already homed the valves, home the valves + if ( ( currentValveStates[ valve ].hasValveBeenHomed != TRUE ) || ( VALVE_FORCE_HOME == force ) ) + { + currentValveStates[ valve ].hasHomingBeenRequested = TRUE; + } + // Otherwise, go to position A (home position) + else + { + setValvePosition( valve, VALVE_POSITION_A_INSERT_EJECT ); + } + result = TRUE; + } + } + else + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_TD_VALVES_INVALID_VALVE8, (U32)valve ) + } + + return result; +} + +/*********************************************************************//** + * @brief * The setValvePosition function sets the commanded position for a given valve. * @details \b Alarm: ALARM_ID_TD_SOFTWARE_FAULT if invalid valve or position given. * @details \b Inputs: none Index: firmware/App/Services/AlarmMgmtSWFaults.h =================================================================== diff -u -rd9b5f588d81e15ed3849222bed3362e15dbf4b0a -r051326e2671e8d5b3e99eaa109ea549e94a929f3 --- firmware/App/Services/AlarmMgmtSWFaults.h (.../AlarmMgmtSWFaults.h) (revision d9b5f588d81e15ed3849222bed3362e15dbf4b0a) +++ firmware/App/Services/AlarmMgmtSWFaults.h (.../AlarmMgmtSWFaults.h) (revision 051326e2671e8d5b3e99eaa109ea549e94a929f3) @@ -139,7 +139,11 @@ SW_FAULT_ID_TD_VALVES_INVALID_VALVE5 = 108, SW_FAULT_ID_TD_VALVES_INVALID_VALVE6 = 109, SW_FAULT_ID_TD_VALVES_INVALID_VALVE7 = 110, - SW_FAULT_ID_TD_VALVES_INVALID_STATE = 111, + SW_FAULT_ID_TD_VALVES_INVALID_VALVE8 = 111, + SW_FAULT_ID_TD_VALVES_INVALID_STATE = 112, + SW_FAULT_ID_BLOOD_FLOW_SET_TOO_HIGH = 113, + SW_FAULT_ID_BLOOD_FLOW_INVALID_BLOOD_PUMP_STATE = 114, + SW_FAULT_ID_TD_AIR_PUMP_INVALID_STATE = 115, NUM_OF_SW_FAULT_IDS } SW_FAULT_ID_T; Index: firmware/App/Services/Messaging.c =================================================================== diff -u -rd9b5f588d81e15ed3849222bed3362e15dbf4b0a -r051326e2671e8d5b3e99eaa109ea549e94a929f3 --- firmware/App/Services/Messaging.c (.../Messaging.c) (revision d9b5f588d81e15ed3849222bed3362e15dbf4b0a) +++ firmware/App/Services/Messaging.c (.../Messaging.c) (revision 051326e2671e8d5b3e99eaa109ea549e94a929f3) @@ -23,6 +23,7 @@ #include "Bubbles.h" #include "Buttons.h" #include "Compatible.h" +#include "CpldInterface.h" #include "LevelSensors.h" #include "Messaging.h" #include "OperationModes.h" @@ -35,7 +36,7 @@ #include "Valve2Way.h" #include "Valves.h" #include "Voltages.h" -#include "Watchdog.h" +#include "WatchdogMgmt.h" /** * @addtogroup Messaging Index: firmware/App/Tasks/TaskGeneral.c =================================================================== diff -u -rd9b5f588d81e15ed3849222bed3362e15dbf4b0a -r051326e2671e8d5b3e99eaa109ea549e94a929f3 --- firmware/App/Tasks/TaskGeneral.c (.../TaskGeneral.c) (revision d9b5f588d81e15ed3849222bed3362e15dbf4b0a) +++ firmware/App/Tasks/TaskGeneral.c (.../TaskGeneral.c) (revision 051326e2671e8d5b3e99eaa109ea549e94a929f3) @@ -17,9 +17,9 @@ #include "AirPump.h" #include "AirTrap.h" +#include "BloodFlow.h" #include "Messaging.h" #include "OperationModes.h" -#include "PeristalticPump.h" #include "Pressures.h" #include "Switches.h" #include "SystemCommTD.h" @@ -81,8 +81,8 @@ // Monitor processor RAM status // execRAMMonitor(); - // Monitor peristaltic pump(s) - readPeristalticPumps(); + // Monitor blood pump/flow + execBloodFlowMonitor(); // Run operation mode state machine execOperationModes(); @@ -94,7 +94,7 @@ execValvesController(); // Control blood pump -// execBloodFlowController(); + execBloodFlowController(); // Control Air Pump execAirPumpController(); Index: firmware/source/sys_main.c =================================================================== diff -u -rd9b5f588d81e15ed3849222bed3362e15dbf4b0a -r051326e2671e8d5b3e99eaa109ea549e94a929f3 --- firmware/source/sys_main.c (.../sys_main.c) (revision d9b5f588d81e15ed3849222bed3362e15dbf4b0a) +++ firmware/source/sys_main.c (.../sys_main.c) (revision 051326e2671e8d5b3e99eaa109ea549e94a929f3) @@ -65,6 +65,7 @@ #include "TDCommon.h" #include "AirPump.h" #include "AirTrap.h" +#include "BloodFlow.h" #include "Bubbles.h" #include "Buttons.h" #include "CpldInterface.h" @@ -198,6 +199,7 @@ // Initialize controllers initAirPump(); initAirTrap(); + initBloodFlow(); initValves(); // Initialize modes initOperationModes();