Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -r9e2667ac9d2f1b2add9811f93477f0365f1b4eb7 -r97e24856b80f612b2556fb9a599e40659710822a --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 9e2667ac9d2f1b2add9811f93477f0365f1b4eb7) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 97e24856b80f612b2556fb9a599e40659710822a) @@ -53,7 +53,7 @@ #define MIN_BLOOD_PUMP_PWM_DUTY_CYCLE 0.12 ///< Controller will error if PWM duty cycle < 10%, so set min to 12% /// Interval (ms/task time) at which the blood pump is controlled. -#define BP_CONTROL_INTERVAL ( 10000 / TASK_GENERAL_INTERVAL ) +static const U32 BP_CONTROL_INTERVAL = ( 10000 / TASK_GENERAL_INTERVAL ); #define BP_P_COEFFICIENT 0.00035 ///< P term for blood pump control #define BP_I_COEFFICIENT 0.00035 ///< I term for blood pump control @@ -63,19 +63,24 @@ #define BP_SPEED_CALC_INTERVAL ( 40 / TASK_PRIORITY_INTERVAL ) /// Number of hall sensor counts kept in buffer to hold last 1 second of count data. #define BP_SPEED_CALC_BUFFER_LEN ( 1000 / BP_SPEED_CALC_INTERVAL / TASK_PRIORITY_INTERVAL ) -#define BP_HALL_EDGE_COUNTS_PER_REV 48 ///< Number of hall sensor edge counts per motor revolution. #define BP_MAX_ROTOR_SPEED_RPM 100.0 ///< Maximum rotor speed allowed for blood pump. #define BP_MAX_FLOW_VS_SPEED_DIFF_RPM 200.0 ///< Maximum difference between measured speed and speed implied by measured flow. #define BP_MAX_MOTOR_SPEED_WHILE_OFF_RPM 100.0 ///< Maximum motor speed (RPM) while motor is commanded off. #define BP_MAX_ROTOR_VS_MOTOR_DIFF_RPM 5.0 ///< Maximum difference in speed between motor and rotor (in rotor RPM). -#define BP_MAX_MOTOR_SPEED_ERROR_RPM 300.0 ///< Maximum difference in speed between measured and commanded RPM. -#define BP_FLOW_VS_SPEED_PERSIST ((5 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL) ///< Persist time (task intervals) for flow vs. motor speed error condition. -#define BP_OFF_ERROR_PERSIST ((5 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL) ///< Persist time (task intervals) for motor off error condition. -#define BP_MOTOR_SPEED_ERROR_PERSIST ((5 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL) ///< Persist time (task intervals) motor speed error condition. -#define BP_ROTOR_SPEED_ERROR_PERSIST ((12 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL) ///< Persist time (task intervals) rotor speed error condition. -#define BP_DIRECTION_ERROR_PERSIST (250 / TASK_PRIORITY_INTERVAL) ///< Persist time (task intervals) pump direction error condition. -#define BP_MAX_ROTOR_SPEED_ERROR_PERSIST ((1 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL) ///< Persist time (task intervals) blood pump rotor speed too fast error condition. +#define BP_MAX_MOTOR_SPEED_ERROR_RPM 300.0 ///< Maximum difference in speed between measured and commanded RPM. +/// Persist time (task intervals) for flow vs. motor speed error condition. +static const U32 BP_FLOW_VS_SPEED_PERSIST = ((5 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL); +/// Persist time (task intervals) for motor off error condition. +static const U32 BP_OFF_ERROR_PERSIST = ((5 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL); +/// Persist time (task intervals) motor speed error condition. +static const U32 BP_MOTOR_SPEED_ERROR_PERSIST = ((5 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL); +/// Persist time (task intervals) rotor speed error condition. +static const U32 BP_ROTOR_SPEED_ERROR_PERSIST = ((12 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL); +/// Persist time (task intervals) pump direction error condition. +static const U32 BP_DIRECTION_ERROR_PERSIST = (250 / TASK_PRIORITY_INTERVAL); +/// Persist time (task intervals) blood pump rotor speed too fast error condition. +static const U32 BP_MAX_ROTOR_SPEED_ERROR_PERSIST = ((1 * MS_PER_SECOND) / TASK_PRIORITY_INTERVAL); #define BP_MAX_CURR_WHEN_STOPPED_MA 150.0 ///< Motor controller current should not exceed this when pump should be stopped #define BP_MAX_CURR_WHEN_RUNNING_MA 2000.0 ///< Motor controller current should not exceed this when pump should be running @@ -382,7 +387,7 @@ *************************************************************************/ U32 getBloodPumpMotorCount( void ) { - return bloodPumpMotorEdgeCount / BP_HALL_EDGE_COUNTS_PER_REV; + return bloodPumpMotorEdgeCount; } /*********************************************************************//** @@ -931,25 +936,32 @@ if ( ++bpMotorSpeedCalcTimerCtr >= BP_SPEED_CALC_INTERVAL ) { U16 bpMotorHallSensorCount = getFPGABloodPumpHallSensorCount(); + U16 last = bpLastMotorHallSensorCounts[ bpMotorSpeedCalcIdx ]; U32 nextIdx = INC_WRAP( bpMotorSpeedCalcIdx, 0, BP_SPEED_CALC_BUFFER_LEN - 1 ); U16 incDelta = ( bpMotorHallSensorCount >= bpLastMotorHallSensorCounts[ nextIdx ] ? \ bpMotorHallSensorCount - bpLastMotorHallSensorCounts[ nextIdx ] : \ ( HEX_64_K - bpLastMotorHallSensorCounts[ nextIdx ] ) + bpMotorHallSensorCount ); U16 decDelta = HEX_64_K - incDelta; - U16 delta; - + U16 spdDelta; + U16 delta; + // Determine blood pump speed/direction from delta hall sensor count since last interval if ( incDelta < decDelta ) { - delta = incDelta; - bloodPumpSpeedRPM.data = ( (F32)delta / (F32)BP_HALL_EDGE_COUNTS_PER_REV ) * (F32)SEC_PER_MIN; + spdDelta = incDelta; + bloodPumpSpeedRPM.data = ( (F32)spdDelta / (F32)BP_HALL_EDGE_COUNTS_PER_REV ) * (F32)SEC_PER_MIN; + // Keep a running 32-bit edge count used for safety check on volume in some functions + delta = ( bpMotorHallSensorCount >= last ? bpMotorHallSensorCount - last : ( HEX_64_K - last ) + bpMotorHallSensorCount ); + bloodPumpMotorEdgeCount += delta; } else { - delta = decDelta; - bloodPumpSpeedRPM.data = ( (F32)delta / (F32)BP_HALL_EDGE_COUNTS_PER_REV ) * (F32)SEC_PER_MIN * -1.0; + spdDelta = decDelta; + bloodPumpSpeedRPM.data = ( (F32)spdDelta / (F32)BP_HALL_EDGE_COUNTS_PER_REV ) * (F32)SEC_PER_MIN * -1.0; + // Keep a running 32-bit edge count used for safety check on volume in some functions + delta = ( bpMotorHallSensorCount <= last ? last - bpMotorHallSensorCount : ( HEX_64_K - bpMotorHallSensorCount ) + last ); + bloodPumpMotorEdgeCount -= (S32)delta; } - bloodPumpMotorEdgeCount += delta; // Update last count for next time bpLastMotorHallSensorCounts[ nextIdx ] = bpMotorHallSensorCount;