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