Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -r9a9d04b84f4345fca87fb14d26f09d497b08aae8 -r94895e32fe18e78b98fe3bb7786838cf00afdbfa --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 9a9d04b84f4345fca87fb14d26f09d497b08aae8) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 94895e32fe18e78b98fe3bb7786838cf00afdbfa) @@ -21,7 +21,6 @@ #include "can.h" #include "etpwm.h" -#include "Common.h" #include "FPGA.h" #include "InternalADC.h" #include "OperationModes.h" @@ -39,34 +38,32 @@ #define MAX_BLOOD_FLOW_RATE 500 // mL/min #define MIN_BLOOD_FLOW_RATE 100 // mL/min -#define MAX_BLOOD_PUMP_PWM_STEP_CHANGE 0.005 // max duty cycle change when ramping +#define MAX_BLOOD_PUMP_PWM_STEP_CHANGE 0.01 // max duty cycle change when ramping #define MAX_BLOOD_PUMP_PWM_DUTY_CYCLE 0.88 // controller will error if PWM duty cycle > 90%, so set max to 88% #define MIN_BLOOD_PUMP_PWM_DUTY_CYCLE 0.12 // controller will error if PWM duty cycle < 10%, so set min to 12% #define BP_CONTROL_INTERVAL ( 1000 / TASK_GENERAL_INTERVAL ) // interval (ms/task time) at which the blood pump is controlled #define BP_P_COEFFICIENT 0.00005 // P term for blood pump control #define BP_I_COEFFICIENT 0.00015 // I term for blood pump control -#define BP_MAX_PWM_DC_DELTA 0.01 // prevents large steps in PWM duty cycle while controlling -#define BP_MIN_PWM_DC_DELTA -0.01 #define BP_MAX_CURR_WHEN_STOPPED_MA 150.0 // motor controller current should not exceed this when pump should be stopped #define BP_MIN_CURR_WHEN_RUNNING_MA 150.0 // motor controller current should always exceed this when pump should be running #define BP_MAX_CURR_WHEN_RUNNING_MA 1000.0 // motor controller current should not exceed this when pump should be running #define BP_MAX_CURR_ERROR_DURATION_MS 2000 // motor controller current errors persisting beyond this duration will trigger an alarm -#define BP_SPEED_ADC_TO_RPM_FACTOR 1.375 // conversion factor from ADC counts to RPM for blood pump motor -#define BP_CURRENT_ADC_TO_MA_FACTOR 2.65 // conversion factor from ADC counts to mA for blood pump motor +#define BP_SPEED_ADC_TO_RPM_FACTOR 1.280938 // conversion factor from ADC counts to RPM for blood pump motor +#define BP_CURRENT_ADC_TO_MA_FACTOR 3.002 // conversion factor from ADC counts to mA for blood pump motor -#define BP_REV_PER_LITER 124.0 // rotor revolutions per liter +#define BP_REV_PER_LITER 150.24 // rotor revolutions per liter #define BP_ML_PER_MIN_TO_PUMP_RPM_FACTOR ( BP_REV_PER_LITER / ML_PER_LITER ) #define BP_GEAR_RATIO 32.0 // blood pump motor to blood pump gear ratio -#define BP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.00042 // ~24 BP motor RPM = 1% PWM duty cycle +#define BP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.00035 // ~28 BP motor RPM = 1% PWM duty cycle #define BP_PWM_ZERO_OFFSET 0.1 // 10% PWM duty cycle = zero speed #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 ) -#define BLOODPUMP_ADC_FULL_SCALE_V 3.0 // BP analog signals are 0-3V (while int. ADC ref V is 3.3V) -#define BLOODPUMP_ADC_MID_PT_BITS ( (F32)( INT_ADC_FULL_SCALE_BITS >> 1 ) * ( BLOODPUMP_ADC_FULL_SCALE_V / INT_ADC_REF_V ) ) -#define SIGN_FROM_12_BIT_VALUE(v) ( (S16)(v) - (S16)BLOODPUMP_ADC_MID_PT_BITS ) +#define BLOODPUMP_ADC_FULL_SCALE_V 3.0 // BP analog signals are 0-3V (while int. ADC ref may be different) +#define BLOODPUMP_ADC_ZERO ( (F32)( INT_ADC_ZERO ) * ( BLOODPUMP_ADC_FULL_SCALE_V / INT_ADC_REF_V ) ) +#define SIGN_FROM_12_BIT_VALUE(v) ( (S16)(v) - (S16)BLOODPUMP_ADC_ZERO ) #define BLOOD_FLOW_SAMPLE_FREQ ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) #define SIZE_OF_ROLLING_AVG ( BLOOD_FLOW_SAMPLE_FREQ * 2 ) // measured blood flow is filtered w/ moving average @@ -94,19 +91,26 @@ #define DIR_CAN3_PORT_MASK 0x00000002 // (Rx - re-purposed as output GPIO for blood pump direction signal) // blood pump stop and direction macros #define SET_BP_DIR() {canREG3->RIOC |= DIR_CAN3_PORT_MASK;} -#define SET_BP_STOP() {canREG3->TIOC |= STOP_CAN3_PORT_MASK;} #define CLR_BP_DIR() {canREG3->RIOC &= ~DIR_CAN3_PORT_MASK;} -#define CLR_BP_STOP() {canREG3->TIOC &= ~STOP_CAN3_PORT_MASK;} +#ifndef BREADBOARD_TARGET + #define SET_BP_STOP() {canREG3->TIOC &= ~STOP_CAN3_PORT_MASK;} + #define CLR_BP_STOP() {canREG3->TIOC |= STOP_CAN3_PORT_MASK;} +#else + #define SET_BP_STOP() {canREG3->TIOC |= STOP_CAN3_PORT_MASK;} + #define CLR_BP_STOP() {canREG3->TIOC &= ~STOP_CAN3_PORT_MASK;} +#endif // ********** private data ********** -static BLOOD_PUMP_STATE_T bloodPumpState = BLOOD_PUMP_OFF_STATE; // current state of blood flow controller state machine -static U32 bloodFlowDataPublicationTimerCounter = 0; // 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 BLOOD_PUMP_STATE_T bloodPumpState = BLOOD_PUMP_OFF_STATE; // current state of blood flow controller state machine +static U32 bloodFlowDataPublicationTimerCounter = 0; // 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. DATA_DECL( U32, BloodFlowDataPub, bloodFlowDataPublishInterval, BLOOD_FLOW_DATA_PUB_INTERVAL, BLOOD_FLOW_DATA_PUB_INTERVAL ); // interval (in ms) at which to publish blood flow data to CAN bus DATA_DECL( S32, TargetBloodFlowRate, targetBloodFlowRate, 0, 0 ); // requested blood flow rate @@ -178,9 +182,10 @@ * Outputs : targetBloodFlowRate, bloodPumpdirection, bloodPumpPWMDutyCyclePct * @param flowRate : new target blood flow rate * @param dir : new blood flow direction + * @param mode : new control mode * @return TRUE if new flow rate & dir are set, FALSE if not *************************************************************************/ -BOOL setBloodPumpTargetFlowRate( U32 flowRate, MOTOR_DIR_T dir ) +BOOL setBloodPumpTargetFlowRate( U32 flowRate, MOTOR_DIR_T dir, PUMP_CONTROL_MODE_T mode ) { BOOL result = FALSE; @@ -193,6 +198,7 @@ resetBloodFlowMovingAverage(); targetBloodFlowRate.data = ( 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 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%) @@ -377,6 +383,13 @@ { resetBloodFlowMovingAverage(); resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, bloodPumpPWMDutyCyclePctSet ); + bloodPumpControlModeSet = bloodPumpControlMode; + // if open loop mode, set PWM to requested duty cycle where it will stay during control state + if ( bloodPumpControlModeSet == PUMP_CONTROL_MODE_OPEN_LOOP ) + { + bloodPumpPWMDutyCyclePctSet = bloodPumpPWMDutyCyclePct; + setBloodPumpControlSignalPWM( bloodPumpPWMDutyCyclePct ); + } result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; } // continue ramp up @@ -414,6 +427,13 @@ { resetBloodFlowMovingAverage(); resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, bloodPumpPWMDutyCyclePctSet ); + bloodPumpControlModeSet = bloodPumpControlMode; + // if open loop mode, set PWM to requested duty cycle where it will stay during control state + if ( bloodPumpControlModeSet == PUMP_CONTROL_MODE_OPEN_LOOP ) + { + bloodPumpPWMDutyCyclePctSet = bloodPumpPWMDutyCyclePct; + setBloodPumpControlSignalPWM( bloodPumpPWMDutyCyclePct ); + } result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; } // continue ramp down @@ -439,16 +459,20 @@ static BLOOD_PUMP_STATE_T handleBloodPumpControlToTargetState( void ) { BLOOD_PUMP_STATE_T result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; - F32 tgtFlow = (F32)getTargetBloodFlowRate(); - F32 actFlow = getMeasuredBloodFlowRate(); - F32 newPWM; // control at set interval if ( ++bpControlTimerCounter >= BP_CONTROL_INTERVAL ) { - newPWM = runPIController( PI_CONTROLLER_ID_BLOOD_FLOW, tgtFlow, actFlow ); - bloodPumpPWMDutyCyclePctSet = newPWM; - setBloodPumpControlSignalPWM( newPWM ); + if ( bloodPumpControlModeSet == PUMP_CONTROL_MODE_CLOSED_LOOP ) + { + F32 tgtFlow = (F32)getTargetBloodFlowRate(); + F32 actFlow = getMeasuredBloodFlowRate(); + F32 newPWM; + + newPWM = runPIController( PI_CONTROLLER_ID_BLOOD_FLOW, tgtFlow, actFlow ); + bloodPumpPWMDutyCyclePctSet = newPWM; + setBloodPumpControlSignalPWM( newPWM ); + } bpControlTimerCounter = 0; } @@ -629,10 +653,14 @@ static void publishBloodFlowData( void ) { // publish blood flow data on interval - if ( ++bloodFlowDataPublicationTimerCounter > getPublishBloodFlowDataInterval() ) + if ( ++bloodFlowDataPublicationTimerCounter >= getPublishBloodFlowDataInterval() ) { S32 flowStPt = (S32)getTargetBloodFlowRate(); +#ifndef SHOW_RAW_FLOW_VALUES F32 measFlow = getMeasuredBloodFlowRate(); +#else + F32 measFlow = getFPGABloodFlow(); +#endif F32 measRotSpd = getMeasuredBloodPumpRotorSpeed(); F32 measSpd = getMeasuredBloodPumpSpeed(); F32 measMCSpd = getMeasuredBloodPumpMCSpeed(); @@ -807,7 +835,9 @@ bpCurrErrorDurationCtr += TASK_PRIORITY_INTERVAL; if ( bpCurrErrorDurationCtr > BP_MAX_CURR_ERROR_DURATION_MS ) { +#ifndef DISABLE_MOTOR_CURRENT_ERRORS SET_ALARM_WITH_1_F32_DATA( ALARM_ID_BLOOD_PUMP_MC_CURRENT_CHECK, getMeasuredBloodPumpMCCurrent() ); +#endif } } else @@ -824,7 +854,9 @@ bpCurrErrorDurationCtr += TASK_PRIORITY_INTERVAL; if ( bpCurrErrorDurationCtr > BP_MAX_CURR_ERROR_DURATION_MS ) { +#ifndef DISABLE_MOTOR_CURRENT_ERRORS SET_ALARM_WITH_1_F32_DATA( ALARM_ID_BLOOD_PUMP_MC_CURRENT_CHECK, getMeasuredBloodPumpMCCurrent() ); +#endif } } else @@ -909,20 +941,67 @@ } /************************************************************************* - * @brief testSetTargetBloodFlowRateOverride and testResetTargetBloodFlowRateOverride + * @brief * The testSetTargetBloodFlowRateOverride function overrides the target \n * blood flow rate. \n - * The testResetTargetBloodFlowRateOverride function resets the override of the \n - * target blood flow rate. * @details * Inputs : none * Outputs : targetBloodFlowRate * @param value : override target blood flow rate (in mL/min) * @return TRUE if override successful, FALSE if not *************************************************************************/ -DATA_OVERRIDE_FUNC( S32, testSetTargetBloodFlowRateOverride, testResetTargetBloodFlowRateOverride, targetBloodFlowRate ) +BOOL testSetTargetBloodFlowRateOverride( S32 value ) +{ + BOOL result = FALSE; + if ( TRUE == isTestingActivated() ) + { + MOTOR_DIR_T dir; + + if ( value < 0 ) + { + dir = MOTOR_DIR_REVERSE; + } + else + { + dir = MOTOR_DIR_FORWARD; + } + targetBloodFlowRate.ovInitData = targetBloodFlowRate.data; // backup current target flow rate + targetBloodFlowRate.ovData = value; + targetBloodFlowRate.override = OVERRIDE_KEY; + result = setBloodPumpTargetFlowRate( ABS(value), dir, bloodPumpControlMode ); + } + + return result; +} + /************************************************************************* + * @brief + * The testResetTargetBloodFlowRateOverride function resets the override of the \n + * target blood flow rate. + * @details + * Inputs : none + * Outputs : targetBloodFlowRate + * @param none + * @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 testSetMeasuredBloodFlowRateOverride and testResetMeasuredBloodFlowRateOverride * The testResetMeasuredBloodFlowRateOverride function overrides the measured \n * blood flow rate. \n