Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -rd71d1e6c2be627158cac9a8bc56adac7cdefd1c8 -ref0b3f0ec00fadc50f95e0db1a6477fb4b076ea1 --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision d71d1e6c2be627158cac9a8bc56adac7cdefd1c8) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision ef0b3f0ec00fadc50f95e0db1a6477fb4b076ea1) @@ -33,15 +33,16 @@ // ********** 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_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 @@ -57,17 +58,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 { @@ -120,6 +124,8 @@ 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 +142,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 +159,11 @@ *************************************************************************/ 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(); } /************************************************************************* @@ -184,14 +187,14 @@ // 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 +236,31 @@ } /************************************************************************* + * @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; + bpFlowError = 0.0; + bpFlowErrorSum = 0.0; + bpControlTimerCounter = 0; +} + +/************************************************************************* * @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 +273,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 ) @@ -321,7 +340,7 @@ { // set initial PWM duty cycle bloodPumpPWMDutyCyclePctSet = MAX_BLOOD_PUMP_PWM_STEP_CHANGE; - etpwmSetCmpA( etpwmREG1, (U32)( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ); + etpwmSetCmpA( etpwmREG1, (U32)FLOAT_TO_INT_WITH_ROUND( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) ); // allow blood pump to run in requested direction setBloodPumpDirection( bloodPumpDirection ); releaseBloodPumpStop(); @@ -353,19 +372,20 @@ { // 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)FLOAT_TO_INT_WITH_ROUND( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) ); result = BLOOD_PUMP_RAMPING_DOWN_STATE; } // have we reached end of ramp up? else if ( bloodPumpPWMDutyCyclePctSet >= bloodPumpPWMDutyCyclePct ) { + resetBloodFlowMovingAverage(); 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)FLOAT_TO_INT_WITH_ROUND( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) ); } return result; @@ -394,13 +414,14 @@ // have we reached end of ramp down? else if ( bloodPumpPWMDutyCyclePctSet <= bloodPumpPWMDutyCyclePct ) { + resetBloodFlowMovingAverage(); 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)FLOAT_TO_INT_WITH_ROUND( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) ); } return result; @@ -446,7 +467,7 @@ 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) + FLOAT_TO_INT_ROUNDUP_OFFSET ) ); + etpwmSetCmpA( etpwmREG1, (U32)FLOAT_TO_INT_WITH_ROUND( bloodPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) ); bpControlTimerCounter = 0; } @@ -636,6 +657,98 @@ } /************************************************************************* + * @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; + 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: + // TODO - s/w fault? + 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.