Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -r46b163d19c65e8c21db7b0247bbb1af0dba1ece5 -rf760ffc4b10556e5186e9ceb90294262063440ca --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 46b163d19c65e8c21db7b0247bbb1af0dba1ece5) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision f760ffc4b10556e5186e9ceb90294262063440ca) @@ -1,14 +1,14 @@ /************************************************************************** * -* Copyright (c) 2019-2022 Diality Inc. - All Rights Reserved. +* Copyright (c) 2019-2023 Diality Inc. - All Rights Reserved. * * THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN * WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER. * * @file BloodFlow.c * * @author (last) Sean Nash -* @date (last) 09-Aug-2022 +* @date (last) 18-Jan-2023 * * @author (original) Sean Nash * @date (original) 07-Nov-2019 @@ -19,7 +19,8 @@ #include "can.h" #include "etpwm.h" - + +#include "Battery.h" #include "BloodFlow.h" #include "FPGA.h" #include "InternalADC.h" @@ -57,7 +58,7 @@ #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_RATE 100 ///< Target pump speed (in estimate mL/min) for homing. +#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). @@ -72,15 +73,14 @@ #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_ROTOR_VS_MOTOR_DIFF_RPM 2.0F ///< Maximum difference in speed between motor and rotor (in rotor RPM). #define BP_MAX_MOTOR_SPEED_ERROR_RPM 300.0F ///< Maximum difference in speed between measured and commanded RPM. +#define BP_MAX_MOTOR_SPEED_VS_TRGT_DIFF_PCT 0.15F ///< Maximum motor speed vs target difference in percent. /// Persist time (in ms) for motor off error condition. static const U32 BP_OFF_ERROR_PERSIST = ( 5 * MS_PER_SECOND ); /// Persist time (in ms) motor speed error condition. static const U32 BP_MOTOR_SPEED_ERROR_PERSIST = ( 5 * MS_PER_SECOND ); -/// Persist time (in ms) rotor speed error condition. -static const U32 BP_ROTOR_SPEED_ERROR_PERSIST = ( 22 * MS_PER_SECOND ); /// Persist time (in ms) pump direction error condition. static const U32 BP_DIRECTION_ERROR_PERSIST = ( 250 ); /// Persist time period (in ms) blood pump rotor speed too fast error condition. @@ -107,11 +107,16 @@ #define BP_ML_PER_MIN_TO_PUMP_RPM_FACTOR ( BP_REV_PER_LITER / ML_PER_LITER ) ///< Conversion factor from mL/min to motor RPM. #define BP_GEAR_RATIO 32.0F ///< Blood pump motor to blood pump gear ratio. #define BP_PWM_ZERO_OFFSET 0.1F ///< 10 pct PWM duty cycle = zero speed. +#define BP_100_PCT_PWM_RPM_RANGE 4000.0F ///< 10-90% PWM range yields 0-3,200 RPM range. Full 100% PWM range would yield 4,000 RPM range. /// Conversion macro from mL/min to estimated PWM duty cycle %. #define BP_PWM_FROM_ML_PER_MIN(rate) ( (rate) * BP_ML_PER_MIN_TO_PUMP_RPM_FACTOR * BP_GEAR_RATIO * BP_MOTOR_RPM_TO_PWM_DC_FACTOR + BP_PWM_ZERO_OFFSET ) /// Conversion from PWM duty cycle % to commanded pump motor speed. -#define BP_PWM_TO_MOTOR_SPEED_RPM(pwm) ( ((pwm) - BP_PWM_ZERO_OFFSET) * 4000.0F ) +#define BP_PWM_TO_MOTOR_SPEED_RPM(pwm,dir) ( ( ((pwm) - BP_PWM_ZERO_OFFSET) * BP_100_PCT_PWM_RPM_RANGE ) * ( dir == MOTOR_DIR_FORWARD ? 1.0F : -1.0F ) ) +/// Conversion from RPM to PWM duty cycle %. +#define BP_MOTOR_SPEED_RPM_TO_PWM(rpm) ( ( (F32)(rpm) / BP_100_PCT_PWM_RPM_RANGE ) + BP_PWM_ZERO_OFFSET ) +/// Conversion macro from mL/min to estimated PWM duty cycle %. +#define BP_ML_PER_MIN_FROM_PWM(pwm) ( ( ( pwm - BP_PWM_ZERO_OFFSET ) / ( BP_ML_PER_MIN_TO_PUMP_RPM_FACTOR * BP_GEAR_RATIO * BP_MOTOR_RPM_TO_PWM_DC_FACTOR ) ) ) /// Measured blood flow is filtered w/ moving average. #define SIZE_OF_ROLLING_AVG ( ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) * 1 ) @@ -166,6 +171,7 @@ static MOTOR_DIR_T bloodPumpDirectionSet = MOTOR_DIR_FORWARD; ///< Currently set blood flow direction static PUMP_CONTROL_MODE_T bloodPumpControlMode = PUMP_CONTROL_MODE_CLOSED_LOOP; ///< Requested blood pump control mode. static PUMP_CONTROL_MODE_T bloodPumpControlModeSet = PUMP_CONTROL_MODE_CLOSED_LOOP; ///< Currently set blood pump control mode. +static U32 errorBloodRotorSpeedPersistTimerCtr = 0; ///< Persistence timer counter for rotor speed error condition. /// Interval (in task intervals) at which to publish blood flow data to CAN bus. static OVERRIDE_U32_T bloodFlowDataPublishInterval = { BLOOD_FLOW_DATA_PUB_INTERVAL, BLOOD_FLOW_DATA_PUB_INTERVAL, BLOOD_FLOW_DATA_PUB_INTERVAL, 0 }; @@ -254,7 +260,6 @@ // Initialize persistent alarm for flow sensor initPersistentAlarm( ALARM_ID_BLOOD_PUMP_OFF_CHECK, 0, BP_OFF_ERROR_PERSIST ); initPersistentAlarm( ALARM_ID_BLOOD_PUMP_MOTOR_SPEED_CHECK, 0, BP_MOTOR_SPEED_ERROR_PERSIST ); - initPersistentAlarm( ALARM_ID_BLOOD_PUMP_ROTOR_SPEED_CHECK, 0, BP_ROTOR_SPEED_ERROR_PERSIST ); initPersistentAlarm( ALARM_ID_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_BLOOD_PUMP_ROTOR_SPEED_TOO_HIGH, 0, BP_MAX_ROTOR_SPEED_ERROR_PERSIST ); @@ -350,6 +355,26 @@ } return result; +} + +/*********************************************************************//** + * @brief + * The setBloodPumpTargetRPM function sets a new target pump speed and pump + * direction. Pump is set to open loop control. + * @details Inputs: none + * @details Outputs: none + * @param rpm new target blood pump speed (in RPM) + * @param dir new blood flow direction + * @return TRUE if new flow rate & direction are set, FALSE if not + *************************************************************************/ +BOOL setBloodPumpTargetRPM( U32 rpm, MOTOR_DIR_T dir ) +{ + BOOL result = FALSE; + F32 pwm = BP_MOTOR_SPEED_RPM_TO_PWM( rpm ); + + result = setBloodPumpTargetFlowRate( (U32)BP_ML_PER_MIN_FROM_PWM( pwm ), dir, PUMP_CONTROL_MODE_OPEN_LOOP ); + + return result; } /*********************************************************************//** @@ -409,7 +434,7 @@ bloodPumpRotorCounter.data++; // Calculate rotor speed (in RPM) - bloodPumpRotorSpeedRPM.data = ( 1.0 / (F32)deltaTime ) * (F32)MS_PER_SECOND * (F32)SEC_PER_MIN; + bloodPumpRotorSpeedRPM.data = ( 1.0F / (F32)deltaTime ) * (F32)MS_PER_SECOND * (F32)SEC_PER_MIN; bpRotorRevStartTime = rotTime; // If we are supposed to stop pump at home position, stop pump now. @@ -435,7 +460,7 @@ { bpStopAtHomePosition = TRUE; bpHomeStartTime = getMSTimerCount(); - result = setBloodPumpTargetFlowRate( BP_HOME_RATE, MOTOR_DIR_FORWARD, PUMP_CONTROL_MODE_OPEN_LOOP ); + result = setBloodPumpTargetRPM( BP_HOME_SPEED, MOTOR_DIR_FORWARD ); } return result; @@ -1136,14 +1161,14 @@ * 2. while pump is controlling, measured motor speed should be within allowed range of commanded speed. * 3. measured motor speed should be within allowed range of measured rotor speed. * All 3 checks have a persistence time that must be met before an alarm is triggered. - * @details Inputs: targetBloodFlowRate, bloodPumpSpeedRPM, bloodPumpRotorSpeedRPM - * @details Outputs: alarm(s) may be triggered + * @details Inputs: bloodPumpState, targetBloodFlowRate, bloodPumpPWMDutyCyclePctSet, bloodPumpDirectionSet + * @details Outputs: errorBloodRotorSpeedPersistTimerCtr, alarm(s) may be triggered * @return none *************************************************************************/ static void checkBloodPumpSpeeds( void ) { - F32 measMotorSpeed = fabs( getMeasuredBloodPumpSpeed() ); - F32 measMCMotorSpeed = fabs( getMeasuredBloodPumpMCSpeed() ); + F32 measMotorSpeed = getMeasuredBloodPumpSpeed(); + F32 measMCMotorSpeed = getMeasuredBloodPumpMCSpeed(); // Check for pump running while commanded off if ( ( TRUE == isPersistentAlarmTriggered( ALARM_ID_BLOOD_PUMP_OFF_CHECK, @@ -1161,12 +1186,13 @@ // 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 ); + F32 cmdMotorSpeed = BP_PWM_TO_MOTOR_SPEED_RPM( bloodPumpPWMDutyCyclePctSet, bloodPumpDirectionSet ); F32 deltaMotorSpeed = fabs( measMotorSpeed - cmdMotorSpeed ); F32 deltaMCMotorSpeed = fabs( measMCMotorSpeed - cmdMotorSpeed ); - F32 measRotorSpeed = getMeasuredBloodPumpRotorSpeed(); - F32 measMotorSpeedInRotorRPM = measMotorSpeed / BP_GEAR_RATIO; - F32 deltaRotorSpeed = fabs( measRotorSpeed - measMotorSpeedInRotorRPM ); + 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_BLOOD_PUMP_MOTOR_SPEED_CHECK, @@ -1181,20 +1207,28 @@ } // Check measured rotor speed vs. measured motor speed while controlling to target - if ( ( TRUE == isPersistentAlarmTriggered( ALARM_ID_BLOOD_PUMP_ROTOR_SPEED_CHECK, deltaRotorSpeed > BP_MAX_ROTOR_VS_MOTOR_DIFF_RPM ) ) ) + 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 ) + if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_PUMP_SPEED_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) #endif - { - SET_ALARM_WITH_2_F32_DATA( ALARM_ID_BLOOD_PUMP_ROTOR_SPEED_CHECK, measRotorSpeed, measMotorSpeed ); - } + { + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_BLOOD_PUMP_ROTOR_SPEED_CHECK, measRotorSpeed, measMotorSpeed ); + } + } + } + else + { + errorBloodRotorSpeedPersistTimerCtr = 0; } } else { resetPersistentAlarmTimer( ALARM_ID_BLOOD_PUMP_MOTOR_SPEED_CHECK ); - resetPersistentAlarmTimer( ALARM_ID_BLOOD_PUMP_ROTOR_SPEED_CHECK ); + errorBloodRotorSpeedPersistTimerCtr = 0; + } } @@ -1215,7 +1249,7 @@ BOOL const isRunningMCCurrentBad = ( ( BLOOD_PUMP_OFF_STATE != bloodPumpState ) && ( bpCurr > BP_MAX_CURR_WHEN_RUNNING_MA ) ? TRUE : FALSE ); if ( ( TRUE == isPersistentAlarmTriggered( ALARM_ID_BLOOD_PUMP_MC_CURRENT_CHECK, isOffMCCurrentBad || isRunningMCCurrentBad ) ) && - ( FALSE == isAlarmActive( ALARM_ID_HD_AC_POWER_LOST ) ) ) + ( FALSE == isACPowerLost() ) ) { #ifndef _RELEASE_ if ( getSoftwareConfigStatus( SW_CONFIG_DISABLE_MOTOR_CURRNT_CHECKS ) != SW_CONFIG_ENABLE_VALUE ) @@ -1634,5 +1668,27 @@ 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; +} /**@}*/