Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -r9d4666bf3064df18a6d935125d7a69e4e8234e84 -rf9b8b75c3686be892799b5446b955fd36ab49fa3 --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 9d4666bf3064df18a6d935125d7a69e4e8234e84) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision f9b8b75c3686be892799b5446b955fd36ab49fa3) @@ -50,6 +50,8 @@ #define BP_HOME_RATE 50 ///< target pump speed (in estimate mL/min) for homing. #define BP_HOME_TIMEOUT_MS 10000 ///< maximum time allowed for homing to complete (in ms). +#define BP_SPEED_CALC_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) ///< interval (ms/task time) at which the blood pump speed is calculated. +#define BP_HALL_EDGE_COUNTS_PER_REV 6 ///< number of hall sensor edge counts per motor revolution. #define BP_MAX_CURR_WHEN_STOPPED_MA 150.0 ///< motor controller current should not exceed this when pump should be stopped #define BP_MIN_CURR_WHEN_RUNNING_MA 150.0 ///< motor controller current should always exceed this when pump should be running @@ -133,6 +135,10 @@ static BOOL bpStopAtHomePosition = FALSE; ///< stop blood pump at next home position static U32 bpHomeStartTime = 0; ///< when did blood pump home command begin? (in ms) +static U16 bpLastMotorHallSensorCount = 0; ///< last hall sensor count for the blood pump motor +static MOTOR_DIR_T bpMotorDirectionFromHallSensors = MOTOR_DIR_FORWARD; ///< pump direction according to hall sensor count +static U32 bpMotorSpeedCalcTimerCtr = 0; ///< counter determines interval for calculating blood pump motor speed from hall sensor count. + static F32 flowReadings[ SIZE_OF_ROLLING_AVG ]; ///< holds flow samples for a rolling average static U32 flowReadingsIdx = 0; ///< index for next sample in rolling average array static F32 flowReadingsTotal = 0.0; ///< rolling total - used to calc average @@ -157,6 +163,8 @@ static void publishBloodFlowData( void ); static void resetBloodFlowMovingAverage( void ); static void filterBloodFlowReadings( F32 flow ); +static void updateBloodPumpSpeedAndDirectionFromHallSensors( void ); +static void checkBloodPumpRotor( void ); static void checkBloodPumpDirection( void ); static void checkBloodPumpMCCurrent( void ); static DATA_GET_PROTOTYPE( U32, getPublishBloodFlowDataInterval ); @@ -337,21 +345,19 @@ filterBloodFlowReadings( bpFlow ); + // calculate blood pump motor speed/direction from hall sensor count + updateBloodPumpSpeedAndDirectionFromHallSensors(); + + // check for home position, zero/low speed + checkBloodPumpRotor(); + // don't start enforcing checks until out of init/POST mode if ( getCurrentOperationMode() != MODE_INIT ) { checkBloodPumpDirection(); checkBloodPumpMCCurrent(); } - // if homing, check timeout - if ( ( TRUE == bpStopAtHomePosition ) && ( TRUE == didTimeout( bpHomeStartTime, BP_HOME_TIMEOUT_MS ) ) ) - { - signalBloodPumpHardStop(); - bpStopAtHomePosition = FALSE; - // TODO - alarm??? - } - // publish blood flow data on interval publishBloodFlowData(); } @@ -893,6 +899,72 @@ /*********************************************************************//** * @brief + * The updateBloodPumpSpeedAndDirectionFromHallSensors function calculates \n + * the blood pump motor speed and direction from hall sensor counter on \n + * a 1 second interval. + * @details + * Inputs : bpLastMotorHallSensorCount, bpMotorSpeedCalcTimerCtr, current count from FPGA + * Outputs : bpMotorDirectionFromHallSensors, bloodPumpSpeedRPM + * @return none + *************************************************************************/ +static void updateBloodPumpSpeedAndDirectionFromHallSensors( void ) +{ + if ( ++bpMotorSpeedCalcTimerCtr >= BP_SPEED_CALC_INTERVAL ) + { + U16 bpMotorHallSensorCount = getFPGABloodPumpHallSensorCount(); + U16 incDelta = ( bpMotorHallSensorCount >= bpLastMotorHallSensorCount ? bpMotorHallSensorCount - bpLastMotorHallSensorCount : ( 0x10000 - bpLastMotorHallSensorCount ) + bpMotorHallSensorCount ); + U16 decDelta = 0x10000 - incDelta; + U16 delta; + + // determine blood pump speed/direction from delta hall sensor count since last interval + if ( incDelta < decDelta ) + { + bpMotorDirectionFromHallSensors = MOTOR_DIR_FORWARD; + delta = incDelta; + bloodPumpSpeedRPM.data = ( (F32)delta / (F32)BP_HALL_EDGE_COUNTS_PER_REV ) * (F32)SEC_PER_MIN; + } + else + { + bpMotorDirectionFromHallSensors = MOTOR_DIR_REVERSE; + delta = decDelta; + bloodPumpSpeedRPM.data = ( (F32)delta / (F32)BP_HALL_EDGE_COUNTS_PER_REV ) * (F32)SEC_PER_MIN * -1.0; + } + + // update last count for next time + bpLastMotorHallSensorCount = bpMotorHallSensorCount; + bpMotorSpeedCalcTimerCtr = 0; + } +} + +/************************************************************************* + * @brief + * The checkBloodPumpRotor function checks the rotor for the blood \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 : bpStopAtHomePosition, bpHomeStartTime, bpRotorRevStartTime + * Outputs : pump may be stopped if homing, bloodPumpRotorSpeedRPM may be set to zero. + * @return none + *************************************************************************/ +static void checkBloodPumpRotor( void ) +{ + // if homing, check timeout + if ( ( TRUE == bpStopAtHomePosition ) && ( TRUE == didTimeout( bpHomeStartTime, BP_HOME_TIMEOUT_MS ) ) ) + { + signalBloodPumpHardStop(); + bpStopAtHomePosition = FALSE; + // TODO - alarm??? + } + + // if pump is stopped or running very slowly, set rotor speed to zero + if ( TRUE == didTimeout( bpRotorRevStartTime, BP_HOME_TIMEOUT_MS ) ) + { + bloodPumpRotorSpeedRPM.data = 0.0; + } +} + +/*********************************************************************//** + * @brief * The checkBloodPumpDirection function checks the set direction vs. \n * the direction implied by the sign of the measured MC speed. * @details Index: firmware/App/Controllers/DialInFlow.c =================================================================== diff -u -r4ebc1f7e1aeb3a332e91fcdd1bbbe1a01873d93a -rf9b8b75c3686be892799b5446b955fd36ab49fa3 --- firmware/App/Controllers/DialInFlow.c (.../DialInFlow.c) (revision 4ebc1f7e1aeb3a332e91fcdd1bbbe1a01873d93a) +++ firmware/App/Controllers/DialInFlow.c (.../DialInFlow.c) (revision f9b8b75c3686be892799b5446b955fd36ab49fa3) @@ -51,6 +51,8 @@ #define DIP_HOME_RATE 50 ///< target pump speed (in estimate mL/min) for homing. #define DIP_HOME_TIMEOUT_MS 10000 ///< maximum time allowed for homing to complete (in ms). +#define DIP_SPEED_CALC_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) ///< interval (ms/task time) at which the dialysate inlet pump speed is calculated. +#define DIP_HALL_EDGE_COUNTS_PER_REV 6 ///< number of hall sensor edge counts per motor revolution. #define DIP_MAX_CURR_WHEN_STOPPED_MA 150.0 ///> motor controller current should not exceed this when pump should be stopped #define DIP_MIN_CURR_WHEN_RUNNING_MA 150.0 ///> motor controller current should always exceed this when pump should be running @@ -134,6 +136,10 @@ static BOOL dipStopAtHomePosition = FALSE; ///< stop dialysate inlet pump at next home position static U32 dipHomeStartTime = 0; ///< when did dialysate inlet pump home command begin? (in ms) +static U16 dipLastMotorHallSensorCount = 0; ///< last hall sensor count for the dialysate inlet pump motor +static MOTOR_DIR_T dipMotorDirectionFromHallSensors = MOTOR_DIR_FORWARD; ///< pump direction according to hall sensor count +static U32 dipMotorSpeedCalcTimerCtr = 0; ///< counter determines interval for calculating dialysate inlet pump motor speed from hall sensor count. + static F32 flowReadings[ SIZE_OF_ROLLING_AVG ]; ///< holds flow samples for a rolling average static U32 flowReadingsIdx = 0; ///< index for next sample in rolling average array static F32 flowReadingsTotal = 0.0; ///< rolling total - used to calc average @@ -158,6 +164,8 @@ static void publishDialInFlowData( void ); static void resetDialInFlowMovingAverage( void ); static void filterDialInFlowReadings( F32 flow ); +static void updateDialInPumpSpeedAndDirectionFromHallSensors( void ); +static void checkDialInPumpRotor( void ); static void checkDialInPumpDirection( void ); static void checkDialInPumpMCCurrent( void ); static DATA_GET_PROTOTYPE( U32, getPublishDialInFlowDataInterval ); @@ -338,21 +346,19 @@ filterDialInFlowReadings( dipFlow ); + // calculate dialysate inlet pump motor speed/direction from hall sensor count + updateDialInPumpSpeedAndDirectionFromHallSensors(); + + // check for home position, zero/low speed + checkDialInPumpRotor(); + // don't start enforcing checks until out of init/POST mode if ( getCurrentOperationMode() != MODE_INIT ) { checkDialInPumpDirection(); checkDialInPumpMCCurrent(); } - // if homing, check timeout - if ( ( TRUE == dipStopAtHomePosition ) && ( TRUE == didTimeout( dipHomeStartTime, DIP_HOME_TIMEOUT_MS ) ) ) - { - signalDialInPumpHardStop(); - dipStopAtHomePosition = FALSE; - // TODO - alarm??? - } - // publish dialIn flow data on interval publishDialInFlowData(); } @@ -814,6 +820,72 @@ /*********************************************************************//** * @brief + * The updateDialInPumpSpeedAndDirectionFromHallSensors function calculates \n + * the dialysate inlet pump motor speed and direction from hall sensor counter on \n + * a 1 second interval. + * @details + * Inputs : dipLastMotorHallSensorCount, dipMotorSpeedCalcTimerCtr, current count from FPGA + * Outputs : dipMotorDirectionFromHallSensors, dialInPumpSpeedRPM + * @return none + *************************************************************************/ +static void updateDialInPumpSpeedAndDirectionFromHallSensors( void ) +{ + if ( ++dipMotorSpeedCalcTimerCtr >= DIP_SPEED_CALC_INTERVAL ) + { + U16 dipMotorHallSensorCount = getFPGADialInPumpHallSensorCount(); + U16 incDelta = ( dipMotorHallSensorCount >= dipLastMotorHallSensorCount ? dipMotorHallSensorCount - dipLastMotorHallSensorCount : ( 0x10000 - dipLastMotorHallSensorCount ) + dipMotorHallSensorCount ); + U16 decDelta = 0x10000 - incDelta; + U16 delta; + + // determine dialysate inlet pump speed/direction from delta hall sensor count since last interval + if ( incDelta < decDelta ) + { + dipMotorDirectionFromHallSensors = MOTOR_DIR_FORWARD; + delta = incDelta; + dialInPumpSpeedRPM.data = ( (F32)delta / (F32)DIP_HALL_EDGE_COUNTS_PER_REV ) * (F32)SEC_PER_MIN; + } + else + { + dipMotorDirectionFromHallSensors = MOTOR_DIR_REVERSE; + delta = decDelta; + dialInPumpSpeedRPM.data = ( (F32)delta / (F32)DIP_HALL_EDGE_COUNTS_PER_REV ) * (F32)SEC_PER_MIN * -1.0; + } + + // update last count for next time + dipLastMotorHallSensorCount = dipMotorHallSensorCount; + dipMotorSpeedCalcTimerCtr = 0; + } +} + +/************************************************************************* + * @brief + * The checkDialInPumpRotor function checks the rotor for the dialysate inlet \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 : dipStopAtHomePosition, dipHomeStartTime, dipRotorRevStartTime + * Outputs : pump may be stopped if homing, dialInPumpRotorSpeedRPM may be set to zero. + * @return none + *************************************************************************/ +static void checkDialInPumpRotor( void ) +{ + // if homing, check timeout + if ( ( TRUE == dipStopAtHomePosition ) && ( TRUE == didTimeout( dipHomeStartTime, DIP_HOME_TIMEOUT_MS ) ) ) + { + signalDialInPumpHardStop(); + dipStopAtHomePosition = FALSE; + // TODO - alarm??? + } + + // if pump is stopped or running very slowly, set rotor speed to zero + if ( TRUE == didTimeout( dipRotorRevStartTime, DIP_HOME_TIMEOUT_MS ) ) + { + dialInPumpRotorSpeedRPM.data = 0.0; + } +} + +/*********************************************************************//** + * @brief * The checkDialInPumpDirection function checks the set direction vs. \n * the direction implied by the sign of the measured MC speed. * @details 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 Index: firmware/App/Services/PIControllers.c =================================================================== diff -u -r9d4666bf3064df18a6d935125d7a69e4e8234e84 -rf9b8b75c3686be892799b5446b955fd36ab49fa3 --- firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision 9d4666bf3064df18a6d935125d7a69e4e8234e84) +++ firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision f9b8b75c3686be892799b5446b955fd36ab49fa3) @@ -78,10 +78,10 @@ void initializePIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal, F32 kP, F32 kI, F32 controlMin, F32 controlMax ) { - PI_CONTROLLER_T *controller; - if ( controllerID < NUM_OF_PI_CONTROLLERS_IDS ) { + PI_CONTROLLER_T *controller; + SET_CONTROLLER( controller, controllerID ); controller->Kp = kP;