Index: firmware/App/Controllers/DialOutFlow.c =================================================================== diff -u -r4ebc1f7e1aeb3a332e91fcdd1bbbe1a01873d93a -rf9b8b75c3686be892799b5446b955fd36ab49fa3 --- firmware/App/Controllers/DialOutFlow.c (.../DialOutFlow.c) (revision 4ebc1f7e1aeb3a332e91fcdd1bbbe1a01873d93a) +++ firmware/App/Controllers/DialOutFlow.c (.../DialOutFlow.c) (revision f9b8b75c3686be892799b5446b955fd36ab49fa3) @@ -54,6 +54,8 @@ #define DOP_HOME_RATE 50 ///< target pump speed (in estimate mL/min) for homing. #define DOP_HOME_TIMEOUT_MS 10000 ///< maximum time allowed for homing to complete (in ms). +#define DOP_SPEED_CALC_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) ///< interval (ms/task time) at which the dialysate outlet pump speed is calculated. +#define DOP_HALL_EDGE_COUNTS_PER_REV 6 ///< number of hall sensor edge counts per motor revolution. #define DOP_MAX_CURR_WHEN_STOPPED_MA 150.0 ///< Motor controller current should not exceed this when pump should be stopped. #define DOP_MIN_CURR_WHEN_RUNNING_MA 150.0 ///< Motor controller current should always exceed this when pump should be running. @@ -136,6 +138,10 @@ static BOOL dopStopAtHomePosition = FALSE; ///< stop dialysate outlet pump at next home position static U32 dopHomeStartTime = 0; ///< when did dialysate outlet pump home command begin? (in ms) +static U16 dopLastMotorHallSensorCount = 0; ///< last hall sensor count for the dialysate outlet pump motor +static MOTOR_DIR_T dopMotorDirectionFromHallSensors = MOTOR_DIR_FORWARD; ///< pump direction according to hall sensor count +static U32 dopMotorSpeedCalcTimerCtr = 0; ///< counter determines interval for calculating dialysate outlet pump motor speed from hall sensor count. + static DIAL_OUT_PUMP_SELF_TEST_STATE_T dialOutPumpSelfTestState = DIAL_OUT_PUMP_SELF_TEST_STATE_START; ///< Current state of the dialysate outlet pump self test state machine. static U32 dialOutPumpSelfTestTimerCount = 0; ///< Timer counter for time reference during self test. @@ -156,6 +162,8 @@ static void publishDialOutFlowData( void ); +static void updateDialOutPumpSpeedAndDirectionFromHallSensors( void ); +static void checkDialOutPumpRotor( void ); static void checkDialOutPumpDirection( void ); static void checkDialOutPumpMCCurrent( void ); @@ -392,21 +400,19 @@ dialOutPumpMCSpeedRPM.data = (F32)(SIGN_FROM_12_BIT_VALUE(bpRPM)) * DOP_SPEED_ADC_TO_RPM_FACTOR; dialOutPumpMCCurrentmA.data = (F32)(SIGN_FROM_12_BIT_VALUE(bpmA)) * DOP_CURRENT_ADC_TO_MA_FACTOR; + // calculate dialysate outlet pump motor speed/direction from hall sensor count + updateDialOutPumpSpeedAndDirectionFromHallSensors(); + + // check for home position, zero/low speed + checkDialOutPumpRotor(); + // don't start enforcing checks until out of init/POST mode if ( getCurrentOperationMode() != MODE_INIT ) { checkDialOutPumpDirection(); checkDialOutPumpMCCurrent(); } - // if homing, check timeout - if ( ( TRUE == dopStopAtHomePosition ) && ( TRUE == didTimeout( dopHomeStartTime, DOP_HOME_TIMEOUT_MS ) ) ) - { - signalDialOutPumpHardStop(); - dopStopAtHomePosition = FALSE; - // TODO - alarm??? - } - publishDialOutFlowData(); } @@ -711,8 +717,74 @@ } } +/*********************************************************************//** + * @brief + * The updateDialOutPumpSpeedAndDirectionFromHallSensors function calculates \n + * the dialysate outlet pump motor speed and direction from hall sensor counter on \n + * a 1 second interval. + * @details + * Inputs : dopLastMotorHallSensorCount, dopMotorSpeedCalcTimerCtr, current count from FPGA + * Outputs : dopMotorDirectionFromHallSensors, dialOutPumpSpeedRPM + * @return none + *************************************************************************/ +static void updateDialOutPumpSpeedAndDirectionFromHallSensors( void ) +{ + if ( ++dopMotorSpeedCalcTimerCtr >= DOP_SPEED_CALC_INTERVAL ) + { + U16 dopMotorHallSensorCount = getFPGADialOutPumpHallSensorCount(); + U16 incDelta = ( dopMotorHallSensorCount >= dopLastMotorHallSensorCount ? dopMotorHallSensorCount - dopLastMotorHallSensorCount : ( 0x10000 - dopLastMotorHallSensorCount ) + dopMotorHallSensorCount ); + U16 decDelta = 0x10000 - incDelta; + U16 delta; + + // determine dialysate outlet pump speed/direction from delta hall sensor count since last interval + if ( incDelta < decDelta ) + { + dopMotorDirectionFromHallSensors = MOTOR_DIR_FORWARD; + delta = incDelta; + dialOutPumpSpeedRPM.data = ( (F32)delta / (F32)DOP_HALL_EDGE_COUNTS_PER_REV ) * (F32)SEC_PER_MIN; + } + else + { + dopMotorDirectionFromHallSensors = MOTOR_DIR_REVERSE; + delta = decDelta; + dialOutPumpSpeedRPM.data = ( (F32)delta / (F32)DOP_HALL_EDGE_COUNTS_PER_REV ) * (F32)SEC_PER_MIN * -1.0; + } + + // update last count for next time + dopLastMotorHallSensorCount = dopMotorHallSensorCount; + dopMotorSpeedCalcTimerCtr = 0; + } +} + /************************************************************************* * @brief + * The checkDialOutPumpRotor function checks the rotor for the dialysate outlet \n + * pump. If homing, this function will stop when hall sensor detected. If pump \n + * is off or running very slowly, rotor speed will be set to zero. + * @details + * Inputs : dopStopAtHomePosition, dopHomeStartTime, dopRotorRevStartTime + * Outputs : pump may be stopped if homing, dialOutPumpRotorSpeedRPM may be set to zero. + * @return none + *************************************************************************/ +static void checkDialOutPumpRotor( void ) +{ + // if homing, check timeout + if ( ( TRUE == dopStopAtHomePosition ) && ( TRUE == didTimeout( dopHomeStartTime, DOP_HOME_TIMEOUT_MS ) ) ) + { + signalDialOutPumpHardStop(); + dopStopAtHomePosition = FALSE; + // TODO - alarm??? + } + + // if pump is stopped or running very slowly, set rotor speed to zero + if ( TRUE == didTimeout( dopRotorRevStartTime, DOP_HOME_TIMEOUT_MS ) ) + { + dialOutPumpRotorSpeedRPM.data = 0.0; + } +} + +/************************************************************************* + * @brief * The checkDialOutPumpDirection function checks the set direction vs. \n * the direction implied by the sign of the measured MC speed. * @details