Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -r3417fd56afc9b21fb4c2d86c75dd33ac31fbd9f1 -rbe83f01a4d54cbd0d92b68cb95a15dcbb06a9a51 --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 3417fd56afc9b21fb4c2d86c75dd33ac31fbd9f1) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision be83f01a4d54cbd0d92b68cb95a15dcbb06a9a51) @@ -29,14 +29,19 @@ // ********** private definitions ********** -#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 BLOOD_FLOW_DATA_PUB_INTERVAL (1000 / TASK_PRIORITY_INTERVAL) // interval (ms/task time) at which the blood flow data is published on the CAN bus #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 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 (200 / TASK_PRIORITY_INTERVAL) // interval (ms/task time) at which the blood pump is controlled +#define BP_P_COEFFICIENT 0.001 // P term for blood pump control +#define BP_I_COEFFICIENT 0.001 // I term for blood pump control +#define BP_MAX_ERROR_SUM 20.0 // for anti-wind-up in I term + #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 @@ -80,20 +85,22 @@ 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 -// interval (in ms) at which to publish blood flow data to CAN bus -DATA_DECL( U32, BloodFlowDataPub, bloodFlowDataPublishInterval, BLOOD_FLOW_DATA_PUB_INTERVAL, BLOOD_FLOW_DATA_PUB_INTERVAL ); 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 +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 -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 +static F32 bpFlowError = 0.0; // blood flow error +static F32 bpFlowErrorSum = 0.0; // blood flow error sum +static U32 bpControlTimerCounter = 0; // determines when to perform control on blood flow + static BLOOD_FLOW_SELF_TEST_STATE_T bloodPumpSelfTestState = BLOOD_FLOW_SELF_TEST_STATE_START; // current blood pump self test state static U32 bloodPumpSelfTestTimerCount = 0; // timer counter for blood pump self test @@ -149,6 +156,10 @@ // 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 + // reset flow control stats + bpFlowError = 0.0; + bpFlowErrorSum = 0.0; + bpControlTimerCounter = 0; switch ( bloodPumpState ) { @@ -164,7 +175,7 @@ bloodPumpState = BLOOD_PUMP_RAMPING_UP_STATE; } break; - case BLOOD_PUMP_CONTROL_TO_TARGET_STATE: // start ramp in appropriate direction + case BLOOD_PUMP_CONTROL_TO_TARGET_STATE: // start ramp to new target in appropriate direction if ( bloodPumpPWMDutyCyclePctSet > bloodPumpPWMDutyCyclePct ) { bloodPumpState = BLOOD_PUMP_RAMPING_DOWN_STATE; @@ -175,7 +186,7 @@ } break; default: - // ok - do nothing + // ok - not all states need to be handled here break; } result = TRUE; @@ -218,7 +229,7 @@ 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 ); + sprintf( debugFlowStr, "Target Flow:%5d, Meas. Flow:%5d, Speed:%5d RPM, Current:%5d mA, PWM%:%5d \n", flowStPt, (S32)measFlow, (S32)measSpd, (S32)measCurr, (S32)bloodPumpPWMDutyCyclePctSet ); sendDebugData( (U08*)debugFlowStr, strlen(debugFlowStr) ); #endif broadcastBloodFlowData( flowStPt, measFlow, measSpd, measCurr ); @@ -318,7 +329,6 @@ // have we reached end of ramp up? else if ( bloodPumpPWMDutyCyclePctSet >= bloodPumpPWMDutyCyclePct ) { - targetBloodFlowRateSet = getTargetBloodFlowRate(); result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; } // continue ramp up @@ -354,7 +364,6 @@ // have we reached end of ramp down? else if ( bloodPumpPWMDutyCyclePctSet <= bloodPumpPWMDutyCyclePct ) { - targetBloodFlowRateSet = getTargetBloodFlowRate(); result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; } // continue ramp down @@ -380,10 +389,28 @@ 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 pTerm, iTerm; + F32 newPWM; - // TODO - control -//MAX_BLOOD_PUMP_PWM_DUTY_CYCLE -//MIN_BLOOD_PUMP_PWM_DUTY_CYCLE + // control at set interval + if ( ++bpControlTimerCounter >= BP_CONTROL_INTERVAL ) + { + // compute P term + bpFlowError = tgtFlow - actFlow; + pTerm = bpFlowError * BP_P_COEFFICIENT; + // compute I term + bpFlowErrorSum += bpFlowError; + iTerm = RANGE( bpFlowErrorSum, BP_MAX_ERROR_SUM * -1.0, BP_MAX_ERROR_SUM ); + iTerm *= BP_I_COEFFICIENT; + // compute new PWM duty cycle % for blood pump motor + newPWM = bloodPumpPWMDutyCyclePctSet + pTerm + iTerm; + newPWM = RANGE( newPWM, MIN_BLOOD_PUMP_PWM_DUTY_CYCLE, MAX_BLOOD_PUMP_PWM_DUTY_CYCLE ); + bloodPumpPWMDutyCyclePctSet = newPWM; + etpwmSetCmpA( etpwmREG1, (U32)(bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD)) ); + bpControlTimerCounter = 0; + } return result; } @@ -400,7 +427,6 @@ static void stopBloodPump( void ) { isBloodPumpOn = FALSE; - targetBloodFlowRateSet = 0; bloodPumpPWMDutyCyclePctSet = 0.0; etpwmSetCmpA( etpwmREG1, 0 ); etpwmStopTBCLK();