Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -r1f3647830e9de0a1f0a4e445ce8d72d5525f51fb -r90f6438e80dbe0a32472a076a0d1bc54db65d15a --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 1f3647830e9de0a1f0a4e445ce8d72d5525f51fb) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 90f6438e80dbe0a32472a076a0d1bc54db65d15a) @@ -14,10 +14,13 @@ * **************************************************************************/ +#include + #include "can.h" #include "etpwm.h" #include "Common.h" +#include "FPGA.h" #include "InternalADC.h" #include "SystemCommMessages.h" #include "TaskPriority.h" @@ -26,14 +29,27 @@ // ********** private definitions ********** -#define MAX_BLOOD_FLOW_RATE 600 // mL/min -#define MAX_BLOOD_PUMP_PWM_STEP_CHANGE 0.005 // duty cycle TODO - fixed or parameterized or set in motor controller? +#define BLOOD_FLOW_DATA_PUB_INTERVAL (1000 / TASK_PRIORITY_INTERVAL) // interval (ms) at which the blood flow data is published on the CAN bus -#define BP_SPEED_ADC_2_RPM_FACTOR 1.0 // conversion factor from ADC counts to RPM for blood pump motor TODO - set appropriate value -#define BP_CURRENT_ADC_2_MA_FACTOR 1.0 // conversion factor from ADC counts to mA for blood pump motor TODO - set appropriate value +#define MAX_BLOOD_FLOW_RATE 600 // mL/min -#define BLOOD_FLOW_DATA_PUB_INTERVAL (1000 / TASK_PRIORITY_INTERVAL) // interval (ms) at which the blood flow data is published on the CAN bus +#define MAX_BLOOD_PUMP_PWM_STEP_CHANGE 0.005 // duty cycle TODO - fixed or parameterized or set in motor controller? +#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_SPEED_ADC_TO_RPM_FACTOR 1.375 // conversion factor from ADC counts to RPM for blood pump motor TODO - set appropriate value +#define BP_CURRENT_ADC_TO_MA_FACTOR 2.65 // conversion factor from ADC counts to mA for blood pump motor TODO - set appropriate value + +#define BP_ML_PER_MIN_TO_PUMP_RPM_FACTOR (80.0 / 1000.0) // 80 pump revolutions = 1 liter or 1,000 mL +#define BP_GEAR_RATIO 32.0 // blood pump motor to blood pump gear ratio +#define BP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.0003125 // ~32 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) + typedef enum BloodPump_States { BLOOD_PUMP_OFF_STATE = 0, @@ -52,13 +68,13 @@ } BLOOD_FLOW_SELF_TEST_STATE_T; // CAN3 port pin assignments for pump stop and direction outputs -#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 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) // 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;} +#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;} // ********** private data ********** @@ -71,8 +87,8 @@ 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 -DATA_DECL( U32, TargetBloodFlowRate, targetBloodFlowRate, 0, 0 ); // requested blood flow rate -static U32 targetBloodFlowRateSet = 0; // currently set blood flow rate +DATA_DECL( S32, TargetBloodFlowRate, targetBloodFlowRate, 0, 0 ); // requested blood flow rate +static S32 targetBloodFlowRateSet = 0; // currently set blood flow rate DATA_DECL( F32, MeasuredBloodFlowRate, measuredBloodFlowRate, 0.0, 0.0 ); // measured blood flow rate DATA_DECL( F32, MeasuredBloodPumpSpeed, adcBloodPumpSpeedRPM, 0.0, 0.0 ); // measured blood pump speed DATA_DECL( F32, MeasuredBloodPumpCurrent, adcBloodPumpCurrentmA, 0.0, 0.0 ); // measured blood pump motor current @@ -126,10 +142,12 @@ // verify flow rate if ( flowRate <= MAX_BLOOD_FLOW_RATE ) { - targetBloodFlowRate.data = flowRate; + targetBloodFlowRate.data = ( dir == MOTOR_DIR_FORWARD ? (S32)flowRate : (S32)flowRate * -1 ); bloodPumpDirection = dir; - // TODO - this is temporary conversion to initial duty cycle - bloodPumpPWMDutyCyclePct = ( (F32)flowRate / 800.0 ); + // 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%) + bloodPumpPWMDutyCyclePct = MIN(bloodPumpPWMDutyCyclePct,MAX_BLOOD_PUMP_PWM_DUTY_CYCLE); // limit pwm duty cycle to controller maximum + switch ( bloodPumpState ) { case BLOOD_PUMP_RAMPING_UP_STATE: // see if we need to reverse direction of ramp @@ -145,7 +163,7 @@ } break; case BLOOD_PUMP_CONTROL_TO_TARGET_STATE: // start ramp in appropriate direction - if ( targetBloodFlowRateSet > getTargetBloodFlowRate() ) + if ( bloodPumpPWMDutyCyclePctSet > bloodPumpPWMDutyCyclePct ) { bloodPumpState = BLOOD_PUMP_RAMPING_DOWN_STATE; } @@ -183,17 +201,24 @@ U16 bpRPM = getIntADCReading( INT_ADC_BLOOD_PUMP_SPEED ); U16 bpmA = getIntADCReading( INT_ADC_BLOOD_PUMP_MOTOR_CURRENT ); - adcBloodPumpSpeedRPM.data = (F32)(SIGN_FROM_12_BIT_VALUE(bpRPM)) * BP_SPEED_ADC_2_RPM_FACTOR; - adcBloodPumpCurrentmA.data = (F32)(SIGN_FROM_12_BIT_VALUE(bpmA)) * BP_CURRENT_ADC_2_MA_FACTOR; + measuredBloodFlowRate.data = getFPGABloodFlow(); + adcBloodPumpSpeedRPM.data = (F32)(SIGN_FROM_12_BIT_VALUE(bpRPM)) * BP_SPEED_ADC_TO_RPM_FACTOR; + adcBloodPumpCurrentmA.data = (F32)(SIGN_FROM_12_BIT_VALUE(bpmA)) * BP_CURRENT_ADC_TO_MA_FACTOR; // publish blood flow data on interval if ( ++bloodFlowDataPublicationTimerCounter > getPublishBloodFlowDataInterval() ) { - U32 flowStPt = getTargetBloodFlowRate(); + S32 flowStPt = (S32)getTargetBloodFlowRate(); F32 measFlow = getMeasuredBloodFlowRate(); F32 measSpd = getMeasuredBloodPumpSpeed(); F32 measCurr = getMeasuredBloodPumpCurrent(); +#ifdef DEBUG_ENABLED + F32 tmp = getFPGADialysateFlow(); + char debugFlowStr[256]; + sprintf( debugFlowStr, "Target Flow:%5d, Meas. Flow:%5d, Speed:%5d RPM, Current:%5d mA \n", flowStPt, (S32)measFlow, (S32)measSpd, (S32)measCurr ); + sendDebugData( (U08*)debugFlowStr, strlen(debugFlowStr) ); +#endif broadcastBloodFlowData( flowStPt, measFlow, measSpd, measCurr ); bloodFlowDataPublicationTimerCounter = 0; } @@ -249,7 +274,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 ( getTargetBloodFlowRate() != 0 ) { // set initial PWM duty cycle bloodPumpPWMDutyCyclePctSet = MAX_BLOOD_PUMP_PWM_STEP_CHANGE; @@ -319,13 +344,8 @@ BLOOD_PUMP_STATE_T result = BLOOD_PUMP_RAMPING_DOWN_STATE; // have we essentially reached zero speed - if ( bloodPumpPWMDutyCyclePctSet < MAX_BLOOD_PUMP_PWM_STEP_CHANGE ) + if ( bloodPumpPWMDutyCyclePctSet < (MAX_BLOOD_PUMP_PWM_STEP_CHANGE + BP_PWM_ZERO_OFFSET) ) { - isBloodPumpOn = FALSE; - targetBloodFlowRateSet = 0; - bloodPumpPWMDutyCyclePctSet = 0.0; - etpwmSetCmpA( etpwmREG1, 0 ); - etpwmStopTBCLK(); stopBloodPump(); result = BLOOD_PUMP_OFF_STATE; } @@ -360,6 +380,8 @@ BLOOD_PUMP_STATE_T result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; // TODO - control +//MAX_BLOOD_PUMP_PWM_DUTY_CYCLE +//MIN_BLOOD_PUMP_PWM_DUTY_CYCLE return result; } @@ -369,12 +391,17 @@ * The stopBloodPump function sets the blood pump stop signal. * @details * Inputs : none - * Outputs : blood pump stop signal + * Outputs : blood pump stop signal activated, PWM duty cycle zeroed * @param none * @return none *************************************************************************/ static void stopBloodPump( void ) { + isBloodPumpOn = FALSE; + targetBloodFlowRateSet = 0; + bloodPumpPWMDutyCyclePctSet = 0.0; + etpwmSetCmpA( etpwmREG1, 0 ); + etpwmStopTBCLK(); SET_BP_STOP(); } @@ -444,7 +471,7 @@ * @param none * @return the current target blood flow rate (in mL/min). *************************************************************************/ -DATA_GET( U32, getTargetBloodFlowRate, targetBloodFlowRate ) +DATA_GET( S32, getTargetBloodFlowRate, targetBloodFlowRate ) /************************************************************************* * @brief getMeasuredBloodFlowRate @@ -549,7 +576,7 @@ * @param value : override target blood flow rate (in mL/min) * @return TRUE if override successful, FALSE if not *************************************************************************/ -DATA_OVERRIDE_FUNC( U32, testSetTargetBloodFlowRateOverride, testResetTargetBloodFlowRateOverride, targetBloodFlowRate ) +DATA_OVERRIDE_FUNC( S32, testSetTargetBloodFlowRateOverride, testResetTargetBloodFlowRateOverride, targetBloodFlowRate ) /************************************************************************* * @brief testSetMeasuredBloodFlowRateOverride and testResetMeasuredBloodFlowRateOverride