Index: firmware/App/Controllers/ConcentratePumps.c =================================================================== diff -u -re0cdf49eb0f54239e5d765282e0952cea7ded1bd -r867521a9810f2218c34d96164fab614494a63f8d --- firmware/App/Controllers/ConcentratePumps.c (.../ConcentratePumps.c) (revision e0cdf49eb0f54239e5d765282e0952cea7ded1bd) +++ firmware/App/Controllers/ConcentratePumps.c (.../ConcentratePumps.c) (revision 867521a9810f2218c34d96164fab614494a63f8d) @@ -45,17 +45,17 @@ #define CONCENTRATE_PUMP_PULSE_PER_REV 2.0F ///< Number of pulses generate for every revolution. #define CONCENTRATE_PUMP_STEP_PER_REV 200.0F ///< Number of steps for every revolution. #define CONCENTRATE_PUMP_HALL_SENSE_PERIOD_RESOLUTION 100.0F ///< Hall sense period resolution in microseconds. -#define CONCENTRATE_PUMP_MIN_ALLOWED_HALL_SENSOR_COUNT 1000.0F ///< Hall sensors minimum allowed value. -#define CONCENTRATE_PUMP_HALL_SENSORS_OUT_OF_RANGE_TIME_MS 5000 ///< Hall sensors out of range time in milliseconds. +#define CONCENTRATE_PUMP_MIN_ALLOWED_HALL_SENSOR_COUNT 1000 ///< Hall sensors minimum allowed value. +#define CONCENTRATE_PUMP_HALL_SENSORS_OUT_OF_RANGE_TIME_MS ( 5 * MS_PER_SECOND ) ///< Hall sensors out of range time in milliseconds. -#define CONCENTRATE_PUMP_MICRO_STEPS_PER_STEP 8.0 ///< Number of micro-steps ( fractions of step) per step. +#define CONCENTRATE_PUMP_MICRO_STEPS_PER_STEP 8.0F ///< Number of micro-steps ( fractions of step) per step. #define CONCENTRATE_PUMP_STEP_PERIOD_RESOLUTION ( 0.5F / ( US_PER_SECOND * SEC_PER_MIN ) ) ///< Convert step period resolution (0.5 us) to minute. /// Volume output per pulse. #define CONCENTRATE_PUMP_VOLUME_PER_PULSE ( CONCENTRATE_PUMP_VOLUME_PER_REV / CONCENTRATE_PUMP_PULSE_PER_REV ) #define CONCENTRATE_PUMP_DATA_PUBLISH_INTERVAL ( 500 / TASK_GENERAL_INTERVAL ) ///< Interval (ms/task time) at which the concentrate pump is monitored. -#define CONCENTRATE_PUMP_CONTROL_INTERVAL ( 50 / TASK_GENERAL_INTERVAL ) ///< Interval (ms/task time) at which the concentrate pump is controlled. +#define CONCENTRATE_PUMP_CONTROL_INTERVAL ( 1 * MS_PER_SECOND / TASK_GENERAL_INTERVAL ) ///< Interval (ms/task time) at which the concentrate pump is controlled. #define CONCENTRATE_PUMP_SPEED_CONTROL_PERSISTENCE_PERIOD ( 5 * MS_PER_SECOND ) ///< Persistence period for concentrate pump speed control error. #define NUMBER_OF_ACID_AND_BICARB_NV_DATA_TO_CHECK 1 ///< Number of acid and bicarb non-volatile data to check. @@ -65,26 +65,27 @@ /// Enumeration of concentrate pump states. typedef enum ConcentratePumpState { - CONCENTRATE_PUMP_OFF_STATE = 0, ///< Concentrate pump off state - CONCENTRATE_PUMP_ON_STATE, ///< Concentrate pump on state - NUM_OF_CONCENTRATE_PUMP_STATES ///< Number of concentrate pump states + CONCENTRATE_PUMP_OFF_STATE = 0, ///< Concentrate pump off state. + CONCENTRATE_PUMP_RAMP_TO_TARGET_SPEED_STATE, ///< Concentrate pump ramp to target state. + CONCENTRATE_PUMP_CONTROL_TARGET_SPEED_STATE, ///< Concentrate pump on state. + NUM_OF_CONCENTRATE_PUMP_STATES ///< Number of concentrate pump states. } CONCENTRATE_PUMP_STATE_T; /// Concentrate pump data structure typedef struct { - U32 controlTimerCounter; ///< Timer counter to perform control on concentrate pump - CONCENTRATE_PUMP_STATE_T execState; ///< Concentrate pump execute current state - BOOL hasTurnOnPumpsBeenRequested; ///< Flag indicates a request to turn concentrate pumps on + U32 controlTimerCounter; ///< Timer counter to perform control on concentrate pump. + CONCENTRATE_PUMP_STATE_T execState; ///< Concentrate pump execute current state. + BOOL hasTurnOnPumpsBeenRequested; ///< Flag indicates a request to turn concentrate pumps on. - F32 pumpTargetSpeed; ///< Target concentrate pumps' speed (mL/min) - F32 currentPumpSpeed; ///< Current controlled concentrate pumps' speed (mL/min) - OVERRIDE_F32_T measuredPumpSpeed; ///< Measured concentrate pump speed (mL/min) - U16 togglePeriodCount; ///< Converted pump speed (mL/min) to toggle period counts (0.5 uS increment counts per step) + F32 pumpTargetSpeed; ///< Target concentrate pumps' speed (mL/min). + F32 currentPumpSpeed; ///< Current controlled concentrate pumps' speed (mL/min). + OVERRIDE_F32_T measuredPumpSpeed; ///< Measured concentrate pump speed (mL/min). + U16 togglePeriodCount; ///< Converted pump speed (mL/min) to toggle period counts (0.5 uS increment counts per step). - U08 direction; ///< Concentrate pump motor direction - void ( *control )( U08 ); ///< Concentrate pump FPGA control function pointer - void ( *setStepSpeed )( U16 ); ///< Concentrate pump FPGA set step speed function pointer + U08 direction; ///< Concentrate pump motor direction. + void ( *control )( U08 ); ///< Concentrate pump FPGA control function pointer. + void ( *setStepSpeed )( U16 ); ///< Concentrate pump FPGA set step speed function pointer. } CONCENTRATE_PUMP_T; // ********** private data ********** @@ -103,10 +104,10 @@ static void stopConcentratePump( CONCENTRATE_PUMPS_T pumpId ); static CONCENTRATE_PUMP_STATE_T handleConcentratePumpOffState( CONCENTRATE_PUMPS_T pumpId ); +static CONCENTRATE_PUMP_STATE_T handleConcentratePumpRampToTargetSpeedState( CONCENTRATE_PUMPS_T pumpId ); +static CONCENTRATE_PUMP_STATE_T handleConcentratePumpControlTargetSpeedState( CONCENTRATE_PUMPS_T pumpId ); -static void stepConcentratePumpToTargetSpeed( CONCENTRATE_PUMPS_T pumpId ); -static CONCENTRATE_PUMP_STATE_T handleConcentratePumpOnState( CONCENTRATE_PUMPS_T pumpId ); - +static BOOL stepConcentratePumpToTargetSpeed( CONCENTRATE_PUMPS_T pumpId ); static void calcMeasuredPumpsSpeed( CONCENTRATE_PUMPS_T pumpId, U16 pulseWidthCount ); /*********************************************************************//** @@ -130,11 +131,11 @@ for ( pumpId = CONCENTRATEPUMPS_CP1_ACID; pumpId < NUM_OF_CONCENTRATE_PUMPS; ++pumpId ) { - concentratePumps[ pumpId ].controlTimerCounter = 0; - concentratePumps[ pumpId ].execState = CONCENTRATE_PUMP_OFF_STATE; + concentratePumps[ pumpId ].controlTimerCounter = 0; + concentratePumps[ pumpId ].execState = CONCENTRATE_PUMP_OFF_STATE; concentratePumps[ pumpId ].hasTurnOnPumpsBeenRequested = FALSE; - concentratePumps[ pumpId ].pumpTargetSpeed = 0.0; - concentratePumps[ pumpId ].direction = CONCENTRATE_PUMP_FORWARD_DIR; + concentratePumps[ pumpId ].pumpTargetSpeed = 0.0; + concentratePumps[ pumpId ].direction = CONCENTRATE_PUMP_FORWARD_DIR; stopConcentratePump( pumpId ); } @@ -167,15 +168,20 @@ if ( ++concentratePumpMonitorTimerCounter >= getU32OverrideValue( &concentratePumpDataPublishInterval ) ) { CONCENTRATE_PUMP_DATA_T data; - U08 const fpgaConcentratePumpsFault = getFPGAConcentratePumpsFault(); + U08 fpgaConcentratePumpsFault = getFPGAConcentratePumpsFault(); + if ( 0 != fpgaConcentratePumpsFault ) { SET_ALARM_WITH_1_U32_DATA( ALARM_ID_DG_CONCENTRATE_PUMP_FAULT, fpgaConcentratePumpsFault ); } - calcMeasuredPumpsSpeed( CONCENTRATEPUMPS_CP1_ACID, getFPGACP1HallSensePulseWidth() ); - calcMeasuredPumpsSpeed( CONCENTRATEPUMPS_CP2_BICARB, getFPGACP2HallSensePulseWidth() ); + if ( ( concentratePumps[ CONCENTRATEPUMPS_CP1_ACID ].execState != CONCENTRATE_PUMP_OFF_STATE ) && + ( concentratePumps[ CONCENTRATEPUMPS_CP2_BICARB ].execState != CONCENTRATE_PUMP_OFF_STATE ) ) + { + calcMeasuredPumpsSpeed( CONCENTRATEPUMPS_CP1_ACID, getFPGACP1HallSensePulseWidth() ); + calcMeasuredPumpsSpeed( CONCENTRATEPUMPS_CP2_BICARB, getFPGACP2HallSensePulseWidth() ); + } // Get CP1 specifications U08 cp1Direction = concentratePumps[ CONCENTRATEPUMPS_CP1_ACID ].direction; @@ -191,10 +197,12 @@ data.cp1MeasuredSpeed = ( CONCENTRATE_PUMP_REVERSE_DIR == cp1Direction ? cp1Speed * -1.0 : cp1Speed ); data.cp2CurrentSetSpeed = ( CONCENTRATE_PUMP_REVERSE_DIR == cp2Direction ? cp2SetSpeed * -1.0 : cp2SetSpeed ); data.cp2MeasuredSpeed = ( CONCENTRATE_PUMP_REVERSE_DIR == cp2Direction ? cp2Speed * -1.0 : cp2Speed ); + data.cp1State = concentratePumps[ CONCENTRATEPUMPS_CP1_ACID ].execState; + data.cp2State = concentratePumps[ CONCENTRATEPUMPS_CP2_BICARB ].execState; #ifndef DISABLE_DIALYSATE_CHECK - F32 const cp1Error = fabs( getMeasuredPumpSpeed( CONCENTRATEPUMPS_CP1_ACID ) - concentratePumps[ CONCENTRATEPUMPS_CP1_ACID ].currentPumpSpeed ) / concentratePumps[ CONCENTRATEPUMPS_CP1_ACID ].currentPumpSpeed; - F32 const cp2Error = fabs( getMeasuredPumpSpeed( CONCENTRATEPUMPS_CP2_BICARB ) - concentratePumps[ CONCENTRATEPUMPS_CP2_BICARB ].currentPumpSpeed ) / concentratePumps[ CONCENTRATEPUMPS_CP2_BICARB ].currentPumpSpeed; + F32 cp1Error = fabs( getMeasuredPumpSpeed( CONCENTRATEPUMPS_CP1_ACID ) - concentratePumps[ CONCENTRATEPUMPS_CP1_ACID ].currentPumpSpeed ) / concentratePumps[ CONCENTRATEPUMPS_CP1_ACID ].currentPumpSpeed; + F32 cp2Error = fabs( getMeasuredPumpSpeed( CONCENTRATEPUMPS_CP2_BICARB ) - concentratePumps[ CONCENTRATEPUMPS_CP2_BICARB ].currentPumpSpeed ) / concentratePumps[ CONCENTRATEPUMPS_CP2_BICARB ].currentPumpSpeed; checkPersistentAlarm( ALARM_ID_CP1_SPEED_CONTROL_ERROR, cp1Error > CONCENTRATE_PUMP_ERROR_TOLERANCE, cp1Error, CONCENTRATE_PUMP_ERROR_TOLERANCE ); checkPersistentAlarm( ALARM_ID_CP2_SPEED_CONTROL_ERROR, cp2Error > CONCENTRATE_PUMP_ERROR_TOLERANCE, cp2Error, CONCENTRATE_PUMP_ERROR_TOLERANCE ); @@ -224,10 +232,14 @@ concentratePumps[ pumpId ].execState = handleConcentratePumpOffState( pumpId ); break; - case CONCENTRATE_PUMP_ON_STATE: - concentratePumps[ pumpId ].execState = handleConcentratePumpOnState( pumpId ); + case CONCENTRATE_PUMP_RAMP_TO_TARGET_SPEED_STATE: + concentratePumps[ pumpId ].execState = handleConcentratePumpRampToTargetSpeedState( pumpId ); break; + case CONCENTRATE_PUMP_CONTROL_TARGET_SPEED_STATE: + concentratePumps[ pumpId ].execState = handleConcentratePumpControlTargetSpeedState( pumpId ); + break; + default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_CONCENTRATE_PUMP_EXEC_INVALID_STATE, pumpId ) concentratePumps[ pumpId ].execState = CONCENTRATE_PUMP_OFF_STATE; @@ -349,9 +361,9 @@ { concentratePumps[ pumpId ].pumpTargetSpeed = 0.0; } - else + else if ( targetSpeed_ml_min > CONCENTRATE_PUMP_MAX_SPEED ) { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_CONCENTRATE_PUMP_SPEED_OUT_OF_RANGE, targetSpeed_ml_min ) + concentratePumps[ pumpId ].pumpTargetSpeed = CONCENTRATE_PUMP_MAX_SPEED; } } else @@ -427,7 +439,7 @@ *************************************************************************/ static void stopConcentratePump( CONCENTRATE_PUMPS_T pumpId ) { - concentratePumps[ pumpId ].currentPumpSpeed = 0.0; + concentratePumps[ pumpId ].currentPumpSpeed = 0.0; concentratePumps[ pumpId ].measuredPumpSpeed.data = 0.0; concentratePumps[ pumpId ].control( CONCENTRATE_PUMP_OFF_CONTROL ); @@ -449,29 +461,85 @@ if ( TRUE == concentratePumps[ pumpId ].hasTurnOnPumpsBeenRequested ) { - U08 const controlValue = ( CONCENTRATE_PUMP_ON_CONTROL | concentratePumps[ pumpId ].direction ); + U08 controlValue = ( CONCENTRATE_PUMP_ON_CONTROL | concentratePumps[ pumpId ].direction ); concentratePumps[ pumpId ].control( controlValue ); - state = CONCENTRATE_PUMP_ON_STATE; + state = CONCENTRATE_PUMP_RAMP_TO_TARGET_SPEED_STATE; } return state; } /*********************************************************************//** * @brief + * The handleConcentratePumpRampToTargetSpeedState function executes the + * concentrate pump ramp up to target speed state. Once the speed is close + * to target the state is transitioned to control target state. + * @details Inputs: none + * @details Outputs: none + * @param pumpId concentrate pump id + * @return next state of the state machine + *************************************************************************/ +static CONCENTRATE_PUMP_STATE_T handleConcentratePumpRampToTargetSpeedState( CONCENTRATE_PUMPS_T pumpId ) +{ + CONCENTRATE_PUMP_STATE_T state = CONCENTRATE_PUMP_RAMP_TO_TARGET_SPEED_STATE; + + if ( TRUE == stepConcentratePumpToTargetSpeed( pumpId ) ) + { + state = CONCENTRATE_PUMP_CONTROL_TARGET_SPEED_STATE; + } + + return state; +} + +/*********************************************************************//** + * @brief + * The handleConcentratePumpControlTargetSpeedState function turns off concentrate + * pumps switch to off state upon request. While in on state, the function + * controls concentrate pumps to a target step speed. + * @details Inputs: currentPumpSpeed[] + * @details Outputs: control concentrate pumps to target step speed + * @param pumpId concentrate pump id + * @return state + *************************************************************************/ +static CONCENTRATE_PUMP_STATE_T handleConcentratePumpControlTargetSpeedState( CONCENTRATE_PUMPS_T pumpId ) +{ + CONCENTRATE_PUMP_STATE_T state = CONCENTRATE_PUMP_CONTROL_TARGET_SPEED_STATE; + + if ( ++concentratePumps[ pumpId ].controlTimerCounter >= CONCENTRATE_PUMP_CONTROL_INTERVAL ) + { + concentratePumps[ pumpId ].controlTimerCounter = 0; + + stepConcentratePumpToTargetSpeed( pumpId ); + + concentratePumps[ pumpId ].setStepSpeed( concentratePumps[ pumpId ].togglePeriodCount ); + } + + if ( FALSE == concentratePumps[ pumpId ].hasTurnOnPumpsBeenRequested ) + { + state = CONCENTRATE_PUMP_OFF_STATE; + stopConcentratePump( pumpId ); + } + + return state; +} + +/*********************************************************************//** + * @brief * The stepConcentratePumpToTargetSpeed function steps current step speed * toward target speed with predefined step increase. * @details Inputs: none * @details Outputs: currentPumpSpeed[] * @param pumpId concentrate pump id to increase current step speed - * @return none + * @return TRUE if the pump has reached to target otherwise, FALSE *************************************************************************/ -static void stepConcentratePumpToTargetSpeed( CONCENTRATE_PUMPS_T pumpId ) +static BOOL stepConcentratePumpToTargetSpeed( CONCENTRATE_PUMPS_T pumpId ) { - F32 const currentToTargetDiff = fabs( concentratePumps[ pumpId ].pumpTargetSpeed - concentratePumps[ pumpId ].currentPumpSpeed ); F32 speedIncrease; + BOOL hasTgtBeenReached = FALSE; + F32 currentToTargetDiff = fabs( concentratePumps[ pumpId ].pumpTargetSpeed - concentratePumps[ pumpId ].currentPumpSpeed ); + if ( currentToTargetDiff > NEARLY_ZERO ) { if ( currentToTargetDiff > CONCENTRATE_PUMP_SPEED_INCREMENT ) @@ -480,7 +548,8 @@ } else { - speedIncrease = currentToTargetDiff; + speedIncrease = currentToTargetDiff; + hasTgtBeenReached = TRUE; } // Subtract current speed when target speed is smaller @@ -494,45 +563,16 @@ if ( concentratePumps[ pumpId ].currentPumpSpeed > NEARLY_ZERO ) { - F32 const timePerStep = CONCENTRATE_PUMP_VOLUME_PER_REV / ( concentratePumps[ pumpId ].currentPumpSpeed * CONCENTRATE_PUMP_STEP_PER_REV ) ; - F32 const stepPeriodCounts = timePerStep / ( CONCENTRATE_PUMP_STEP_PERIOD_RESOLUTION * CONCENTRATE_PUMP_MICRO_STEPS_PER_STEP ); - concentratePumps[ pumpId ].togglePeriodCount = (U16)( stepPeriodCounts + 0.5 ); + F32 timePerStep = CONCENTRATE_PUMP_VOLUME_PER_REV / ( concentratePumps[ pumpId ].currentPumpSpeed * CONCENTRATE_PUMP_STEP_PER_REV ); + F32 stepPeriodCounts = timePerStep / ( CONCENTRATE_PUMP_STEP_PERIOD_RESOLUTION * CONCENTRATE_PUMP_MICRO_STEPS_PER_STEP ); + concentratePumps[ pumpId ].togglePeriodCount = (U16)( stepPeriodCounts + FLOAT_TO_INT_ROUNDUP_OFFSET ); } else { concentratePumps[ pumpId ].togglePeriodCount = CONCENTRATE_PUMP_ZERO_FLOW_RATE; } -} -/*********************************************************************//** - * @brief - * The handleConcentratePumpOnState function turns off concentrate pumps switch - * to off state upon request. While in on state, the function controls concentrate - * pumps to a target step speed. - * @details Inputs: currentPumpSpeed[] - * @details Outputs: control concentrate pumps to target step speed - * @param pumpId concentrate pump id - * @return state - *************************************************************************/ -static CONCENTRATE_PUMP_STATE_T handleConcentratePumpOnState( CONCENTRATE_PUMPS_T pumpId ) -{ - CONCENTRATE_PUMP_STATE_T state = CONCENTRATE_PUMP_ON_STATE; - - if ( ++concentratePumps[ pumpId ].controlTimerCounter >= CONCENTRATE_PUMP_CONTROL_INTERVAL ) - { - concentratePumps[ pumpId ].controlTimerCounter = 0; - - stepConcentratePumpToTargetSpeed( pumpId ); - concentratePumps[ pumpId ].setStepSpeed( concentratePumps[ pumpId ].togglePeriodCount ); - } - - if ( FALSE == concentratePumps[ pumpId ].hasTurnOnPumpsBeenRequested ) - { - state = CONCENTRATE_PUMP_OFF_STATE; - stopConcentratePump( pumpId ); - } - - return state; + return hasTgtBeenReached; } /*********************************************************************//** @@ -548,17 +588,19 @@ static void calcMeasuredPumpsSpeed( CONCENTRATE_PUMPS_T pumpId, U16 pulseWidthCount ) { F32 pulseWidthInSecond = ( (F32)pulseWidthCount * CONCENTRATE_PUMP_HALL_SENSE_PERIOD_RESOLUTION ) / US_PER_SECOND; + BOOL isPulseWidthOut = ( pulseWidthInSecond <= (F32)CONCENTRATE_PUMP_MIN_ALLOWED_HALL_SENSOR_COUNT ? TRUE : FALSE ); + + // TODO remove this line once the below code is tested concentratePumps[ pumpId ].measuredPumpSpeed.data = ( 1.0 / pulseWidthInSecond ) * CONCENTRATE_PUMP_VOLUME_PER_PULSE * SEC_PER_MIN; - /* TODO will implement in DEN-12224 - // BOOL isPulseWidthOut = ( pulseWidthInSecond <= CONCENTRATE_PUMP_MIN_ALLOWED_HALL_SENSOR_COUNT ? TRUE : FALSE ); + // TODO will implement in DEN-12224 //checkPersistentAlarm( ALARM_ID_DG_CONC_PUMP_HALL_SENSOR_OUT_OF_RANGE, isPulseWidthOut, pulseWidthInSecond, CONCENTRATE_PUMP_MIN_ALLOWED_HALL_SENSOR_COUNT ); - if ( FALSE == isPulseWidthOut ) - { - concentratePumps[ pumpId ].measuredPumpSpeed.data = ( 1.0 / pulseWidthInSecond ) * CONCENTRATE_PUMP_VOLUME_PER_PULSE * SEC_PER_MIN; - } - TODO will implement in DEN-12224 */ + //if ( FALSE == isPulseWidthOut ) + //{ + // concentratePumps[ pumpId ].measuredPumpSpeed.data = ( 1.0 / pulseWidthInSecond ) * CONCENTRATE_PUMP_VOLUME_PER_PULSE * SEC_PER_MIN; + //} + // TODO will implement in DEN-12224 if ( CONCENTRATE_PUMP_ZERO_FLOW_RATE == pulseWidthCount ) {