Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -r5dea4d2934a06e63b87c74732d5d5d7756bdc62a -rf3a26c402a2ec88f5ee7dbb8eb7127ab5b4692aa --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 5dea4d2934a06e63b87c74732d5d5d7756bdc62a) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision f3a26c402a2ec88f5ee7dbb8eb7127ab5b4692aa) @@ -76,7 +76,7 @@ #define BP_RATE_FROM_RPM( rpm ) ( rpm / 5 ) ///< Macro to estimate a flow rate (mL/min) from a given speed (RPM). #define BP_RPM_FROM_RATE( rate ) ( rate * 5 ) ///< Macro to estimate a pump speed (RPM) from a given flow rate (mL/min). -#define BP_RAMP_STEP_SPEED_RPM 5 ///< Blood pump ramp step size (in RPM). +#define BP_RAMP_STEP_SPEED_RPM 50 ///< Blood pump ramp step size (in RPM). #define BP_FLOW_ALPHA_Y_INTERCEPT 1.11F ///< Y intercept used for alpha flow coefficient calculation. #define BP_FLOW_WEAR_A_TERM 0.00000000896F ///< A term used for wear portion of alpha flow coefficient. @@ -118,6 +118,9 @@ static U16 bloodPumpRampToSpeedRPM; ///< Estimated target speed that we want to ramp to. static U16 bloodPumpSetSpeedRPM; ///< Current set speed for the blood pump. +/// Macro for determining the signed set speed for the blood pump based on currently set direction and unsigned set speed. +#define BP_SIGNED_SET_SPEED ( MOTOR_DIR_REVERSE == bloodPumpDirectionSet ? (S32)bloodPumpSetSpeedRPM * -1 : (S32)bloodPumpSetSpeedRPM ) + static OVERRIDE_U32_T bloodFlowDataPublishInterval; ///< Interval (in task intervals) at which to publish blood flow data to CAN bus. static S32 targetBloodFlowRate; ///< Requested blood flow rate. static OVERRIDE_F32_T measuredBloodFlowRate; ///< Measured (calculated now) blood flow rate. @@ -166,6 +169,8 @@ *************************************************************************/ void initBloodFlow( void ) { + U32 i; + // Initialize peristaltic pump driver initPeristalticPumpDriver(); @@ -207,14 +212,13 @@ bloodPumpRotorCounter.override = OVERRIDE_RESET; // Initialize pump speed filter - memset( &rpmReadings[ 0 ], 0, sizeof( rpmReadings ) ); - rpmReadingsIdx = 0; - rpmReadingsTotal = 0.0; - rpmReadingsCount = 0; - filteredBloodPumpSpeed = 0.0; - - // Reset pump speed moving average and rotor count + for ( i = 0; i < SIZE_OF_ROLLING_AVG; i++ ) + { + rpmReadings[ i ] = 0.0F; + } resetBloodPumpRPMMovingAverage(); + + // Reset pump rotor count resetBloodPumpRotorCount(); // Initialize blood flow PI controller @@ -262,7 +266,7 @@ // isFlowInrange = TRUE; // } #endif - // Verify flow rate of if the bypass flow limit has been enabled + // Verify flow rate valid if ( TRUE == isFlowInrange ) { resetBloodPumpRPMMovingAverage(); @@ -492,8 +496,8 @@ * @brief * The execBloodFlowMonitor function executes the blood flow monitor. * @details \b Inputs: none - * @details \b Outputs: measuredBloodFlowRate, adcBloodPumpMCSpeedRPM, - * adcBloodPumpMCCurrentmA + * @details \b Outputs: BP sensor readings updated, filteredBloodPumpSpeed, + * bloodPumpSpeedRPM, bloodPumpDirection, measuredBloodFlowRate * @return none *************************************************************************/ void execBloodFlowMonitor( void ) @@ -508,7 +512,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; - filterBloodPumpRPMReadings( (F32)getPeristalticPumpMeasSpeed() ); + filterBloodPumpRPMReadings( getPeristalticPumpMeasSpeed() ); bloodPumpSpeedRPM.data = filteredBloodPumpSpeed; bloodPumpDirection = ( getMeasuredBloodPumpSpeed() < 0.0F ? MOTOR_DIR_REVERSE : MOTOR_DIR_FORWARD ); //measuredBloodFlowRate.data = calcBloodFlow(); // TODO-restore when flow estimation function is implemented @@ -584,8 +588,7 @@ { // Start ramp up to target flow rate bloodPumpSetSpeedRPM = BP_RAMP_STEP_SPEED_RPM; - setBPDirection( bloodPumpDirectionSet ); - setBPSetSpeed( bloodPumpSetSpeedRPM ); + setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); isBloodPumpOn = TRUE; result = BLOOD_PUMP_RAMPING_UP_STATE; } @@ -615,15 +618,15 @@ { // Start ramp down to stop bloodPumpSetSpeedRPM -= BP_RAMP_STEP_SPEED_RPM; - setBPSetSpeed( bloodPumpSetSpeedRPM ); + setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); result = BLOOD_PUMP_RAMPING_DN_STATE; } // Have we reached end of ramp up? else if ( bloodPumpSetSpeedRPM >= bloodPumpRampToSpeedRPM ) { resetBloodPumpRPMMovingAverage(); bloodPumpSetSpeedRPM = bloodPumpRampToSpeedRPM; - setBPSetSpeed( bloodPumpSetSpeedRPM ); + setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); // resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, bloodPumpSetSpeedRPM ); bpControlTimerCounter = 0; result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; @@ -633,7 +636,7 @@ { bloodPumpSetSpeedRPM += BP_RAMP_STEP_SPEED_RPM; bloodPumpSetSpeedRPM = MIN( bloodPumpSetSpeedRPM, bloodPumpRampToSpeedRPM ); - setBPSetSpeed( bloodPumpSetSpeedRPM ); + setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); } return result; @@ -661,9 +664,8 @@ else if ( bloodPumpSetSpeedRPM <= bloodPumpRampToSpeedRPM ) { resetBloodPumpRPMMovingAverage(); - bloodPumpSetSpeedRPM = 0; // resetPIController( PI_CONTROLLER_ID_BLOOD_FLOW, bloodPumpSetSpeedRPM ); - setBPSetSpeed( bloodPumpSetSpeedRPM ); + setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); bpControlTimerCounter = 0; result = BLOOD_PUMP_CONTROL_TO_TARGET_STATE; } @@ -672,7 +674,7 @@ { bloodPumpSetSpeedRPM -= BP_RAMP_STEP_SPEED_RPM; bloodPumpSetSpeedRPM = MAX( bloodPumpSetSpeedRPM, bloodPumpRampToSpeedRPM ); - setBPSetSpeed( bloodPumpSetSpeedRPM ); + setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); } return result; @@ -702,7 +704,7 @@ // newRPM = runPIController( PI_CONTROLLER_ID_BLOOD_FLOW, tgtFlow, actFlow ); // newRPM = ( newRPM < 0.0F ? 0.0F : newRPM ); bloodPumpSetSpeedRPM = (U32)((S32)(newRPM)); - setBPSetSpeed( bloodPumpSetSpeedRPM ); + setPeristalticPumpSetSpeed( BP_SIGNED_SET_SPEED ); } bpControlTimerCounter = 0; } @@ -801,22 +803,22 @@ TD_OP_MODE_T opMode = getCurrentOperationMode(); U32 hallSensor = 0; // TODO gioGetBit( hetPORT1, BP_ROTOR_HALL_SENSOR_NHET_ID ); - payload.setFlowRate = targetBloodFlowRate; - payload.measFlow = getMeasuredBloodFlowRate(); - payload.measRotorSpd = getMeasuredBloodPumpRotorSpeed(); - payload.measPumpSpd = getMeasuredBloodPumpSpeed(); -// payload.measCurr = getMeasuredBloodPumpMCCurrent(); - payload.setRPM = bloodPumpSetSpeedRPM; - payload.rotorCount = getBloodPumpRotorCount(); - if ( ( MODE_PRET == opMode ) || ( MODE_TREA == opMode ) || ( MODE_POST == opMode ) ) - { // prescribed flow only available in treatment modes + payload.h4SetFlowRate = targetBloodFlowRate; + payload.h4MeasFlow = getMeasuredBloodFlowRate(); + payload.h4MeasRotorSpd = getMeasuredBloodPumpRotorSpeed(); + payload.h4MeasPumpSpd = getMeasuredBloodPumpSpeed(); + payload.h4MeasCurr = 0.0F; // TODO getMeasuredBloodPumpMCCurrent(); + payload.h4SetRPM = bloodPumpSetSpeedRPM; + payload.h4RotorCount = getBloodPumpRotorCount(); +// if ( ( MODE_PRET == opMode ) || ( MODE_TREA == opMode ) || ( MODE_POST == opMode ) ) +// { // prescribed flow only available in treatment modes // payload.presFlow = getTreatmentParameterU32( TREATMENT_PARAM_BLOOD_FLOW ); // TODO - restore when Tx Param Mode implemented // } // else -// { - payload.presFlow = 0; + { + payload.h4PresFlow = 0; } - payload.rotorHall = ( hallSensor > 0 ? 0 : 1 ); // 1=home, 0=not home + payload.h4RotorHallState = ( hallSensor > 0 ? 0 : 1 ); // 1=home, 0=not home broadcastData( MSG_ID_TD_BLOOD_PUMP_DATA, COMM_BUFFER_OUT_CAN_TD_BROADCAST, (U08*)&payload, sizeof( BLOOD_PUMP_STATUS_PAYLOAD_T ) ); bloodFlowDataPublicationTimerCounter = 0; } @@ -834,8 +836,8 @@ { rpmReadingsIdx = 0; rpmReadingsCount = 0; - rpmReadingsTotal = 0.0; - filteredBloodPumpSpeed = 0.0; + rpmReadingsTotal = 0.0F; + filteredBloodPumpSpeed = 0.0F; } /*********************************************************************//**