Index: firmware/App/Controllers/SyringePump.c =================================================================== diff -u -rfa0ab419491364c1d2a28caf216e20175181372d -r21180e0b65fddfa27f66d8fb59cb55b2db304230 --- firmware/App/Controllers/SyringePump.c (.../SyringePump.c) (revision fa0ab419491364c1d2a28caf216e20175181372d) +++ firmware/App/Controllers/SyringePump.c (.../SyringePump.c) (revision 21180e0b65fddfa27f66d8fb59cb55b2db304230) @@ -98,7 +98,7 @@ #define SYRINGE_PUMP_RETRACT_POS_MIN ( 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_RAMP_DIVISOR 11 ///< 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. @@ -143,6 +143,9 @@ #define SYRINGE_PUMP_ENCODER_DIRECTION_ERROR_BITS 0x3F ///< Syringe pump encoder direction error counter bits in FPGA register. #define SYRINGE_PUMP_ENCODER_DIRECTION_BIT 0x80 ///< Syringe pump encoder direction bit in FPGA register. +#define SYRINGE_PUMP_RAMP_STALL_TIME ( 500 / TASK_PRIORITY_INTERVAL ) ///< Syringe pump ramp stall timeout. +#define SYRINGE_PUMP_RAMP_STALL_RETRIES 3 ///< Syringe pump ramp stall retries allowed. + /// Defined states for the syringe pump control state machine. typedef enum SyringePump_States { @@ -206,6 +209,9 @@ static U08 lastSyringePumpADCReadCtr; ///< Remember last ADC read counter to check new reads are fresh. +static U32 syringePumpStallCtr; ///< Counts time when position is not changing during ramp. +static U32 syringePumpStallRetryCount; ///< Counts pump ramp up stall retries. + // ********** private function prototypes ********** static void resetSyringePumpRequestFlags( void ); @@ -273,6 +279,9 @@ lastSyringePumpADCReadCtr = 0; + syringePumpStallCtr = 0; + syringePumpStallRetryCount = 0; + // Initialize persistent alarms initPersistentAlarm( ALARM_ID_HD_SYRINGE_PUMP_ADC_ERROR, 0, SYRINGE_PUMP_ADC_READ_PERSISTENCE ); initPersistentAlarm( ALARM_ID_HD_SYRINGE_PUMP_ENCODER_DIRECTION_ERROR, 0, SYRINGE_PUMP_DIR_ALARM_PERSISTENCE ); @@ -491,6 +500,10 @@ { syringePumpSetRate = SYRINGE_PUMP_PRIME_RATE; syringePumpPrimeRequested = TRUE; + // Reset volume and start pos before starting prime operation + syringePumpVolumeDelivered.data = 0.0; + syringePumpSafetyVolumeDelivered = 0.0; + syringePumpVolumeStartPosition = getSyringePumpPosition(); } return syringePumpPrimeRequested; @@ -518,6 +531,10 @@ { syringePumpSetRate = tgtRate; syringePumpBolusRequested = TRUE; + // Reset volume and start pos before starting bolus operation + syringePumpVolumeDelivered.data = 0.0; + syringePumpSafetyVolumeDelivered = 0.0; + syringePumpVolumeStartPosition = getSyringePumpPosition(); } } else @@ -727,10 +744,10 @@ /*********************************************************************//** * @brief - * The getSyringePumpForceV function gets the current syringe pump force. - * @details Inputs: syringePumpMeasForce + * The getSyringePumpStatus function gets the current syringe pump status. + * @details Inputs: syringePumpStatus * @details Outputs: none - * @return the current syringe pump force (in V). + * @return the current syringe pump status. *************************************************************************/ static U08 getSyringePumpStatus() { @@ -746,10 +763,11 @@ /*********************************************************************//** * @brief - * The getSyringePumpForceV function gets the current syringe pump force. - * @details Inputs: syringePumpMeasForce + * The getSyringePumpEncoderStatus function gets the current syringe pump + * encoder status. + * @details Inputs: syringePumpEncoderStatus * @details Outputs: none - * @return the current syringe pump force (in V). + * @return the current syringe pump encoder status. *************************************************************************/ static U08 getSyringePumpEncoderStatus() { @@ -765,10 +783,11 @@ /*********************************************************************//** * @brief - * The getSyringePumpForceV function gets the current syringe pump force. - * @details Inputs: syringePumpMeasForce + * The getSyringePumpADCReadCounter function gets the current syringe pump + * ADC read counter. + * @details Inputs: syringePumpADCReadCtr * @details Outputs: none - * @return the current syringe pump force (in V). + * @return the current syringe pump ADC read counter. *************************************************************************/ static U08 getSyringePumpADCReadCounter() { @@ -1601,11 +1620,37 @@ *************************************************************************/ static void rampSyringePump( void ) { + // Check for ramp stall - restart ramp if retry allowed + if ( syringePumpLastPosition == getSyringePumpPosition() ) + { + syringePumpStallCtr++; + if ( syringePumpStallCtr >= SYRINGE_PUMP_RAMP_STALL_TIME ) + { + if ( ++syringePumpStallRetryCount <= SYRINGE_PUMP_RAMP_STALL_RETRIES ) + { + // Restart ramp + syringePumpRampTimerCtr = 0; + syringePumpRampUpToggleTime = SYRINGE_PUMP_START_RAMP_SPEED; + } + else + { + // TODO - stall alarm + stopSyringePump(); + syringePumpState = SYRINGE_PUMP_OFF_STATE; + } + } + else + { + syringePumpStallCtr = 0; + } + } + + // Ramp up syringe pump toward target speed syringePumpRampTimerCtr++; if ( syringePumpRampUpToggleTime > syringePumpSetToggleTime ) { syringePumpRampUpToggleTime = (U32)((F32)SYRINGE_PUMP_START_RAMP_SPEED / - (F32)( syringePumpRampTimerCtr * syringePumpRampTimerCtr * syringePumpRampTimerCtr / SYRINGE_PUMP_RAMP_DIVISOR )); + (F32)( ( syringePumpRampTimerCtr * syringePumpRampTimerCtr * syringePumpRampTimerCtr ) / SYRINGE_PUMP_RAMP_DIVISOR ) ); if ( syringePumpRampUpToggleTime > syringePumpSetToggleTime ) { setFPGASyringePumpStepToggleTime( syringePumpRampUpToggleTime ); @@ -1640,6 +1685,9 @@ syringePumpSetToggleTime = FLOAT_TO_INT_WITH_ROUND( conv ); // Set stepper ramp toggle time to initial ramp up speed initially syringePumpRampUpToggleTime = SYRINGE_PUMP_START_RAMP_SPEED; + // Reset ramp stall timer and retry counters + syringePumpStallCtr = 0; + syringePumpStallRetryCount = 0; } /*********************************************************************//**