Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -r620104f4a9e3148575703981a3063b9605b6e9b8 -r941afbaab7fc86f40fa49f9d110d481f65b44b68 --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 620104f4a9e3148575703981a3063b9605b6e9b8) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 941afbaab7fc86f40fa49f9d110d481f65b44b68) @@ -133,6 +133,9 @@ static void stopBloodPump( void ); static void releaseBloodPumpStop( void ); static void setBloodPumpDirection( MOTOR_DIR_T dir ); +static void publishBloodFlowData( void ); +static void checkBloodPumpDirection( void ); +static void checkBloodPumpMCCurrent( void ); static DATA_GET_PROTOTYPE( U32, getPublishBloodFlowDataInterval ); /************************************************************************* @@ -183,7 +186,6 @@ 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%) - 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; @@ -241,8 +243,7 @@ { U16 bpRPM = getIntADCReading( INT_ADC_BLOOD_PUMP_SPEED ); U16 bpmA = getIntADCReading( INT_ADC_BLOOD_PUMP_MOTOR_CURRENT ); - F32 bpFlow = getFPGABloodFlow(); // TODO - change to avg. blood flow when available from FPGA - MOTOR_DIR_T bpMCDir; + F32 bpFlow = getFPGABloodFlow(); adcBloodPumpSpeedRPM.data = (F32)(SIGN_FROM_12_BIT_VALUE(bpRPM)) * BP_SPEED_ADC_TO_RPM_FACTOR; adcBloodPumpCurrentmA.data = (F32)(SIGN_FROM_12_BIT_VALUE(bpmA)) * BP_CURRENT_ADC_TO_MA_FACTOR; @@ -256,71 +257,12 @@ // don't start enforcing checks until out of init/POST mode if ( getCurrentOperationMode() != MODE_INIT ) { - if ( BLOOD_PUMP_CONTROL_TO_TARGET_STATE == bloodPumpState ) - { - // check set direction vs. direction from sign of motor controller speed - bpMCDir = ( getMeasuredBloodPumpSpeed() >= 0.0 ? MOTOR_DIR_FORWARD : MOTOR_DIR_REVERSE ); - if ( bloodPumpDirectionSet != bpMCDir ) - { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_BLOOD_PUMP_MC_DIRECTION_CHECK, (U32)bloodPumpDirectionSet, (U32)bpMCDir ) - } - } - - // check motor controller current thresholds as appropriate - if ( BLOOD_PUMP_OFF_STATE == bloodPumpState ) - { - F32 bpCurr = fabs( getMeasuredBloodPumpCurrent() ); - - if ( bpCurr > BP_MAX_CURR_WHEN_STOPPED_MA ) - { - bpCurrErrorDurationCtr += TASK_PRIORITY_INTERVAL; - if ( bpCurrErrorDurationCtr > BP_MAX_CURR_ERROR_DURATION_MS ) - { - SET_ALARM_WITH_1_F32_DATA( ALARM_ID_BLOOD_PUMP_MC_CURRENT_CHECK, getMeasuredBloodPumpCurrent() ); - } - } - else - { - bpCurrErrorDurationCtr = 0; - } - } - else // pump s/b running - { - F32 bpCurr = fabs( getMeasuredBloodPumpCurrent() ); - - if ( ( bpCurr < BP_MIN_CURR_WHEN_RUNNING_MA ) || ( bpCurr > BP_MAX_CURR_WHEN_RUNNING_MA ) ) - { - bpCurrErrorDurationCtr += TASK_PRIORITY_INTERVAL; - if ( bpCurrErrorDurationCtr > BP_MAX_CURR_ERROR_DURATION_MS ) - { - SET_ALARM_WITH_1_F32_DATA( ALARM_ID_BLOOD_PUMP_MC_CURRENT_CHECK, getMeasuredBloodPumpCurrent() ); - } - } - else - { - bpCurrErrorDurationCtr = 0; - } - } + checkBloodPumpDirection(); + checkBloodPumpMCCurrent(); } // publish blood flow data on interval - if ( ++bloodFlowDataPublicationTimerCounter > getPublishBloodFlowDataInterval() ) - { - S32 flowStPt = (S32)getTargetBloodFlowRate(); - F32 measFlow = getMeasuredBloodFlowRate(); - F32 measSpd = getMeasuredBloodPumpSpeed(); - F32 measCurr = getMeasuredBloodPumpCurrent(); -#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)measSpd, (S32)measCurr, pwm ); - sendDebugData( (U08*)debugFlowStr, strlen(debugFlowStr) ); -#endif - broadcastBloodFlowData( flowStPt, measFlow, measSpd, measCurr ); - bloodFlowDataPublicationTimerCounter = 0; - } + publishBloodFlowData(); } /************************************************************************* @@ -498,7 +440,6 @@ bpFlowErrorSum += bpFlowError; iTerm = RANGE( bpFlowErrorSum, BP_MIN_ERROR_SUM, BP_MAX_ERROR_SUM ); iTerm *= BP_I_COEFFICIENT; - iTerm = RANGE( iTerm, BP_MIN_PWM_DC_DELTA, BP_MAX_PWM_DC_DELTA ); // 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 ); @@ -633,6 +574,114 @@ DATA_GET( F32, getMeasuredBloodPumpCurrent, adcBloodPumpCurrentmA ) /************************************************************************* + * @brief publishBloodFlowData + * The publishBloodFlowData function publishes blood flow data at the set \n + * interval. + * @details + * Inputs : target flow rate, measured flow rate, measured MC speed, \n + * measured MC current + * Outputs : Blood flow data is published to CAN bus. + * @param none + * @return none + *************************************************************************/ +static void publishBloodFlowData( void ) +{ + // publish blood flow data on interval + if ( ++bloodFlowDataPublicationTimerCounter > getPublishBloodFlowDataInterval() ) + { + S32 flowStPt = (S32)getTargetBloodFlowRate(); + F32 measFlow = getMeasuredBloodFlowRate(); + F32 measSpd = getMeasuredBloodPumpSpeed(); + F32 measCurr = getMeasuredBloodPumpCurrent(); +#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)measSpd, (S32)measCurr, pwm ); + sendDebugData( (U08*)debugFlowStr, strlen(debugFlowStr) ); +#endif + broadcastBloodFlowData( flowStPt, measFlow, measSpd, measCurr ); + bloodFlowDataPublicationTimerCounter = 0; + } +} + +/************************************************************************* + * @brief checkBloodPumpDirection + * The checkBloodPumpDirection function checks the set direction vs. \n + * the direction implied by the sign of the measured MC speed. + * @details + * Inputs : + * Outputs : + * @param none + * @return none + *************************************************************************/ +static void checkBloodPumpDirection( void ) +{ + MOTOR_DIR_T bpMCDir; + + if ( BLOOD_PUMP_CONTROL_TO_TARGET_STATE == bloodPumpState ) + { + // check set direction vs. direction from sign of motor controller speed + bpMCDir = ( getMeasuredBloodPumpSpeed() >= 0.0 ? MOTOR_DIR_FORWARD : MOTOR_DIR_REVERSE ); + if ( bloodPumpDirectionSet != bpMCDir ) + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_BLOOD_PUMP_MC_DIRECTION_CHECK, (U32)bloodPumpDirectionSet, (U32)bpMCDir ) + } + } +} + +/************************************************************************* + * @brief checkBloodPumpMCCurrent + * The checkBloodPumpMCCurrent function checks the measured MC current vs. \n + * the set state of the blood pump (stopped or running). + * @details + * Inputs : + * Outputs : + * @param none + * @return none + *************************************************************************/ +static void checkBloodPumpMCCurrent( void ) +{ + F32 bpCurr; + + // blood pump should be off + if ( BLOOD_PUMP_OFF_STATE == bloodPumpState ) + { + bpCurr = fabs( getMeasuredBloodPumpCurrent() ); + if ( bpCurr > BP_MAX_CURR_WHEN_STOPPED_MA ) + { + bpCurrErrorDurationCtr += TASK_PRIORITY_INTERVAL; + if ( bpCurrErrorDurationCtr > BP_MAX_CURR_ERROR_DURATION_MS ) + { + SET_ALARM_WITH_1_F32_DATA( ALARM_ID_BLOOD_PUMP_MC_CURRENT_CHECK, getMeasuredBloodPumpCurrent() ); + } + } + else + { + bpCurrErrorDurationCtr = 0; + } + } + // blood pump should be running + else + { + bpCurr = fabs( getMeasuredBloodPumpCurrent() ); + if ( ( bpCurr < BP_MIN_CURR_WHEN_RUNNING_MA ) || ( bpCurr > BP_MAX_CURR_WHEN_RUNNING_MA ) ) + { + bpCurrErrorDurationCtr += TASK_PRIORITY_INTERVAL; + if ( bpCurrErrorDurationCtr > BP_MAX_CURR_ERROR_DURATION_MS ) + { + SET_ALARM_WITH_1_F32_DATA( ALARM_ID_BLOOD_PUMP_MC_CURRENT_CHECK, getMeasuredBloodPumpCurrent() ); + } + } + else + { + bpCurrErrorDurationCtr = 0; + } + } +} + +/************************************************************************* * @brief execBloodFlowTest * The execBloodFlowTest function executes the state machine for the \n * BloodFlow self test.