Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -r52863cba9685f31136ab3f4b4764a17ccf34fc05 -rf7e3018ec6ab762fe08efb42b21fb2ca970174b0 --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 52863cba9685f31136ab3f4b4764a17ccf34fc05) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision f7e3018ec6ab762fe08efb42b21fb2ca970174b0) @@ -25,6 +25,7 @@ #include "FPGA.h" #include "InternalADC.h" #include "OperationModes.h" +#include "PIControllers.h" #include "SystemCommMessages.h" #include "TaskGeneral.h" #include "TaskPriority.h" @@ -33,20 +34,19 @@ // ********** private definitions ********** -#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 BLOOD_FLOW_DATA_PUB_INTERVAL ( MS_PER_SECOND / 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_FLOW_RATE 500 // mL/min +#define MIN_BLOOD_FLOW_RATE 100 // mL/min -#define MAX_BLOOD_PUMP_PWM_STEP_CHANGE 0.02 // duty cycle TODO - fixed or parameterized or set in motor controller? +#define MAX_BLOOD_PUMP_PWM_STEP_CHANGE 0.005 // 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 (500 / TASK_GENERAL_INTERVAL) // interval (ms/task time) at which the blood pump is controlled +#define BP_CONTROL_INTERVAL ( 500 / TASK_GENERAL_INTERVAL ) // interval (ms/task time) at which the blood pump is controlled #define BP_P_COEFFICIENT 0.0002 // P term for blood pump control #define BP_I_COEFFICIENT 0.00002 // I term for blood pump control -#define BP_MAX_ERROR_SUM 10.0 // for anti-wind-up in I term -#define BP_MIN_ERROR_SUM -10.0 -#define BP_MAX_PWM_DC_DELTA 0.01 // prevents large steps in PWM duty cycle +#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 @@ -57,17 +57,20 @@ #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_ML_PER_MIN_TO_PUMP_RPM_FACTOR (124.0 / ML_PER_LITER)// 124 pump revolutions = 1 liter or 1,000 mL +#define BP_REV_PER_LITER 124.0 // 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.0003125 // ~32 BP motor RPM = 1% PWM duty cycle +#define BP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.0003717 // ~27 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 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_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 SIZE_OF_ROLLING_AVG 100 // flow samples in rolling average calculations +#define BLOOD_FLOW_SAMPLE_FREQ ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) +#define SIZE_OF_ROLLING_AVG (U32)( (F32)BLOOD_FLOW_SAMPLE_FREQ * 0.8 ) // measured blood flow is filtered w/ moving average +#define MAX_FLOW_FILTER_INTERVAL 5 // slowest sample interval for filter is every 5th sample typedef enum BloodPump_States { @@ -86,7 +89,7 @@ NUM_OF_BLOOD_FLOW_SELF_TEST_STATES } BLOOD_FLOW_SELF_TEST_STATE_T; -// CAN3 port pin assignments for pump stop and direction outputs +// 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) // blood pump stop and direction macros @@ -113,13 +116,13 @@ DATA_DECL( F32, MeasuredBloodPumpMCSpeed, adcBloodPumpMCSpeedRPM, 0.0, 0.0 ); // measured blood pump motor controller speed DATA_DECL( F32, MeasuredBloodPumpMCCurrent, adcBloodPumpMCCurrentmA, 0.0, 0.0 );// measured blood pump motor controller 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 F32 flowReadings[ SIZE_OF_ROLLING_AVG ]; // holds flow samples for a rolling average static U32 flowReadingsIdx = 0; // index for next sample in rolling average array static F32 flowReadingsTotal = 0.0; // rolling total - used to calc average +static U32 flowReadingsCount = 0; // # of samples in flow rolling average buffer +static U32 flowReadingsTmrCtr = 0; // determines when to add samples to filter static U32 bpCurrErrorDurationCtr = 0; // used for tracking persistence of bp current errors @@ -136,6 +139,8 @@ static void releaseBloodPumpStop( void ); static void setBloodPumpDirection( MOTOR_DIR_T dir ); static void publishBloodFlowData( void ); +static void resetBloodFlowMovingAverage( void ); +static void filterBloodFlowReadings( F32 flow ); static void checkBloodPumpDirection( void ); static void checkBloodPumpMCCurrent( void ); static DATA_GET_PROTOTYPE( U32, getPublishBloodFlowDataInterval ); @@ -151,16 +156,16 @@ *************************************************************************/ void initBloodFlow( void ) { - U32 i; - stopBloodPump(); setBloodPumpDirection( MOTOR_DIR_FORWARD ); // zero rolling flow average buffer - for ( i = 0; i < SIZE_OF_ROLLING_AVG; i++ ) - { - flowReadings[ i ] = 0.0; - } + resetBloodFlowMovingAverage(); + + // initialize blood flow PI controller + initializePIController( PI_CONTROLLER_ID_BLOOD_FLOW, MIN_BLOOD_PUMP_PWM_DUTY_CYCLE, + BP_P_COEFFICIENT, BP_I_COEFFICIENT, + MIN_BLOOD_PUMP_PWM_DUTY_CYCLE, MAX_BLOOD_PUMP_PWM_DUTY_CYCLE ); } /************************************************************************* @@ -184,14 +189,11 @@ // verify flow rate if ( flowRate <= MAX_BLOOD_FLOW_RATE ) { + resetBloodFlowMovingAverage(); targetBloodFlowRate.data = ( dir == MOTOR_DIR_FORWARD ? (S32)flowRate : (S32)flowRate * -1 ); bloodPumpDirection = dir; // 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%) - // reset flow control stats - bpFlowError = 0.0; - bpFlowErrorSum = 0.0; - bpControlTimerCounter = 0; switch ( bloodPumpState ) { @@ -233,11 +235,30 @@ } /************************************************************************* + * @brief signalBloodPumpHardStop + * The signalBloodPumpHardStop function stops the blood pump immediately. + * @details + * Inputs : none + * Outputs : Blood pump stopped, set point reset, state changed to off + * @param none + * @return none + *************************************************************************/ +void signalBloodPumpHardStop( void ) +{ + targetBloodFlowRate.data = 0; + stopBloodPump(); + bloodPumpState = BLOOD_PUMP_OFF_STATE; + bloodPumpPWMDutyCyclePct = 0.0; + bpControlTimerCounter = 0; + resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, MIN_BLOOD_PUMP_PWM_DUTY_CYCLE ); +} + +/************************************************************************* * @brief execBloodFlowMonitor * The execBloodFlowMonitor function executes the blood flow monitor. * @details * Inputs : none - * Outputs : none + * Outputs : measuredBloodFlowRate, adcBloodPumpMCSpeedRPM, adcBloodPumpMCCurrentmA * @param none * @return none *************************************************************************/ @@ -250,11 +271,7 @@ adcBloodPumpMCSpeedRPM.data = (F32)(SIGN_FROM_12_BIT_VALUE(bpRPM)) * BP_SPEED_ADC_TO_RPM_FACTOR; adcBloodPumpMCCurrentmA.data = (F32)(SIGN_FROM_12_BIT_VALUE(bpmA)) * BP_CURRENT_ADC_TO_MA_FACTOR; - flowReadingsTotal -= flowReadings[ flowReadingsIdx ]; - flowReadings[ flowReadingsIdx ] = bpFlow; - flowReadingsTotal += bpFlow; - flowReadingsIdx = INC_WRAP( flowReadingsIdx, 0, SIZE_OF_ROLLING_AVG-1 ); - measuredBloodFlowRate.data = flowReadingsTotal / (F32)SIZE_OF_ROLLING_AVG; + filterBloodFlowReadings( bpFlow ); // don't start enforcing checks until out of init/POST mode if ( getCurrentOperationMode() != MODE_INIT ) @@ -320,13 +337,11 @@ if ( getTargetBloodFlowRate() != 0 ) { // set initial PWM duty cycle - bloodPumpPWMDutyCyclePctSet = MAX_BLOOD_PUMP_PWM_STEP_CHANGE; - etpwmSetCmpA( etpwmREG1, (U32)( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ); + bloodPumpPWMDutyCyclePctSet = BP_PWM_ZERO_OFFSET + MAX_BLOOD_PUMP_PWM_STEP_CHANGE; + etpwmSetCmpA( etpwmREG1, (U32)( (S32)( ( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) ); // allow blood pump to run in requested direction setBloodPumpDirection( bloodPumpDirection ); releaseBloodPumpStop(); - // start PWM for blood pump - etpwmStartTBCLK(); isBloodPumpOn = TRUE; result = BLOOD_PUMP_RAMPING_UP_STATE; } @@ -353,19 +368,21 @@ { // start ramp down to stop bloodPumpPWMDutyCyclePctSet -= MAX_BLOOD_PUMP_PWM_STEP_CHANGE; - etpwmSetCmpA( etpwmREG1, (U32)( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ); + etpwmSetCmpA( etpwmREG1, (U32)( (S32)( ( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) ); result = BLOOD_PUMP_RAMPING_DOWN_STATE; } // have we reached end of ramp up? else if ( bloodPumpPWMDutyCyclePctSet >= bloodPumpPWMDutyCyclePct ) { + resetBloodFlowMovingAverage(); + resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, bloodPumpPWMDutyCyclePctSet ); result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; } // continue ramp up else { bloodPumpPWMDutyCyclePctSet += MAX_BLOOD_PUMP_PWM_STEP_CHANGE; - etpwmSetCmpA( etpwmREG1, (U32)( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ); + etpwmSetCmpA( etpwmREG1, (U32)( (S32)( ( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) ); } return result; @@ -394,13 +411,15 @@ // have we reached end of ramp down? else if ( bloodPumpPWMDutyCyclePctSet <= bloodPumpPWMDutyCyclePct ) { + resetBloodFlowMovingAverage(); + resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, bloodPumpPWMDutyCyclePctSet ); result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; } // continue ramp down else { bloodPumpPWMDutyCyclePctSet -= MAX_BLOOD_PUMP_PWM_STEP_CHANGE; - etpwmSetCmpA( etpwmREG1, (U32)( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ); + etpwmSetCmpA( etpwmREG1, (U32)( (S32)( ( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) ); } return result; @@ -421,32 +440,14 @@ BLOOD_PUMP_STATE_T result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; F32 tgtFlow = (F32)getTargetBloodFlowRate(); F32 actFlow = getMeasuredBloodFlowRate(); - F32 pTerm, iTerm; F32 newPWM; // control at set interval if ( ++bpControlTimerCounter >= BP_CONTROL_INTERVAL ) { - // compute P term - if ( MOTOR_DIR_FORWARD == bloodPumpDirectionSet ) - { - bpFlowError = tgtFlow - actFlow; - } - else - { - bpFlowError = (tgtFlow * -1.0) - (actFlow * -1.0); - } - pTerm = bpFlowError * BP_P_COEFFICIENT; - pTerm = RANGE( pTerm, BP_MIN_PWM_DC_DELTA, BP_MAX_PWM_DC_DELTA ); - // compute I term - bpFlowErrorSum += bpFlowError; - iTerm = RANGE( bpFlowErrorSum, BP_MIN_ERROR_SUM, 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 ); + newPWM = runPIController( PI_CONTROLLER_ID_BLOOD_FLOW, tgtFlow, actFlow ); bloodPumpPWMDutyCyclePctSet = newPWM; - etpwmSetCmpA( etpwmREG1, (U32)( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ); + etpwmSetCmpA( etpwmREG1, (U32)( (S32)( ( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ) ); bpControlTimerCounter = 0; } @@ -467,7 +468,6 @@ isBloodPumpOn = FALSE; bloodPumpPWMDutyCyclePctSet = 0.0; etpwmSetCmpA( etpwmREG1, 0 ); - etpwmStopTBCLK(); SET_BP_STOP(); } @@ -621,20 +621,113 @@ F32 measSpd = getMeasuredBloodPumpSpeed(); F32 measMCSpd = getMeasuredBloodPumpMCSpeed(); F32 measMCCurr = getMeasuredBloodPumpMCCurrent(); + F32 pumpPWMPctDutyCycle = bloodPumpPWMDutyCyclePctSet * FRACTION_TO_PERCENT_FACTOR; #ifdef DEBUG_ENABLED // TODO - temporary debug code - remove later - S32 pwm = (S32)( 100.0 * bloodPumpPWMDutyCyclePctSet ); char debugFlowStr[ 256 ]; - sprintf( debugFlowStr, "Target Flow:%5d, Meas. Flow:%5d, Speed:%5d RPM, Current:%5d mA, PWM:%5d \n", flowStPt, (S32)measFlow, (S32)measMCSpd, (S32)measMCCurr, pwm ); + sprintf( debugFlowStr, "Blood Set Pt.:%5d, Meas. Flow:%5d, Speed:%5d RPM, Current:%5d mA, PWM:%5d \n", flowStPt, (S32)measFlow, (S32)measMCSpd, (S32)measMCCurr, (S32)pumpPWMPctDutyCycle ); sendDebugData( (U08*)debugFlowStr, strlen(debugFlowStr) ); #endif - broadcastBloodFlowData( flowStPt, measFlow, measRotSpd, measSpd, measMCSpd, measMCCurr ); + broadcastBloodFlowData( flowStPt, measFlow, measRotSpd, measSpd, measMCSpd, measMCCurr, pumpPWMPctDutyCycle ); bloodFlowDataPublicationTimerCounter = 0; } } /************************************************************************* + * @brief resetBloodFlowMovingAverage + * The resetBloodFlowMovingAverage function re-sizes and re-initializes the \n + * blood flow moving average sample buffer. + * @details + * Inputs : none + * Outputs : flowReadingsTotal, flowReadingsIdx, flowReadingsCount all set to zero. + * @param initFlow : the new blood flow set pt. + * @param flowDir : the new set direction + * @return none + *************************************************************************/ +static void resetBloodFlowMovingAverage( void ) +{ + flowReadingsTotal = 0.0; + flowReadingsIdx = 0; + flowReadingsCount = 0; + flowReadingsTmrCtr = 0; + bpControlTimerCounter = 0; +} + +/************************************************************************* + * @brief filterBloodFlowReadings + * The filterBloodFlowReadings function adds a new flow sample to the filter \n + * if decimation rate for current set point calls for it. + * @details + * Inputs : none + * Outputs : flowReadings[], flowReadingsIdx, flowReadingsCount + * @param flow : newest blood flow sample + * @return none + *************************************************************************/ +static void filterBloodFlowReadings( F32 flow ) +{ + BOOL addSampleToFilter = FALSE; + + if ( ( targetBloodFlowRate.data < MIN_BLOOD_FLOW_RATE ) || ( targetBloodFlowRate.data >= MAX_BLOOD_FLOW_RATE ) ) + { + addSampleToFilter = TRUE; + } + else + { + switch ( flowReadingsTmrCtr ) + { + case 0: + addSampleToFilter = TRUE; + break; + + case 1: + addSampleToFilter = FALSE; + break; + + case 2: + if ( targetBloodFlowRate.data >= 400 ) + { + addSampleToFilter = TRUE; + } + break; + + case 3: + if ( targetBloodFlowRate.data >= 200 ) + { + addSampleToFilter = TRUE; + } + break; + + case 4: + if ( targetBloodFlowRate.data >= 300 ) + { + addSampleToFilter = TRUE; + } + break; + + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_BLOOD_FLOW_INVALID_FILTER_STATE, flowReadingsTmrCtr ) + break; + } + } + + if ( TRUE == addSampleToFilter ) + { + if ( flowReadingsCount >= SIZE_OF_ROLLING_AVG ) + { + flowReadingsTotal -= flowReadings[ flowReadingsIdx ]; + } + flowReadings[ flowReadingsIdx ] = flow; + flowReadingsTotal += flow; + flowReadingsIdx = INC_WRAP( flowReadingsIdx, 0, SIZE_OF_ROLLING_AVG - 1 ); + flowReadingsCount = INC_CAP( flowReadingsCount, SIZE_OF_ROLLING_AVG ); + measuredBloodFlowRate.data = flowReadingsTotal / (F32)flowReadingsCount; + } + + flowReadingsTmrCtr = INC_WRAP( flowReadingsTmrCtr, 0, MAX_FLOW_FILTER_INTERVAL - 1 ); +} + +/************************************************************************* * @brief checkBloodPumpDirection * The checkBloodPumpDirection function checks the set direction vs. \n * the direction implied by the sign of the measured MC speed.