Index: firmware/App/Controllers/SyringePump.c =================================================================== diff -u -rf25fbefbf6baa354b26dcbe17787821124b56c82 -r9798f57bc288270fe058fe098a76088cbb34d50c --- firmware/App/Controllers/SyringePump.c (.../SyringePump.c) (revision f25fbefbf6baa354b26dcbe17787821124b56c82) +++ firmware/App/Controllers/SyringePump.c (.../SyringePump.c) (revision 9798f57bc288270fe058fe098a76088cbb34d50c) @@ -63,7 +63,7 @@ #define SYRINGE_PUMP_RETRACT_RATE 10800.0 ///< Retract rate is 15 mL/ 5 s = 3 mL/s = 10,800 mL/hr. #define SYRINGE_PUMP_SEEK_RATE 3600.0 ///< Seek plunger rate is 5 mL/ 5 s = 1 mL/s = 3,600 mL/hr. #define SYRINGE_PUMP_PRIME_RATE 635.0 ///< Prime rate is 0.5 mm ^ 2 x PI x 450 mm = 0.353 mL / 2s = 635 mL/hr. -#define SYRINGE_PUMP_MAX_RATE 11000.0 ///< Maximum rate of the syringe pump (in mL). +#define SYRINGE_PUMP_MAX_RATE 11000.0 ///< Maximum rate of the syringe pump (in mL/hr). #define SYRINGE_PUMP_RATE_ALARM_PERSISTENCE ( 1 * MS_PER_SECOND ) ///< Alarm persistence period for syringe pump speed check alarms. @@ -80,9 +80,17 @@ #define SYRINGE_PUMP_HOME_DETECT_THRESHOLD_V 0.25 ///< Syringe pump home detected threshold (in V). #define SYRINGE_PUMP_PRIME_VOLUME_ML 0.353 ///< Target syringe prime volume (in mL). +/// Expected position of empty in relation to home postion. +#define SYRINGE_PUMP_EMPTY_POS ( SYRINGE_ENCODER_COUNTS_PER_ML * 11.0 ) +/// Margin of error for empty position determination. +#define SYRINGE_PUMP_EMPTY_POS_MARGIN ( SYRINGE_ENCODER_COUNTS_PER_ML * 0.5 ) + #define SYRINGE_PUMP_START_RAMP_SPEED 300000 ///< Starting speed for all syringe pump operations to ramp up from. #define SYRINGE_PUMP_RAMP_DIVISOR 5 ///< Used for ramping profile. +#define SYRINGE_PUMP_RATE_CHECK_MARGIN 0.05 ///< 5 pct margin on commanded vs. measured rate check. +#define SYRINGE_PUMP_VOLUME_CHECK_MARGIN 0.05 ///< 5 pct margin on commanded vs. encoder based volume check. + // Bit definitions for syringe pump control register #define SYRINGE_PUMP_CONTROL_SLEEP_OFF 0x40 ///< Syringe pump control register bit for sleep mode (active low). #define SYRINGE_PUMP_CONTROL_NOT_RESET 0x20 ///< Syringe pump control register bit for resetting stepper motor (active low). @@ -205,7 +213,7 @@ static BOOL checkOcclusionOrEmpty( BOOL stopPump ); static BOOL checkSyringeRemoved( BOOL stopPump ); static BOOL checkMaxTravel( BOOL stopPump, S32 maxPos ); -static BOOL checkMaxMeasRate( BOOL stopPump, F32 maxRate ); +static BOOL checkMaxMeasRate( BOOL stopPump, F32 pctMargin ); static BOOL checkVolumeVsSafetyVolume( BOOL stopPump, F32 pctMargin ); static void publishSyringePumpData( void ); @@ -317,22 +325,21 @@ { F32 tgtRate = 0.0; - // TODO - test code - remove later - setTreatmentParameterF32( TREATMENT_PARAM_HEPARIN_BOLUS_VOLUME, 2.0 ); - // Get set Heparin bolus volume (mL) and convert to target rate (mL/hr) tgtRate = getTreatmentParameterF32( TREATMENT_PARAM_HEPARIN_BOLUS_VOLUME ) / HEPARIN_BOLUS_TIME_HR; // If valid to start a bolus, kick it off - if ( ( tgtRate >= MIN_HEPARIN_BOLUS_RATE ) && ( tgtRate <= MAX_HEPARIN_BOLUS_RATE ) && - ( TRUE == isSyringeDetected() ) && ( FALSE == isSyringePumpHome() ) && ( SYRINGE_PUMP_OFF_STATE == syringePumpState ) ) + if ( ( tgtRate >= MIN_HEPARIN_BOLUS_RATE ) && ( tgtRate <= MAX_HEPARIN_BOLUS_RATE ) && ( FALSE == isSyringePumpHome() ) ) { - syringePumpSetRate = tgtRate; - syringePumpBolusRequested = TRUE; + if ( ( TRUE == isSyringeDetected() ) && ( SYRINGE_PUMP_OFF_STATE == syringePumpState ) ) + { + syringePumpSetRate = tgtRate; + syringePumpBolusRequested = TRUE; + } } else { - // TODO - s/w fault + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, (F32)SW_FAULT_ID_HD_SYRINGE_INVALID_BOLUS_CMD, tgtRate ) } return syringePumpBolusRequested; @@ -349,18 +356,17 @@ { F32 flowRate = getTreatmentParameterF32( TREATMENT_PARAM_HEPARIN_DISPENSE_RATE ); - if ( ( flowRate >= MIN_HEPARIN_CONTINUOUS_RATE ) && ( flowRate <= MAX_HEPARIN_CONTINUOUS_RATE ) && - ( TRUE == isSyringeDetected() ) && ( FALSE == isSyringePumpHome() ) ) + if ( ( flowRate >= MIN_HEPARIN_CONTINUOUS_RATE ) && ( flowRate <= MAX_HEPARIN_CONTINUOUS_RATE ) && ( FALSE == isSyringePumpHome() ) ) { - if ( SYRINGE_PUMP_OFF_STATE == syringePumpState ) + if ( ( TRUE == isSyringeDetected() ) && ( SYRINGE_PUMP_OFF_STATE == syringePumpState ) ) { syringePumpSetRate = flowRate; syringePumpContinuousRequested = TRUE; } } else { - // TODO - s/w fault + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, (F32)SW_FAULT_ID_HD_SYRINGE_INVALID_CONT_CMD, flowRate ) } return syringePumpContinuousRequested; @@ -386,7 +392,7 @@ } else { - // TODO - s/w fault + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, (F32)SW_FAULT_ID_HD_SYRINGE_INVALID_VREF, vRef ) } return syringePumpDACVrefSetRequested; @@ -621,9 +627,9 @@ if ( syringePumpDACVrefWriteInProgress != TRUE ) { // Check ADC read is fresh - if ( lastSyringePumpADCReadCtr == adcReadCtr ) + if ( lastSyringePumpADCReadCtr == adcReadCtr ) // TODO - add persistence { - // TODO - fault + activateAlarmNoData( ALARM_ID_HD_SYRINGE_PUMP_ADC_ERROR ); } lastSyringePumpADCReadCtr = adcReadCtr; } @@ -677,7 +683,7 @@ break; default: - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, 0, syringePumpState ) // TODO - replace 0 with appropriate enum + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_HD_SYRINGE_INVALID_STATE, syringePumpState ) break; } @@ -722,6 +728,8 @@ SYRINGE_PUMP_STATE_T result = SYRINGE_PUMP_OFF_STATE; // TODO - check position is not changing while stopped + // activateAlarmNoData( ALARM_ID_HD_SYRINGE_PUMP_NOT_STOPPED_ERROR ); + // activateSafetyShutdown(); // Check for request flags if ( TRUE == syringePumpRetractRequested ) @@ -803,8 +811,8 @@ else if ( TRUE == isSyringeDetected() ) { stopSyringePump(); + activateAlarmNoData( ALARM_ID_HD_SYRINGE_PUMP_SYRINGE_DETECTED_IN_RETRACT ); result = SYRINGE_PUMP_OFF_STATE; - // TODO - alarm } return result; @@ -884,6 +892,9 @@ // Check for occlusion stopPump = checkOcclusionOrEmpty( stopPump ); + // Check position > max travel + stopPump = checkMaxTravel( stopPump, syringePumpVolumeStartPosition * ( SYRINGE_PUMP_PRIME_VOLUME_ML * SYRINGE_ENCODER_COUNTS_PER_ML ) ); + // If anything found that would require stopping the pump, stop pump and go to off state if ( TRUE == stopPump ) { @@ -893,7 +904,6 @@ return result; } - /*********************************************************************//** * @brief * The handleSyringePumpBolusState function handles the bolus delivery state @@ -906,26 +916,32 @@ { SYRINGE_PUMP_STATE_T result = SYRINGE_PUMP_HEP_BOLUS_STATE; BOOL stopPump = FALSE; + F32 bolusVol = getTreatmentParameterF32( TREATMENT_PARAM_HEPARIN_BOLUS_VOLUME ); // Handle ramp up rampSyringePump(); // Is bolus completed? - if ( getSyringePumpVolumeDelivered() >= getTreatmentParameterF32( TREATMENT_PARAM_HEPARIN_BOLUS_VOLUME ) ) + if ( getSyringePumpVolumeDelivered() >= bolusVol ) { stopPump = TRUE; } // Has syringe been removed? stopPump = checkSyringeRemoved( stopPump ); - // TODO - check volume vs. safety volume (fault) + // Check volume vs. safety volume + stopPump = checkVolumeVsSafetyVolume( stopPump, SYRINGE_PUMP_VOLUME_CHECK_MARGIN ); // Check for occlusion stopPump = checkOcclusionOrEmpty( stopPump ); - // TODO - check for meas. speed > 1.05 mL/hr for TBD sec (fault) + // Check for commanded vs. meas. rate + stopPump = checkMaxMeasRate( stopPump, SYRINGE_PUMP_RATE_CHECK_MARGIN ); + // Check position > max travel + stopPump = checkMaxTravel( stopPump, syringePumpVolumeStartPosition * ( SYRINGE_PUMP_PRIME_VOLUME_ML * SYRINGE_ENCODER_COUNTS_PER_ML ) ); + // If anything found that would require stopping the pump, stop pump and go to off state if ( TRUE == stopPump ) { @@ -955,14 +971,17 @@ // Has syringe been removed? stopPump = checkSyringeRemoved( stopPump ); - // TODO - check volume vs. safety volume (fault) + // Check volume vs. safety volume + stopPump = checkVolumeVsSafetyVolume( stopPump, SYRINGE_PUMP_VOLUME_CHECK_MARGIN ); // Check for occlusion stopPump = checkOcclusionOrEmpty( stopPump ); - // TODO - check for meas. speed > 1.05 mL/hr for TBD sec (fault) + // Check for commanded vs. meas. rate + stopPump = checkMaxMeasRate( stopPump, SYRINGE_PUMP_RATE_CHECK_MARGIN ); - // TODO - check position > max travel (alarm for empty syringe) + // Check position > max travel + stopPump = checkMaxTravel( stopPump, SYRINGE_PUMP_EMPTY_POS + SYRINGE_PUMP_EMPTY_POS_MARGIN ); // If anything found that would require stopping the pump, stop pump and go to off state if ( TRUE == stopPump ) @@ -976,21 +995,57 @@ /*********************************************************************//** * @brief + * The handleSyringePumpCalibrateForceSensorState function handles the + * calibrate force sensor state of the syringe pump control state machine. + * of the pressure/occlusion monitor state machine. + * @details Inputs: DAC status + * @details Outputs: syringePumpDACVrefWriteInProgress, ADC read mode restored + * @return next state + *************************************************************************/ +static SYRINGE_PUMP_STATE_T handleSyringePumpCalibrateForceSensorState( void ) +{ + SYRINGE_PUMP_STATE_T result = SYRINGE_PUMP_CONFIG_FORCE_SENSOR_STATE; + + // Wait for DAC setting write to EEPROM to complete + if ( ( getFPGASyringePumpADCandDACStatus() & SYRINGE_PUMP_ADC_DAC_ERROR_COUNT_DAC_WR_DONE ) != 0 ) + { + syringePumpDACVrefWriteInProgress = FALSE; + // Switch back from DAC to ADC control + setFPGASyringePumpADCandDACControlFlags( SYRINGE_PUMP_ADC_DAC_CONTROL_RD_DAC_ON_ADC | SYRINGE_PUMP_ADC_DAC_CONTROL_ENABLE_ADC ); + // Back to off state + result = SYRINGE_PUMP_OFF_STATE; + } + + return result; +} + +/*********************************************************************//** + * @brief * The checkOcclusionOrEmpty function checks the force sensor for excessive * pressure. Would indicate occlusion or jam or empty syringe. * @details Inputs: TBD * @details Outputs: TBD * @param stopPump flag passed in by caller indicating whether pump should be stopped - * @return TRUE if occlusion/empty condition detected, FALSE if not + * @return TRUE if pump should be stopped, FALSE if not *************************************************************************/ static BOOL checkOcclusionOrEmpty( BOOL stopPump ) { BOOL result = stopPump; + F32 force = getSyringePumpForceV(); - if ( getSyringePumpForceV() >= SYRINGE_FORCE_OCCLUSION_THRESHOLD_V ) + if ( force >= SYRINGE_FORCE_OCCLUSION_THRESHOLD_V ) { + S32 pos = getSyringePumpPosition(); + + if ( fabs( pos - SYRINGE_PUMP_EMPTY_POS ) < SYRINGE_PUMP_EMPTY_POS_MARGIN ) + { + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_SYRINGE_PUMP_SYRINGE_EMPTY, (F32)pos, force ) + } + else + { + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_SYRINGE_PUMP_OCCLUSION, (F32)pos, force ) + } result = TRUE; - // TODO - alarm (alarm for occlusion or empty syringe depending on position) } return result; @@ -1004,7 +1059,7 @@ * @details Inputs: TBD * @details Outputs: TBD * @param stopPump flag passed in by caller indicating whether pump should be stopped - * @return TRUE if syringe removed, FALSE if not + * @return TRUE if pump should be stopped, FALSE if not *************************************************************************/ static BOOL checkSyringeRemoved( BOOL stopPump ) { @@ -1013,7 +1068,7 @@ if ( FALSE == isSyringeDetected() ) { result = TRUE; - // TODO - alarm + SET_ALARM_WITH_1_U32_DATA( ALARM_ID_HD_SYRINGE_PUMP_SYRINGE_REMOVED, (U32)syringePumpState ) } return result; @@ -1028,14 +1083,23 @@ * @details Outputs: TBD * @param stopPump flag passed in by caller indicating whether pump should be stopped * @param maxPos maximum position allowed in current state - * @return TRUE if max travel exceeded, FALSE if not + * @return TRUE if pump should be stopped, FALSE if not *************************************************************************/ static BOOL checkMaxTravel( BOOL stopPump, S32 maxPos ) { BOOL result = stopPump; + S32 pos = getSyringePumpPosition(); + if ( pos > ( SYRINGE_PUMP_EMPTY_POS + SYRINGE_PUMP_EMPTY_POS_MARGIN ) ) + { + result = TRUE; + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_SYRINGE_PUMP_SYRINGE_EMPTY, (F32)pos, getSyringePumpForceV() ) + } + else if ( pos > maxPos ) + { + result = TRUE; + } - return result; } @@ -1047,15 +1111,23 @@ * @details Inputs: TBD * @details Outputs: TBD * @param stopPump flag passed in by caller indicating whether pump should be stopped - * @param maxRate maximum measured rate allowed in current state - * @return TRUE if max rate exceeded, FALSE if not + * @param pctMargin percent tolerance allowed between set and measured rate + * @return TRUE if pump should be stopped, FALSE if not *************************************************************************/ -static BOOL checkMaxMeasRate( BOOL stopPump, F32 maxRate ) +static BOOL checkMaxMeasRate( BOOL stopPump, F32 pctMargin ) { BOOL result = stopPump; + F32 rate = getSyringePumpMeasRate(); + F32 delta = fabs( syringePumpSetRate - rate ); + F32 max = MAX( rate, syringePumpSetRate ); + F32 error = ( max > 0.0 ? ( 1.0 - ( delta / max ) ) : 0.0 ); + if ( error > pctMargin ) + { + result = TRUE; + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_SYRINGE_PUMP_SPEED_ERROR, syringePumpSetRate, rate ) + } - return result; } @@ -1069,38 +1141,20 @@ * @details Outputs: TBD * @param stopPump flag passed in by caller indicating whether pump should be stopped * @param pctMargin percent tolerance allowed between volume and safety volume - * @return TRUE if volume check fails, FALSE if not + * @return TRUE if pump should be stopped, FALSE if not *************************************************************************/ static BOOL checkVolumeVsSafetyVolume( BOOL stopPump, F32 pctMargin ) { BOOL result = stopPump; + F32 vol = getSyringePumpVolumeDelivered(); + F32 delta = fabs( vol - syringePumpSafetyVolumeDelivered ); + F32 max = MAX( vol, syringePumpSafetyVolumeDelivered ); + F32 error = ( fabs( max ) < NEARLY_ZERO ? 0.0 : ( 1.0 - ( delta / max ) ) ); - - - return result; -} - -/*********************************************************************//** - * @brief - * The handleSyringePumpCalibrateForceSensorState function handles the - * calibrate force sensor state of the syringe pump control state machine. - * of the pressure/occlusion monitor state machine. - * @details Inputs: DAC status - * @details Outputs: syringePumpDACVrefWriteInProgress, ADC read mode restored - * @return next state - *************************************************************************/ -static SYRINGE_PUMP_STATE_T handleSyringePumpCalibrateForceSensorState( void ) -{ - SYRINGE_PUMP_STATE_T result = SYRINGE_PUMP_CONFIG_FORCE_SENSOR_STATE; - - // Wait for DAC setting write to EEPROM to complete - if ( ( getFPGASyringePumpADCandDACStatus() & SYRINGE_PUMP_ADC_DAC_ERROR_COUNT_DAC_WR_DONE ) != 0 ) + if ( error > pctMargin ) { - syringePumpDACVrefWriteInProgress = FALSE; - // Switch back from DAC to ADC control - setFPGASyringePumpADCandDACControlFlags( SYRINGE_PUMP_ADC_DAC_CONTROL_RD_DAC_ON_ADC | SYRINGE_PUMP_ADC_DAC_CONTROL_ENABLE_ADC ); - // Back to off state - result = SYRINGE_PUMP_OFF_STATE; + result = TRUE; + SET_ALARM_WITH_2_F32_DATA( ALARM_ID_HD_SYRINGE_PUMP_VOLUME_ERROR, vol, syringePumpSafetyVolumeDelivered ) } return result;