Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -rce58760d9ec26a8ddc4d2462671d7f3db961f621 -reb75e7807c5e85481f1447a3dfebf11db16d8259 --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision ce58760d9ec26a8ddc4d2462671d7f3db961f621) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision eb75e7807c5e85481f1447a3dfebf11db16d8259) @@ -42,7 +42,7 @@ #define MAX_SETTABLE_BLOOD_FLOW_RATE 700 ///< Maximum settable blood flow rate (in mL/min). -#define BP_CONTROL_INTERVAL_SEC 2 ///< Blood pump control interval (in seconds). +#define BP_CONTROL_INTERVAL_SEC 5 ///< Blood pump control interval (in seconds). /// Interval (ms/task time) at which the blood pump is controlled. static const U32 BP_CONTROL_INTERVAL = ( BP_CONTROL_INTERVAL_SEC * MS_PER_SECOND / TASK_GENERAL_INTERVAL ); @@ -73,9 +73,6 @@ /// Persist time period (in ms) blood pump rotor speed too fast error condition. static const U32 BP_MAX_ROTOR_SPEED_ERROR_PERSIST = ( 10 * MS_PER_SECOND ); -/// Measured blood pump speed is filtered w/ 1 second moving average. -#define SIZE_OF_BP_ROLLING_AVG ( ( MS_PER_SECOND / TASK_GENERAL_INTERVAL ) * 1 ) - #define BP_RAMP_STEP_SPEED_RPM 50 ///< Blood pump ramp step size (in RPM). #define BP_ML_PER_ROTOR_REV 12.26F ///< Default blood pump volume (mL) per rotor revolution. @@ -89,7 +86,10 @@ #define DATA_PUBLISH_COUNTER_START_COUNT 20 ///< Data publish counter start count. -#define SIZE_OF_ROLLING_AVG 20 ///< Number of pump speed samples in rolling average. +#define MAX_BP_SPEED_FILTER_SAMPLES 600 ///< Max MA samples (50 ms ticks); 600 = 30 s span at 50 mL/min. +#define BP_SPEED_FILTER_TIME_FLOW_PRODUCT 1500 ///< s*mL/min; filter duration T = PRODUCT / |Q| (e.g. 2.5 s @ 600, 5 s @ 300). +#define BP_SPEED_FILTER_DEFAULT_SAMPLES 20 ///< MA samples when target flow is zero (~1 s @ 50 ms). +#define BP_SPEED_FILTER_MIN_SAMPLES 5 ///< Minimum MA window (samples). #define BP_TORQUE_PERIOD_RESOLUTION_US 1.0F ///< Blood pump torque period resolution in microseconds (1 us). #define BP_1KHZ_TO_TORQUE_CONVERSION_MNM 10.0F ///< Blood pump 1kHz to torque conversion in milli-newtonmeter @@ -135,10 +135,11 @@ static OVERRIDE_F32_T bloodPumpSpeedRPM; ///< Measured blood pump motor speed. static OVERRIDE_F32_T bloodPumpTorquemNm; ///< Measured blood pump torque in mNm. -static F32 rpmReadings[ SIZE_OF_ROLLING_AVG ]; ///< Holds RPM samples for a rolling average. +static F32 rpmReadings[ MAX_BP_SPEED_FILTER_SAMPLES ]; ///< Holds RPM samples for a rolling average. static U32 rpmReadingsIdx; ///< Index for next sample in rolling average array. static F32 rpmReadingsTotal; ///< Rolling total - used to calc average. static U32 rpmReadingsCount; ///< Number of samples in RPM rolling average buffer. +static U32 rpmFilterActiveWindow; ///< Active MA window (samples); 0 after reset until first filter pass. static F32 filteredBloodPumpSpeed; ///< Filtered blood pump speed used in blood flow estimation. static U32 bpControlTimerCounter; ///< Determines when to perform control on blood flow. @@ -171,6 +172,7 @@ static F32 getBPStrokeVolume( void ); static U32 getBPMaxRotorCountForWear( void ); static void resetBloodPumpRPMMovingAverage( void ); +static U32 calcBpSpeedFilterWindowSamples( void ); static void filterBloodPumpRPMReadings( F32 rpm ); static F32 calcBloodPumpTorque( void ); @@ -203,6 +205,7 @@ bloodFlowDataPublicationTimerCounter = DATA_PUBLISH_COUNTER_START_COUNT; bpHomeRequested = FALSE; bpHomeStartTime = 0; + rpmFilterActiveWindow = 0; // Initialize overrides bloodFlowDataPublishInterval.data = BLOOD_FLOW_DATA_PUB_INTERVAL; @@ -247,7 +250,7 @@ bpMaxRotorCountForWear.override = OVERRIDE_RESET; // Initialize pump speed filter - for ( i = 0; i < SIZE_OF_ROLLING_AVG; i++ ) + for ( i = 0; i < MAX_BP_SPEED_FILTER_SAMPLES; i++ ) { rpmReadings[ i ] = 0.0F; } @@ -748,7 +751,7 @@ { F32 tgtFlow = (F32)targetBloodFlowRate; F32 actFlow = getMeasuredBloodFlowRate(); - F32 newRPM= runPIController( PI_CONTROLLER_ID_BLOOD_FLOW, tgtFlow, actFlow ); + F32 newRPM = runPIController( PI_CONTROLLER_ID_BLOOD_FLOW, tgtFlow, actFlow ); bloodPumpSetSpeedRPM = (U32)((S32)(newRPM)); setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); @@ -947,27 +950,94 @@ rpmReadingsCount = 0; rpmReadingsTotal = 0.0F; filteredBloodPumpSpeed = 0.0F; + rpmFilterActiveWindow = 0; } /*********************************************************************//** * @brief + * The calcBpSpeedFilterWindowSamples function computes how many general-task + * samples to use for the blood pump motor speed moving average. Let Q be the + * magnitude of target blood flow (mL/min). Averaging duration T (seconds) is + * BP_SPEED_FILTER_TIME_FLOW_PRODUCT / Q. Sample count N is round( T times + * MS_PER_SECOND / TASK_GENERAL_INTERVAL ), clamped to BP_SPEED_FILTER_MIN_SAMPLES + * through MAX_BP_SPEED_FILTER_SAMPLES. If target blood flow is zero, N is + * BP_SPEED_FILTER_DEFAULT_SAMPLES. + * @details \b Inputs: targetBloodFlowRate, MAX_SETTABLE_BLOOD_FLOW_RATE, + * BP_SPEED_FILTER_TIME_FLOW_PRODUCT, MS_PER_SECOND, TASK_GENERAL_INTERVAL, + * BP_SPEED_FILTER_MIN_SAMPLES, BP_SPEED_FILTER_DEFAULT_SAMPLES, + * MAX_BP_SPEED_FILTER_SAMPLES + * @details \b Outputs: none + * @return moving-average window size in samples + *************************************************************************/ +static U32 calcBpSpeedFilterWindowSamples( void ) +{ + U32 absFlowU32; + F32 tFilterSec; + F32 windowSamplesF; + U32 windowSamples; + + if ( 0 == targetBloodFlowRate ) + { + return BP_SPEED_FILTER_DEFAULT_SAMPLES; + } + + absFlowU32 = ( targetBloodFlowRate >= 0 ) ? (U32)targetBloodFlowRate : (U32)( -targetBloodFlowRate ); + if ( 0U == absFlowU32 ) + { + return BP_SPEED_FILTER_DEFAULT_SAMPLES; + } + if ( absFlowU32 > (U32)MAX_SETTABLE_BLOOD_FLOW_RATE ) + { + absFlowU32 = (U32)MAX_SETTABLE_BLOOD_FLOW_RATE; + } + + tFilterSec = (F32)BP_SPEED_FILTER_TIME_FLOW_PRODUCT / (F32)absFlowU32; + windowSamplesF = ( tFilterSec * (F32)MS_PER_SECOND / (F32)TASK_GENERAL_INTERVAL ) + 0.5F; + windowSamples = (U32)windowSamplesF; + + if ( windowSamples < BP_SPEED_FILTER_MIN_SAMPLES ) + { + windowSamples = BP_SPEED_FILTER_MIN_SAMPLES; + } + if ( windowSamples > (U32)MAX_BP_SPEED_FILTER_SAMPLES ) + { + windowSamples = (U32)MAX_BP_SPEED_FILTER_SAMPLES; + } + + return windowSamples; +} + +/*********************************************************************//** + * @brief * The filterBloodPumpRPMReadings function adds a new pump speed sample to - * the filter. - * @details \b Inputs: none - * @details \b Outputs: rpmReadings[], rpmReadingsIdx, rpmReadingsCount, rpmReadingsTotal - * @param rpm newest blood pump speed (in RPM) sample to add to filter + * the moving average used for blood flow estimation. + * @param rpm newest blood pump motor speed (in RPM) sample to add to the filter + * @details \b Inputs: rpm, targetBloodFlowRate (via calcBpSpeedFilterWindowSamples) + * @details \b Outputs: rpmReadings[], rpmReadingsIdx, rpmReadingsCount, + * rpmReadingsTotal, filteredBloodPumpSpeed, rpmFilterActiveWindow * @return none *************************************************************************/ static void filterBloodPumpRPMReadings( F32 rpm ) { - if ( rpmReadingsCount >= SIZE_OF_ROLLING_AVG ) + U32 filterWindowSamples = calcBpSpeedFilterWindowSamples(); + + if ( filterWindowSamples != rpmFilterActiveWindow ) { + rpmReadingsIdx = 0; + rpmReadingsCount = 0; + rpmReadingsTotal = 0.0F; + filteredBloodPumpSpeed = 0.0F; + rpmFilterActiveWindow = filterWindowSamples; + } + + if ( rpmReadingsCount >= filterWindowSamples ) + { rpmReadingsTotal -= rpmReadings[ rpmReadingsIdx ]; } rpmReadings[ rpmReadingsIdx ] = rpm; rpmReadingsTotal += rpm; - rpmReadingsIdx = INC_WRAP( rpmReadingsIdx, 0, SIZE_OF_ROLLING_AVG - 1 ); - rpmReadingsCount = INC_CAP( rpmReadingsCount, SIZE_OF_ROLLING_AVG ); + rpmReadingsIdx = INC_WRAP( rpmReadingsIdx, 0, filterWindowSamples - 1 ); + rpmReadingsCount = INC_CAP( rpmReadingsCount, filterWindowSamples ); filteredBloodPumpSpeed = rpmReadingsTotal / (F32)rpmReadingsCount; }