Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -r1a685471524555a374854c0c9ec8e208e71fe2df -r933a18d740285e70be9d00696ed0f5a5381bc8e4 --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 1a685471524555a374854c0c9ec8e208e71fe2df) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 933a18d740285e70be9d00696ed0f5a5381bc8e4) @@ -59,7 +59,10 @@ #define BP_HOME_RATE 100 ///< target pump speed (in estimate mL/min) for homing. #define BP_HOME_TIMEOUT_MS 10000 ///< maximum time allowed for homing to complete (in ms). -#define BP_SPEED_CALC_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) ///< interval (ms/task time) at which the blood pump speed is calculated. +/// interval (ms/task time) at which the blood pump speed is calculated (every 40 ms). +#define BP_SPEED_CALC_INTERVAL ( 40 / TASK_PRIORITY_INTERVAL ) +/// number of hall sensor counts kept in buffer to hold last 1 second of count data. +#define BP_SPEED_CALC_BUFFER_LEN ( 1000 / BP_SPEED_CALC_INTERVAL / TASK_PRIORITY_INTERVAL ) #define BP_HALL_EDGE_COUNTS_PER_REV 48 ///< number of hall sensor edge counts per motor revolution. #define BP_MAX_ROTOR_SPEED_RPM 100.0 ///< maximum rotor speed allowed for blood pump. @@ -150,12 +153,12 @@ /// 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 OVERRIDE_S32_T targetBloodFlowRate = { 0, 0, 0, 0 }; ///< requested blood flow rate (mL/min) -static OVERRIDE_F32_T measuredBloodFlowRate = { 0.0, 0.0, 0.0, 0 }; ///< measured blood flow rate (mL/min) -static OVERRIDE_F32_T bloodPumpRotorSpeedRPM = { 0.0, 0.0, 0.0, 0 }; ///< measured blood pump rotor speed (RPM) -static OVERRIDE_F32_T bloodPumpSpeedRPM = { 0.0, 0.0, 0.0, 0 }; ///< measured blood pump motor speed (RPM) -static OVERRIDE_F32_T adcBloodPumpMCSpeedRPM = { 0.0, 0.0, 0.0, 0 }; ///< measured blood pump motor controller speed (RPM) -static OVERRIDE_F32_T adcBloodPumpMCCurrentmA = { 0.0, 0.0, 0.0, 0 }; ///< measured blood pump motor controller current (mA) +static S32 targetBloodFlowRate = 0; ///< requested blood flow rate +static OVERRIDE_F32_T measuredBloodFlowRate = { 0.0, 0.0, 0.0, 0 }; ///< measured 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_F32_T bloodFlowSignalStrength = { 0.0, 0.0, 0.0, 0 }; ///< measured blood flow signal strength (%) static U32 bpControlTimerCounter = 0; ///< determines when to perform control on blood flow @@ -165,9 +168,10 @@ 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 U16 bpLastMotorHallSensorCount = 0; ///< last hall sensor count for the blood pump motor -static U32 bloodPumpMotorEdgeCount = 0; ///< running counter for blood pump motor revolutions -static U32 bpMotorSpeedCalcTimerCtr = 0; ///< counter determines interval for calculating blood pump motor speed from hall sensor count. +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 U32 errorBloodFlowVsMotorSpeedPersistTimerCtr = 0; ///< persistence timer counter for flow vs. motor speed error condition. static U32 errorBloodMotorOffPersistTimerCtr = 0; ///< persistence timer counter for motor off check error condition. @@ -216,15 +220,22 @@ * @return none *************************************************************************/ void initBloodFlow( void ) -{ - bpLastMotorHallSensorCount = getFPGABloodPumpHallSensorCount(); +{ + U32 i; stopBloodPump(); setBloodPumpDirection( MOTOR_DIR_FORWARD ); // zero rolling flow average buffer resetBloodFlowMovingAverage(); - + + // zero motor hall sensors counts buffer + bpMotorSpeedCalcIdx = 0; + for ( i = 0; i < BP_SPEED_CALC_BUFFER_LEN; i++ ) + { + bpLastMotorHallSensorCounts[ i ] = 0; + } + // initialize blood flow PI controller initializePIController( PI_CONTROLLER_ID_BLOOD_FLOW, MIN_BLOOD_PUMP_PWM_DUTY_CYCLE, BP_P_COEFFICIENT, BP_I_COEFFICIENT, @@ -258,7 +269,7 @@ if ( flowRate <= MAX_BLOOD_FLOW_RATE ) { resetBloodFlowMovingAverage(); - targetBloodFlowRate.data = ( dir == MOTOR_DIR_FORWARD ? (S32)flowRate : (S32)flowRate * -1 ); + targetBloodFlowRate = ( dir == MOTOR_DIR_FORWARD ? (S32)flowRate : (S32)flowRate * -1 ); bloodPumpDirection = dir; bloodPumpControlMode = mode; // set PWM duty cycle target to an estimated initial target to ramp to based on target flow rate - then we'll control to flow when ramp completed @@ -312,7 +323,7 @@ *************************************************************************/ void signalBloodPumpHardStop( void ) { - targetBloodFlowRate.data = 0; + targetBloodFlowRate = 0; stopBloodPump(); bloodPumpState = BLOOD_PUMP_OFF_STATE; bloodPumpPWMDutyCyclePct = 0.0; @@ -498,7 +509,7 @@ BLOOD_PUMP_STATE_T result = BLOOD_PUMP_OFF_STATE; // if we've been given a flow rate, setup ramp up and transition to ramp up state - if ( getTargetBloodFlowRate() != 0 ) + if ( targetBloodFlowRate != 0 ) { // set initial PWM duty cycle bloodPumpPWMDutyCyclePctSet = BP_PWM_ZERO_OFFSET + MAX_BLOOD_PUMP_PWM_STEP_UP_CHANGE; @@ -526,7 +537,7 @@ BLOOD_PUMP_STATE_T result = BLOOD_PUMP_RAMPING_UP_STATE; // have we been asked to stop the blood pump? - if ( 0 == getTargetBloodFlowRate() ) + if ( 0 == targetBloodFlowRate ) { // start ramp down to stop bloodPumpPWMDutyCyclePctSet -= MAX_BLOOD_PUMP_PWM_STEP_DN_CHANGE; @@ -616,7 +627,7 @@ { if ( bloodPumpControlModeSet == PUMP_CONTROL_MODE_CLOSED_LOOP ) { - F32 tgtFlow = (F32)getTargetBloodFlowRate(); + F32 tgtFlow = (F32)targetBloodFlowRate; F32 actFlow = getMeasuredBloodFlowRate(); F32 newPWM; @@ -722,26 +733,6 @@ /*********************************************************************//** * @brief - * The getTargetBloodFlowRate function gets the current target blood flow - * rate. - * @details Inputs: targetBloodFlowRate - * @details Outputs: none - * @return the current target blood flow rate (in mL/min). - *************************************************************************/ -S32 getTargetBloodFlowRate( void ) -{ - S32 result = targetBloodFlowRate.data; - - if ( OVERRIDE_KEY == targetBloodFlowRate.override ) - { - result = targetBloodFlowRate.ovData; - } - - return result; -} - -/*********************************************************************//** - * @brief * The getMeasuredBloodFlowRate function gets the measured blood flow * rate. * @details Inputs: measuredBloodFlowRate @@ -876,7 +867,7 @@ { BLOOD_PUMP_STATUS_PAYLOAD_T payload; - payload.setPoint = (S32)getTargetBloodFlowRate(); + payload.setPoint = targetBloodFlowRate; payload.measFlow = getMeasuredBloodFlowRate(); payload.measRotorSpd = getMeasuredBloodPumpRotorSpeed(); payload.measPumpSpd = getMeasuredBloodPumpSpeed(); @@ -943,8 +934,11 @@ { if ( ++bpMotorSpeedCalcTimerCtr >= BP_SPEED_CALC_INTERVAL ) { - U16 bpMotorHallSensorCount = getFPGABloodPumpHallSensorCount(); - U16 incDelta = ( bpMotorHallSensorCount >= bpLastMotorHallSensorCount ? bpMotorHallSensorCount - bpLastMotorHallSensorCount : ( HEX_64_K - bpLastMotorHallSensorCount ) + bpMotorHallSensorCount ); + U16 bpMotorHallSensorCount = getFPGABloodPumpHallSensorCount(); + U32 nextIdx = INC_WRAP( bpMotorSpeedCalcIdx, 0, BP_SPEED_CALC_BUFFER_LEN - 1 ); + U16 incDelta = ( bpMotorHallSensorCount >= bpLastMotorHallSensorCounts[ nextIdx ] ? \ + bpMotorHallSensorCount - bpLastMotorHallSensorCounts[ nextIdx ] : \ + ( HEX_64_K - bpLastMotorHallSensorCounts[ nextIdx ] ) + bpMotorHallSensorCount ); U16 decDelta = HEX_64_K - incDelta; U16 delta; @@ -962,7 +956,8 @@ bloodPumpMotorEdgeCount += delta; // update last count for next time - bpLastMotorHallSensorCount = bpMotorHallSensorCount; + bpLastMotorHallSensorCounts[ nextIdx ] = bpMotorHallSensorCount; + bpMotorSpeedCalcIdx = nextIdx; bpMotorSpeedCalcTimerCtr = 0; } } @@ -1070,8 +1065,8 @@ *************************************************************************/ static void checkBloodPumpSpeeds( void ) { - F32 measMotorSpeed = fabs( getMeasuredBloodPumpSpeed() ); - S32 cmdRate = getTargetBloodFlowRate(); + F32 measMotorSpeed = fabs( getMeasuredBloodPumpSpeed() ); + S32 cmdRate = targetBloodFlowRate; // check for pump running while commanded off if ( 0 == cmdRate ) @@ -1252,7 +1247,7 @@ F32 sigStrength = getMeasuredBloodFlowSignalStrength(); BOOL outOfRange = ( sigStrength < MIN_FLOW_SIG_STRENGTH ? TRUE : FALSE ); - checkPersistentAlarm( PERSISTENT_ALARM_BLOOD_FLOW_SIGNAL_STRENGTH, outOfRange, sigStrength ); + checkPersistentAlarm( PERSISTENT_ALARM_BLOOD_FLOW_SIGNAL_STRENGTH, outOfRange, sigStrength, MIN_FLOW_SIG_STRENGTH ); } #endif } @@ -1428,9 +1423,7 @@ } if ( ctrlMode < NUM_OF_PUMP_CONTROL_MODES ) { - targetBloodFlowRate.ovInitData = targetBloodFlowRate.data; // backup current target flow rate - targetBloodFlowRate.ovData = value; - targetBloodFlowRate.override = OVERRIDE_KEY; + targetBloodFlowRate = value; result = setBloodPumpTargetFlowRate( abs(value), dir, (PUMP_CONTROL_MODE_T)ctrlMode ); } } @@ -1440,30 +1433,6 @@ /*********************************************************************//** * @brief - * The testResetTargetBloodFlowRateOverride function resets the override of the - * target blood flow rate. - * @details Inputs: none - * @details Outputs: targetBloodFlowRate - * @return TRUE if override reset successful, FALSE if not - *************************************************************************/ -BOOL testResetTargetBloodFlowRateOverride( void ) -{ - BOOL result = FALSE; - - if ( TRUE == isTestingActivated() ) - { - targetBloodFlowRate.data = targetBloodFlowRate.ovInitData; // restore pre-override target flow rate - targetBloodFlowRate.override = OVERRIDE_RESET; - targetBloodFlowRate.ovInitData = 0; - targetBloodFlowRate.ovData = 0; - result = setBloodPumpTargetFlowRate( targetBloodFlowRate.data, bloodPumpDirection, bloodPumpControlMode ); - } - - return result; -} - -/*********************************************************************//** - * @brief * The testResetMeasuredBloodFlowRateOverride function overrides the measured * blood flow rate. * @details Inputs: none