Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -rf1e3338feb8fccadf01aa629aec4a55132f6fd77 -rfe29b840bff79d853e49baf3a78b080e1cf428b6 --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision f1e3338feb8fccadf01aa629aec4a55132f6fd77) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision fe29b840bff79d853e49baf3a78b080e1cf428b6) @@ -21,9 +21,11 @@ #include "CpldInterface.h" #include "FpgaTD.h" #include "Messaging.h" +#include "ModeTxParams.h" #include "OperationModes.h" #include "PersistentAlarm.h" -//#include "PIControllers.h" +#include "PIControllers.h" +#include "Pressures.h" #include "TaskGeneral.h" #include "Timers.h" #include "Utilities.h" @@ -49,10 +51,11 @@ #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_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. @@ -73,17 +76,18 @@ /// 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_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 6.81F ///< 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.00000000000F ///< 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. @@ -221,9 +225,9 @@ 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 ); @@ -335,14 +339,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 = BP_FLOW_WEAR_A_TERM * (F32)rotCnt + BP_FLOW_WEAR_B_TERM; + F32 alpha = wear * artPresL + BP_FLOW_ALPHA_Y_INTERCEPT; + F32 flow = BP_ML_PER_ROTOR_REV * rotSpd; // * alpha; TODO return flow; } @@ -362,7 +366,7 @@ isBloodPumpOn = FALSE; bpControlTimerCounter = 0; setPeristalticPumpSetSpeed( bloodPumpSetSpeedRPM ); -// resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, MIN_BLOOD_PUMP_PWM_DUTY_CYCLE ); + resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, 0.0F, 0.0F ); } /*********************************************************************//** @@ -479,8 +483,7 @@ 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(); // Do not start enforcing checks until out of init/POST mode TODO-add checks later // if ( opMode != MODE_INIT ) @@ -639,7 +642,7 @@ resetBloodPumpRPMMovingAverage(); bloodPumpSetSpeedRPM = bloodPumpRampToSpeedRPM; setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); -// resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, bloodPumpSetSpeedRPM ); + resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, (F32)bloodPumpSetSpeedRPM, 0.0F ); bpControlTimerCounter = 0; result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; } @@ -676,7 +679,7 @@ else if ( bloodPumpSetSpeedRPM <= bloodPumpRampToSpeedRPM ) { resetBloodPumpRPMMovingAverage(); -// resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, 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; @@ -713,8 +716,8 @@ F32 actFlow = getMeasuredBloodFlowRate(); F32 newRPM = bloodPumpSetSpeedRPM; // TODO - don't set speed here when control is implemented below. -// newRPM = runPIController( PI_CONTROLLER_ID_BLOOD_FLOW, tgtFlow, actFlow ); -// newRPM = ( newRPM < 0.0F ? 0.0F : newRPM ); + newRPM = runPIController( PI_CONTROLLER_ID_BLOOD_FLOW, tgtFlow, actFlow ); + newRPM = ( newRPM < 0.0F ? 0.0F : newRPM ); bloodPumpSetSpeedRPM = (U32)((S32)(newRPM)); setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); } @@ -812,11 +815,11 @@ payload.h4MeasCurr = 0.0F; // TODO getMeasuredBloodPumpMCCurrent(); payload.h4SetRPM = bloodPumpSetSpeedRPM; payload.h4RotorCount = getBloodPumpRotorCount(); -// if ( ( MODE_PRET == opMode ) || ( MODE_TREA == opMode ) || ( MODE_POST == opMode ) ) -// { // prescribed flow only available in treatment modes -// payload.presFlow = getTreatmentParameterU32( TREATMENT_PARAM_BLOOD_FLOW ); // TODO - restore when Tx Param Mode implemented -// } -// else + if ( ( MODE_PRET == opMode ) || ( MODE_TREA == opMode ) || ( MODE_POST == opMode ) ) + { // prescribed flow only available in treatment modes + payload.h4PresFlow = getTreatmentParameterU32( TREATMENT_PARAM_BLOOD_FLOW ); + } + else { payload.h4PresFlow = 0; }