Index: firmware/App/Controllers/DialInFlow.c =================================================================== diff -u --- firmware/App/Controllers/DialInFlow.c (revision 0) +++ firmware/App/Controllers/DialInFlow.c (revision 96bff7c1a8c755eb28a89e131a737dc1c4a7d9fc) @@ -0,0 +1,983 @@ +/************************************************************************** + * + * Copyright (c) 2019-2019 Diality Inc. - All Rights Reserved. + * + * THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN + * WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER. + * + * @file DialInFlow.c + * + * @date 16-Dec-2019 + * @author S. Nash + * + * @brief Monitor/Controller for dialysate inlet pump and flow sensor. + * + **************************************************************************/ + +#ifndef _VECTORCAST_ + #include +#endif + +#include "can.h" +#include "etpwm.h" + +#include "Common.h" +#include "FPGA.h" +#include "InternalADC.h" +#include "OperationModes.h" +#include "SystemCommMessages.h" +#include "TaskGeneral.h" +#include "TaskPriority.h" +#include "Timers.h" +#include "DialInFlow.h" + +// ********** private definitions ********** + +#define DIAL_IN_FLOW_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) // interval (ms/task time) at which the dialIn flow data is published on the CAN bus + +#define MAX_DIAL_IN_FLOW_RATE 500 // mL/min +#define MIN_DIAL_IN_FLOW_RATE 100 // mL/min + +#define MAX_DIAL_IN_PUMP_PWM_STEP_CHANGE 0.02 // duty cycle TODO - fixed or parameterized or set in motor controller? +#define MAX_DIAL_IN_PUMP_PWM_DUTY_CYCLE 0.88 // controller will error if PWM duty cycle > 90%, so set max to 88% +#define MIN_DIAL_IN_PUMP_PWM_DUTY_CYCLE 0.12 // controller will error if PWM duty cycle < 10%, so set min to 12% + +#define DIP_CONTROL_INTERVAL ( 500 / TASK_GENERAL_INTERVAL ) // interval (ms/task time) at which the dialIn pump is controlled +#define DIP_P_COEFFICIENT 0.0002 // P term for dialIn pump control +#define DIP_I_COEFFICIENT 0.00002 // I term for dialIn pump control +#define DIP_MAX_ERROR_SUM 10.0 // for anti-wind-up in I term +#define DIP_MIN_ERROR_SUM -10.0 +#define DIP_MAX_PWM_DC_DELTA 0.01 // prevents large steps in PWM duty cycle +#define DIP_MIN_PWM_DC_DELTA -0.01 + +#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 +#define DIP_MAX_CURR_WHEN_RUNNING_MA 1000.0 // motor controller current should not exceed this when pump should be running +#define DIP_MAX_CURR_ERROR_DURATION_MS 2000 // motor controller current errors persisting beyond this duration will trigger an alarm + +#define DIP_SPEED_ADC_TO_RPM_FACTOR 1.375 // conversion factor from ADC counts to RPM for dialIn pump motor +#define DIP_CURRENT_ADC_TO_MA_FACTOR 2.65 // conversion factor from ADC counts to mA for dialIn pump motor + +#define DIP_REV_PER_LITER 124.0 // rotor revolutions per liter +#define DIP_ML_PER_MIN_TO_PUMP_RPM_FACTOR ( DIP_REV_PER_LITER / ML_PER_LITER ) +#define DIP_GEAR_RATIO 32.0 // dialIn pump motor to dialIn pump gear ratio +#define DIP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.0003717 // ~27 BP motor RPM = 1% PWM duty cycle +#define DIP_PWM_ZERO_OFFSET 0.1 // 10% PWM duty cycle = zero speed +#define DIP_PWM_FROM_ML_PER_MIN(rate) ( (rate) * DIP_ML_PER_MIN_TO_PUMP_RPM_FACTOR * DIP_GEAR_RATIO * DIP_MOTOR_RPM_TO_PWM_DC_FACTOR + DIP_PWM_ZERO_OFFSET ) + +#define DIAL_INPUMP_ADC_FULL_SCALE_V 3.0 // BP analog signals are 0-3V (while int. ADC ref V is 3.3V) +#define DIAL_INPUMP_ADC_MID_PT_BITS ( (F32)( INT_ADC_FULL_SCALE_BITS >> 1 ) * ( DIAL_INPUMP_ADC_FULL_SCALE_V / INT_ADC_REF_V ) ) +#define SIGN_FROM_12_BIT_VALUE(v) ( (S16)(v) - (S16)DIAL_INPUMP_ADC_MID_PT_BITS ) + +#define DIAL_IN_FLOW_SAMPLE_FREQ ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) +#define SIZE_OF_ROLLING_AVG (U32)( (F32)DIAL_IN_FLOW_SAMPLE_FREQ * 0.8 ) // measured dialIn flow is filtered w/ moving average +#define MAX_FLOW_FILTER_INTERVAL 5 // slowest sample interval for filter is every 5th sample + +typedef enum DialInPump_States +{ + DIAL_IN_PUMP_OFF_STATE = 0, + DIAL_IN_PUMP_RAMPING_UP_STATE, + DIAL_IN_PUMP_RAMPING_DOWN_STATE, + DIAL_IN_PUMP_CONTROL_TO_TARGET_STATE, + NUM_OF_DIAL_IN_PUMP_STATES +} DIAL_IN_PUMP_STATE_T; + +typedef enum DialInFlow_Self_Test_States +{ + DIAL_IN_FLOW_SELF_TEST_STATE_START = 0, + DIAL_IN_FLOW_TEST_STATE_IN_PROGRESS, + DIAL_IN_FLOW_TEST_STATE_COMPLETE, + NUM_OF_DIAL_IN_FLOW_SELF_TEST_STATES +} DIAL_IN_FLOW_SELF_TEST_STATE_T; + +// CAN3 port pin assignments for pump stop and direction outputs +#define STOP_CAN3_PORT_MASK 0x00000002 // (Tx - re-purposed as output GPIO for dialIn pump stop signal) +#define DIR_CAN3_PORT_MASK 0x00000002 // (Rx - re-purposed as output GPIO for dialIn pump direction signal) +// dialIn pump stop and direction macros +#define SET_DIP_DIR() {canREG3->RIOC |= DIR_CAN3_PORT_MASK;} +#define SET_DIP_STOP() {canREG3->TIOC |= STOP_CAN3_PORT_MASK;} +#define CLR_DIP_DIR() {canREG3->RIOC &= ~DIR_CAN3_PORT_MASK;} +#define CLR_DIP_STOP() {canREG3->TIOC &= ~STOP_CAN3_PORT_MASK;} + +// ********** private data ********** + +static DIAL_IN_PUMP_STATE_T dialInPumpState = DIAL_IN_PUMP_OFF_STATE; // current state of dialIn flow controller state machine +static U32 dialInFlowDataPublicationTimerCounter = 0; // used to schedule dialIn flow data publication to CAN bus +static BOOL isDialInPumpOn = FALSE; // dialIn pump is currently running +static F32 dialInPumpPWMDutyCyclePct = 0.0; // initial dialIn pump PWM duty cycle +static F32 dialInPumpPWMDutyCyclePctSet = 0.0; // currently set dialIn pump PWM duty cycle +static MOTOR_DIR_T dialInPumpDirection = MOTOR_DIR_FORWARD; // requested dialIn flow direction +static MOTOR_DIR_T dialInPumpDirectionSet = MOTOR_DIR_FORWARD; // currently set dialIn flow direction + +DATA_DECL( U32, DialInFlowDataPub, dialInFlowDataPublishInterval, DIAL_IN_FLOW_DATA_PUB_INTERVAL, DIAL_IN_FLOW_DATA_PUB_INTERVAL ); // interval (in ms) at which to publish dialIn flow data to CAN bus +DATA_DECL( S32, TargetDialInFlowRate, targetDialInFlowRate, 0, 0 ); // requested dialIn flow rate +DATA_DECL( F32, MeasuredDialInFlowRate, measuredDialInFlowRate, 0.0, 0.0 ); // measured dialIn flow rate +DATA_DECL( F32, MeasuredDialInPumpRotorSpeed, dialInPumpRotorSpeedRPM, 0.0, 0.0 );// measured dialIn pump rotor speed +DATA_DECL( F32, MeasuredDialInPumpSpeed, dialInPumpSpeedRPM, 0.0, 0.0 ); // measured dialIn pump motor speed +DATA_DECL( F32, MeasuredDialInPumpMCSpeed, adcDialInPumpMCSpeedRPM, 0.0, 0.0 ); // measured dialIn pump motor controller speed +DATA_DECL( F32, MeasuredDialInPumpMCCurrent, adcDialInPumpMCCurrentmA, 0.0, 0.0 );// measured dialIn pump motor controller current + +static F32 dipFlowError = 0.0; // dialIn flow error +static F32 dipFlowErrorSum = 0.0; // dialIn flow error sum +static U32 dipControlTimerCounter = 0; // determines when to perform control on dialIn flow + +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 +static U32 flowReadingsCount = 0; // # of samples in flow rolling average buffer +static U32 flowReadingsTmrCtr = 0; // determines when to add samples to filter + +static U32 dipCurrErrorDurationCtr = 0; // used for tracking persistence of dip current errors + +static DIAL_IN_FLOW_SELF_TEST_STATE_T dialInPumpSelfTestState = DIAL_IN_FLOW_SELF_TEST_STATE_START; // current dialIn pump self test state +static U32 dialInPumpSelfTestTimerCount = 0; // timer counter for dialIn pump self test + +// ********** private function prototypes ********** + +static DIAL_IN_PUMP_STATE_T handleDialInPumpOffState( void ); +static DIAL_IN_PUMP_STATE_T handleDialInPumpRampingUpState( void ); +static DIAL_IN_PUMP_STATE_T handleDialInPumpRampingDownState( void ); +static DIAL_IN_PUMP_STATE_T handleDialInPumpControlToTargetState( void ); +static void stopDialInPump( void ); +static void releaseDialInPumpStop( void ); +static void setDialInPumpDirection( MOTOR_DIR_T dir ); +static void publishDialInFlowData( void ); +static void resetDialInFlowMovingAverage( void ); +static void filterDialInFlowReadings( F32 flow ); +static void checkDialInPumpDirection( void ); +static void checkDialInPumpMCCurrent( void ); +static DATA_GET_PROTOTYPE( U32, getPublishDialInFlowDataInterval ); + +/************************************************************************* + * @brief initDialInFlow + * The initDialInFlow function initializes the DialInFlow module. + * @details + * Inputs : none + * Outputs : DialInFlow module initialized. + * @param none + * @return none + *************************************************************************/ +void initDialInFlow( void ) +{ + stopDialInPump(); + setDialInPumpDirection( MOTOR_DIR_FORWARD ); + + // zero rolling flow average buffer + resetDialInFlowMovingAverage(); +} + +/************************************************************************* + * @brief setDialInPumpTargetFlowRate + * The setDialInPumpTargetFlowRate function sets a new target flow rate and + * pump direction. + * @details + * Inputs : isDialInPumpOn, dialInPumpDirectionSet + * Outputs : targetDialInFlowRate, dialInPumpdirection, dialInPumpPWMDutyCyclePct + * @param flowRate : new target dialIn flow rate + * @param dir : new dialIn flow direction + * @return TRUE if new flow rate & dir are set, FALSE if not + *************************************************************************/ +BOOL setDialInPumpTargetFlowRate( U32 flowRate, MOTOR_DIR_T dir ) +{ + BOOL result = FALSE; + + // direction change while pump is running is not allowed + if ( ( FALSE == isDialInPumpOn ) || ( 0 == flowRate ) || ( dir == dialInPumpDirectionSet ) ) + { + // verify flow rate + if ( flowRate <= MAX_DIAL_IN_FLOW_RATE ) + { + resetDialInFlowMovingAverage(); + targetDialInFlowRate.data = ( dir == MOTOR_DIR_FORWARD ? (S32)flowRate : (S32)flowRate * -1 ); + dialInPumpDirection = dir; + // set PWM duty cycle target to an estimated initial target to ramp to based on target flow rate - then we'll control to flow when ramp completed + dialInPumpPWMDutyCyclePct = DIP_PWM_FROM_ML_PER_MIN((F32)flowRate); // ~ 8% per 100 mL/min with a 10% zero offset added in (e.g. 100 mL/min = 8+10 = 18%) + // reset flow control stats + dipFlowError = 0.0; + dipFlowErrorSum = 0.0; + + switch ( dialInPumpState ) + { + case DIAL_IN_PUMP_RAMPING_UP_STATE: // see if we need to reverse direction of ramp + if ( dialInPumpPWMDutyCyclePct < dialInPumpPWMDutyCyclePctSet ) + { + dialInPumpState = DIAL_IN_PUMP_RAMPING_DOWN_STATE; + } + break; + case DIAL_IN_PUMP_RAMPING_DOWN_STATE: // see if we need to reverse direction of ramp + if ( dialInPumpPWMDutyCyclePct > dialInPumpPWMDutyCyclePctSet ) + { + dialInPumpState = DIAL_IN_PUMP_RAMPING_UP_STATE; + } + break; + case DIAL_IN_PUMP_CONTROL_TO_TARGET_STATE: // start ramp to new target in appropriate direction + if ( dialInPumpPWMDutyCyclePctSet > dialInPumpPWMDutyCyclePct ) + { + dialInPumpState = DIAL_IN_PUMP_RAMPING_DOWN_STATE; + } + else + { + dialInPumpState = DIAL_IN_PUMP_RAMPING_UP_STATE; + } + break; + default: + // ok - not all states need to be handled here + break; + } + result = TRUE; + } + else // requested flow rate too high + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_DIAL_IN_FLOW_SET_TOO_HIGH, flowRate ) + } + } + + return result; +} + +/************************************************************************* + * @brief signalDialInPumpHardStop + * The signalDialInPumpHardStop function stops the dialIn pump immediately. + * @details + * Inputs : none + * Outputs : DialIn pump stopped, set point reset, state changed to off + * @param none + * @return none + *************************************************************************/ +void signalDialInPumpHardStop( void ) +{ + targetDialInFlowRate.data = 0; + stopDialInPump(); + dialInPumpState = DIAL_IN_PUMP_OFF_STATE; + dialInPumpPWMDutyCyclePct = 0.0; + dipFlowError = 0.0; + dipFlowErrorSum = 0.0; + dipControlTimerCounter = 0; +} + +/************************************************************************* + * @brief execDialInFlowMonitor + * The execDialInFlowMonitor function executes the dialIn flow monitor. + * @details + * Inputs : none + * Outputs : measuredDialInFlowRate, adcDialInPumpMCSpeedRPM, adcDialInPumpMCCurrentmA + * @param none + * @return none + *************************************************************************/ +void execDialInFlowMonitor( void ) +{ + U16 dipRPM = getIntADCReading( INT_ADC_DIAL_IN_PUMP_SPEED ); + U16 dipmA = getIntADCReading( INT_ADC_DIAL_IN_PUMP_MOTOR_CURRENT ); + F32 dipFlow = getFPGADialysateFlow(); + + adcDialInPumpMCSpeedRPM.data = (F32)(SIGN_FROM_12_BIT_VALUE(dipRPM)) * DIP_SPEED_ADC_TO_RPM_FACTOR; + adcDialInPumpMCCurrentmA.data = (F32)(SIGN_FROM_12_BIT_VALUE(dipmA)) * DIP_CURRENT_ADC_TO_MA_FACTOR; + + filterDialInFlowReadings( dipFlow ); + + // don't start enforcing checks until out of init/POST mode + if ( getCurrentOperationMode() != MODE_INIT ) + { + checkDialInPumpDirection(); + checkDialInPumpMCCurrent(); + } + + // publish dialIn flow data on interval + publishDialInFlowData(); +} + +/************************************************************************* + * @brief execDialInFlowController + * The execDialInFlowController function executes the dialIn flow controller. + * @details + * Inputs : dialInPumpState + * Outputs : dialInPumpState + * @param none + * @return none + *************************************************************************/ +void execDialInFlowController( void ) +{ + switch ( dialInPumpState ) + { + case DIAL_IN_PUMP_OFF_STATE: + dialInPumpState = handleDialInPumpOffState(); + break; + + case DIAL_IN_PUMP_RAMPING_UP_STATE: + dialInPumpState = handleDialInPumpRampingUpState(); + break; + + case DIAL_IN_PUMP_RAMPING_DOWN_STATE: + dialInPumpState = handleDialInPumpRampingDownState(); + break; + + case DIAL_IN_PUMP_CONTROL_TO_TARGET_STATE: + dialInPumpState = handleDialInPumpControlToTargetState(); + break; + + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_DIAL_IN_FLOW_INVALID_DIAL_IN_PUMP_STATE, dialInPumpState ) + break; + } +} + +/************************************************************************* + * @brief handleDialInPumpOffState + * The handleDialInPumpOffState function handles the dialIn pump off state \n + * of the dialIn pump controller state machine. + * @details + * Inputs : targetDialInFlowRate, dialInPumpDirection + * Outputs : dialInPumpPWMDutyCyclePctSet, dialInPumpDirectionSet, isDialInPumpOn + * @param none + * @return next state + *************************************************************************/ +static DIAL_IN_PUMP_STATE_T handleDialInPumpOffState( void ) +{ + DIAL_IN_PUMP_STATE_T result = DIAL_IN_PUMP_OFF_STATE; + + // if we've been given a flow rate, setup ramp up and transition to ramp up state + if ( getTargetDialInFlowRate() != 0 ) + { + // set initial PWM duty cycle + dialInPumpPWMDutyCyclePctSet = MAX_DIAL_IN_PUMP_PWM_STEP_CHANGE; + etpwmSetCmpA( etpwmREG1, (U32)FLOAT_TO_INT_WITH_ROUND( dialInPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) ); + // allow dialIn pump to run in requested direction + setDialInPumpDirection( dialInPumpDirection ); + releaseDialInPumpStop(); + // start PWM for dialIn pump + etpwmStartTBCLK(); + isDialInPumpOn = TRUE; + result = DIAL_IN_PUMP_RAMPING_UP_STATE; + } + + return result; +} + +/************************************************************************* + * @brief handleDialInPumpRampingUpState + * The handleDialInPumpRampingUpState function handles the ramp up state \n + * of the dialIn pump controller state machine. + * @details + * Inputs : dialInPumpPWMDutyCyclePctSet + * Outputs : dialInPumpPWMDutyCyclePctSet + * @param none + * @return next state + *************************************************************************/ +static DIAL_IN_PUMP_STATE_T handleDialInPumpRampingUpState( void ) +{ + DIAL_IN_PUMP_STATE_T result = DIAL_IN_PUMP_RAMPING_UP_STATE; + + // have we been asked to stop the dialIn pump? + if ( 0 == getTargetDialInFlowRate() ) + { + // start ramp down to stop + dialInPumpPWMDutyCyclePctSet -= MAX_DIAL_IN_PUMP_PWM_STEP_CHANGE; + etpwmSetCmpA( etpwmREG1, (U32)FLOAT_TO_INT_WITH_ROUND( dialInPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) ); + result = DIAL_IN_PUMP_RAMPING_DOWN_STATE; + } + // have we reached end of ramp up? + else if ( dialInPumpPWMDutyCyclePctSet >= dialInPumpPWMDutyCyclePct ) + { + resetDialInFlowMovingAverage(); + result = DIAL_IN_PUMP_CONTROL_TO_TARGET_STATE; + } + // continue ramp up + else + { + dialInPumpPWMDutyCyclePctSet += MAX_DIAL_IN_PUMP_PWM_STEP_CHANGE; + etpwmSetCmpA( etpwmREG1, (U32)FLOAT_TO_INT_WITH_ROUND( dialInPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) ); + } + + return result; +} + +/************************************************************************* + * @brief handleDialInPumpRampingDownState + * The handleDialInPumpRampingDownState function handles the ramp down state \n + * of the dialIn pump controller state machine. + * @details + * Inputs : dialInPumpPWMDutyCyclePctSet + * Outputs : dialInPumpPWMDutyCyclePctSet + * @param none + * @return next state + *************************************************************************/ +static DIAL_IN_PUMP_STATE_T handleDialInPumpRampingDownState( void ) +{ + DIAL_IN_PUMP_STATE_T result = DIAL_IN_PUMP_RAMPING_DOWN_STATE; + + // have we essentially reached zero speed + if ( dialInPumpPWMDutyCyclePctSet < (MAX_DIAL_IN_PUMP_PWM_STEP_CHANGE + DIP_PWM_ZERO_OFFSET) ) + { + stopDialInPump(); + result = DIAL_IN_PUMP_OFF_STATE; + } + // have we reached end of ramp down? + else if ( dialInPumpPWMDutyCyclePctSet <= dialInPumpPWMDutyCyclePct ) + { + resetDialInFlowMovingAverage(); + result = DIAL_IN_PUMP_CONTROL_TO_TARGET_STATE; + } + // continue ramp down + else + { + dialInPumpPWMDutyCyclePctSet -= MAX_DIAL_IN_PUMP_PWM_STEP_CHANGE; + etpwmSetCmpA( etpwmREG1, (U32)FLOAT_TO_INT_WITH_ROUND( dialInPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) ); + } + + return result; +} + +/************************************************************************* + * @brief handleDialInPumpControlToTargetState + * The handleDialInPumpControlToTargetState function handles the "control to \n + * target" state of the dialIn pump controller state machine. + * @details + * Inputs : none + * Outputs : dialInPumpState + * @param none + * @return next state + *************************************************************************/ +static DIAL_IN_PUMP_STATE_T handleDialInPumpControlToTargetState( void ) +{ + DIAL_IN_PUMP_STATE_T result = DIAL_IN_PUMP_CONTROL_TO_TARGET_STATE; + F32 tgtFlow = (F32)getTargetDialInFlowRate(); + F32 actFlow = getMeasuredDialInFlowRate(); + F32 pTerm, iTerm; + F32 newPWM; + + // control at set interval + if ( ++dipControlTimerCounter >= DIP_CONTROL_INTERVAL ) + { + // compute P term + if ( MOTOR_DIR_FORWARD == dialInPumpDirectionSet ) + { + dipFlowError = tgtFlow - actFlow; + } + else + { + dipFlowError = (tgtFlow * -1.0) - (actFlow * -1.0); + } + pTerm = dipFlowError * DIP_P_COEFFICIENT; + pTerm = RANGE( pTerm, DIP_MIN_PWM_DC_DELTA, DIP_MAX_PWM_DC_DELTA ); + // compute I term + dipFlowErrorSum += dipFlowError; + iTerm = RANGE( dipFlowErrorSum, DIP_MIN_ERROR_SUM, DIP_MAX_ERROR_SUM ); + iTerm *= DIP_I_COEFFICIENT; + // compute new PWM duty cycle % for dialIn pump motor + newPWM = dialInPumpPWMDutyCyclePctSet + pTerm + iTerm; + newPWM = RANGE( newPWM, MIN_DIAL_IN_PUMP_PWM_DUTY_CYCLE, MAX_DIAL_IN_PUMP_PWM_DUTY_CYCLE ); + dialInPumpPWMDutyCyclePctSet = newPWM; + etpwmSetCmpA( etpwmREG1, (U32)FLOAT_TO_INT_WITH_ROUND( dialInPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) ); + dipControlTimerCounter = 0; + } + + return result; +} + +/************************************************************************* + * @brief stopDialInPump + * The stopDialInPump function sets the dialIn pump stop signal. + * @details + * Inputs : none + * Outputs : dialIn pump stop signal activated, PWM duty cycle zeroed + * @param none + * @return none + *************************************************************************/ +static void stopDialInPump( void ) +{ + isDialInPumpOn = FALSE; + dialInPumpPWMDutyCyclePctSet = 0.0; + etpwmSetCmpA( etpwmREG1, 0 ); + etpwmStopTBCLK(); + SET_DIP_STOP(); +} + +/************************************************************************* + * @brief releaseDialInPumpStop + * The releaseDialInPumpStop function clears the dialIn pump stop signal. + * @details + * Inputs : none + * Outputs : dialIn pump stop signal + * @param none + * @return none + *************************************************************************/ +static void releaseDialInPumpStop( void ) +{ + CLR_DIP_STOP(); +} + +/************************************************************************* + * @brief setDialInPumpDirection + * The setDialInPumpDirection function sets the set dialIn pump direction to \n + * the given direction. + * @details + * Inputs : dialInPumpState + * Outputs : dialInPumpState + * @param dir : dialIn pump direction to set + * @return none + *************************************************************************/ +static void setDialInPumpDirection( MOTOR_DIR_T dir ) +{ + switch ( dir ) + { + case MOTOR_DIR_FORWARD: + dialInPumpDirectionSet = dir; + SET_DIP_DIR(); + break; + + case MOTOR_DIR_REVERSE: + dialInPumpDirectionSet = dir; + CLR_DIP_DIR(); + break; + + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_DIAL_IN_FLOW_INVALID_DIAL_IN_PUMP_DIRECTION, dir ) + break; + } +} + +/************************************************************************* + * @brief getPublishDialInFlowDataInterval + * The getPublishDialInFlowDataInterval function gets the dialIn flow data \n + * publication interval. + * @details + * Inputs : dialInFlowDataPublishInterval + * Outputs : none + * @param none + * @return the current dialIn flow data publication interval (in ms). + *************************************************************************/ +DATA_GET( U32, getPublishDialInFlowDataInterval, dialInFlowDataPublishInterval ) + +/************************************************************************* + * @brief getTargetDialInFlowRate + * The getTargetDialInFlowRate function gets the current target dialIn flow \n + * rate. + * @details + * Inputs : targetDialInFlowRate + * Outputs : none + * @param none + * @return the current target dialIn flow rate (in mL/min). + *************************************************************************/ +DATA_GET( S32, getTargetDialInFlowRate, targetDialInFlowRate ) + +/************************************************************************* + * @brief getMeasuredDialInFlowRate + * The getMeasuredDialInFlowRate function gets the measured dialIn flow \n + * rate. + * @details + * Inputs : measuredDialInFlowRate + * Outputs : none + * @param none + * @return the current dialIn flow rate (in mL/min). + *************************************************************************/ +DATA_GET( F32, getMeasuredDialInFlowRate, measuredDialInFlowRate ) + +/************************************************************************* + * @brief getMeasuredDialInPumpRotorSpeed + * The getMeasuredDialInPumpRotorSpeed function gets the measured dialIn flow \n + * rate. + * @details + * Inputs : dialInPumpRotorSpeedRPM + * Outputs : none + * @param none + * @return the current dialIn flow rate (in mL/min). + *************************************************************************/ +DATA_GET( F32, getMeasuredDialInPumpRotorSpeed, dialInPumpRotorSpeedRPM ) + +/************************************************************************* + * @brief getMeasuredDialInPumpSpeed + * The getMeasuredDialInPumpSpeed function gets the measured dialIn flow \n + * rate. + * @details + * Inputs : dialInPumpSpeedRPM + * Outputs : none + * @param none + * @return the current dialIn flow rate (in mL/min). + *************************************************************************/ +DATA_GET( F32, getMeasuredDialInPumpSpeed, dialInPumpSpeedRPM ) + +/************************************************************************* + * @brief getMeasuredDialInPumpMCSpeed + * The getMeasuredDialInPumpMCSpeed function gets the measured dialIn pump \n + * speed. + * @details + * Inputs : adcDialInPumpMCSpeedRPM + * Outputs : none + * @param none + * @return the current dialIn pump speed (in RPM). + *************************************************************************/ +DATA_GET( F32, getMeasuredDialInPumpMCSpeed, adcDialInPumpMCSpeedRPM ) + +/************************************************************************* + * @brief getMeasuredDialInPumpMCCurrent + * The getMeasuredDialInPumpMCCurrent function gets the measured dialIn pump \n + * current. + * @details + * Inputs : adcDialInPumpMCCurrentmA + * Outputs : none + * @param none + * @return the current dialIn pump current (in mA). + *************************************************************************/ +DATA_GET( F32, getMeasuredDialInPumpMCCurrent, adcDialInPumpMCCurrentmA ) + +/************************************************************************* + * @brief publishDialInFlowData + * The publishDialInFlowData function publishes dialIn flow data at the set \n + * interval. + * @details + * Inputs : target flow rate, measured flow rate, measured MC speed, \n + * measured MC current + * Outputs : DialIn flow data is published to CAN bus. + * @param none + * @return none + *************************************************************************/ +static void publishDialInFlowData( void ) +{ + // publish dialIn flow data on interval + if ( ++dialInFlowDataPublicationTimerCounter > getPublishDialInFlowDataInterval() ) + { + S32 flowStPt = (S32)getTargetDialInFlowRate(); + F32 measFlow = getMeasuredDialInFlowRate(); + F32 measRotSpd = getMeasuredDialInPumpRotorSpeed(); + F32 measSpd = getMeasuredDialInPumpSpeed(); + F32 measMCSpd = getMeasuredDialInPumpMCSpeed(); + F32 measMCCurr = getMeasuredDialInPumpMCCurrent(); +#ifdef DEBUG_ENABLED + // TODO - temporary debug code - remove later + S32 pwm = (S32)( 100.0 * dialInPumpPWMDutyCyclePctSet ); + char debugFlowStr[ 256 ]; + + sprintf( debugFlowStr, "Target Flow:%5d, Meas. Flow:%5d, Speed:%5d RPM, Current:%5d mA, PWM:%5d \n", flowStPt, (S32)measFlow, (S32)measMCSpd, (S32)measMCCurr, pwm ); + sendDebugData( (U08*)debugFlowStr, strlen(debugFlowStr) ); +#endif + // TODO - broadcastDialInFlowData( flowStPt, measFlow, measRotSpd, measSpd, measMCSpd, measMCCurr ); + dialInFlowDataPublicationTimerCounter = 0; + } +} + +/************************************************************************* + * @brief resetDialInFlowMovingAverage + * The resetDialInFlowMovingAverage function re-sizes and re-initializes the \n + * dialIn flow moving average sample buffer. + * @details + * Inputs : none + * Outputs : flowReadingsTotal, flowReadingsIdx, flowReadingsCount all set to zero. + * @param initFlow : the new dialIn flow set pt. + * @param flowDir : the new set direction + * @return none + *************************************************************************/ +static void resetDialInFlowMovingAverage( void ) +{ + flowReadingsTotal = 0.0; + flowReadingsIdx = 0; + flowReadingsCount = 0; + dipControlTimerCounter = 0; +} + +/************************************************************************* + * @brief filterDialInFlowReadings + * The filterDialInFlowReadings function adds a new flow sample to the filter \n + * if decimation rate for current set point calls for it. + * @details + * Inputs : none + * Outputs : flowReadings[], flowReadingsIdx, flowReadingsCount + * @param flow : newest dialIn flow sample + * @return none + *************************************************************************/ +static void filterDialInFlowReadings( F32 flow ) +{ + BOOL addSampleToFilter = FALSE; + + if ( ( targetDialInFlowRate.data < MIN_DIAL_IN_FLOW_RATE ) || ( targetDialInFlowRate.data >= MAX_DIAL_IN_FLOW_RATE ) ) + { + addSampleToFilter = TRUE; + } + else + { + switch ( flowReadingsTmrCtr ) + { + case 0: + addSampleToFilter = TRUE; + break; + + case 1: + addSampleToFilter = FALSE; + break; + + case 2: + if ( targetDialInFlowRate.data >= 400 ) + { + addSampleToFilter = TRUE; + } + break; + + case 3: + if ( targetDialInFlowRate.data >= 200 ) + { + addSampleToFilter = TRUE; + } + break; + + case 4: + if ( targetDialInFlowRate.data >= 300 ) + { + addSampleToFilter = TRUE; + } + break; + + default: + // TODO - s/w fault? + break; + } + } + + if ( TRUE == addSampleToFilter ) + { + if ( flowReadingsCount >= SIZE_OF_ROLLING_AVG ) + { + flowReadingsTotal -= flowReadings[ flowReadingsIdx ]; + } + flowReadings[ flowReadingsIdx ] = flow; + flowReadingsTotal += flow; + flowReadingsIdx = INC_WRAP( flowReadingsIdx, 0, SIZE_OF_ROLLING_AVG - 1 ); + flowReadingsCount = INC_CAP( flowReadingsCount, SIZE_OF_ROLLING_AVG ); + measuredDialInFlowRate.data = flowReadingsTotal / (F32)flowReadingsCount; + } + + flowReadingsTmrCtr = INC_WRAP( flowReadingsTmrCtr, 0, MAX_FLOW_FILTER_INTERVAL - 1 ); +} + +/************************************************************************* + * @brief checkDialInPumpDirection + * The checkDialInPumpDirection function checks the set direction vs. \n + * the direction implied by the sign of the measured MC speed. + * @details + * Inputs : + * Outputs : + * @param none + * @return none + *************************************************************************/ +static void checkDialInPumpDirection( void ) +{ + MOTOR_DIR_T dipMCDir; + + if ( DIAL_IN_PUMP_CONTROL_TO_TARGET_STATE == dialInPumpState ) + { + // check set direction vs. direction from sign of motor controller speed + dipMCDir = ( getMeasuredDialInPumpMCSpeed() >= 0.0 ? MOTOR_DIR_FORWARD : MOTOR_DIR_REVERSE ); + if ( dialInPumpDirectionSet != dipMCDir ) + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DIAL_IN_PUMP_MC_DIRECTION_CHECK, (U32)dialInPumpDirectionSet, (U32)dipMCDir ) + } + } +} + +/************************************************************************* + * @brief checkDialInPumpMCCurrent + * The checkDialInPumpMCCurrent function checks the measured MC current vs. \n + * the set state of the dialIn pump (stopped or running). + * @details + * Inputs : + * Outputs : + * @param none + * @return none + *************************************************************************/ +static void checkDialInPumpMCCurrent( void ) +{ + F32 dipCurr; + + // dialIn pump should be off + if ( DIAL_IN_PUMP_OFF_STATE == dialInPumpState ) + { + dipCurr = fabs( getMeasuredDialInPumpMCCurrent() ); + if ( dipCurr > DIP_MAX_CURR_WHEN_STOPPED_MA ) + { + dipCurrErrorDurationCtr += TASK_PRIORITY_INTERVAL; + if ( dipCurrErrorDurationCtr > DIP_MAX_CURR_ERROR_DURATION_MS ) + { + SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DIAL_IN_PUMP_MC_CURRENT_CHECK, getMeasuredDialInPumpMCCurrent() ); + } + } + else + { + dipCurrErrorDurationCtr = 0; + } + } + // dialIn pump should be running + else + { + dipCurr = fabs( getMeasuredDialInPumpMCCurrent() ); + if ( ( dipCurr < DIP_MIN_CURR_WHEN_RUNNING_MA ) || ( dipCurr > DIP_MAX_CURR_WHEN_RUNNING_MA ) ) + { + dipCurrErrorDurationCtr += TASK_PRIORITY_INTERVAL; + if ( dipCurrErrorDurationCtr > DIP_MAX_CURR_ERROR_DURATION_MS ) + { + SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DIAL_IN_PUMP_MC_CURRENT_CHECK, getMeasuredDialInPumpMCCurrent() ); + } + } + else + { + dipCurrErrorDurationCtr = 0; + } + } +} + +/************************************************************************* + * @brief execDialInFlowTest + * The execDialInFlowTest function executes the state machine for the \n + * DialInFlow self test. + * @details + * Inputs : none + * Outputs : none + * @param none + * @return the current state of the DialInFlow self test. + *************************************************************************/ +SELF_TEST_STATUS_T execDialInFlowTest( void ) +{ + SELF_TEST_STATUS_T result = SELF_TEST_STATUS_FAILED; + + // TODO - implement self test(s) + + return result; +} + + +/************************************************************************* + * TEST SUPPORT FUNCTIONS + *************************************************************************/ + + +/************************************************************************* + * @brief testSetDialInFlowDataPublishIntervalOverride + * The testSetDialInFlowDataPublishIntervalOverride function overrides the \n + * dialIn flow data publish interval. + * @details + * Inputs : none + * Outputs : dialInFlowDataPublishInterval + * @param value : override dialIn flow data publish interval with (in ms) + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +BOOL testSetDialInFlowDataPublishIntervalOverride( U32 value ) +{ + BOOL result = FALSE; + + if ( TRUE == isTestingActivated() ) + { + U32 intvl = value / TASK_PRIORITY_INTERVAL; + + result = TRUE; + dialInFlowDataPublishInterval.ovData = intvl; + dialInFlowDataPublishInterval.override = OVERRIDE_KEY; + } + + return result; +} + +/************************************************************************* + * @brief testResetDialInFlowDataPublishIntervalOverride + * The testResetDialInFlowDataPublishIntervalOverride function resets the override \n + * of the dialIn flow data publish interval. + * @details + * Inputs : none + * Outputs : dialInFlowDataPublishInterval + * @return TRUE if override reset successful, FALSE if not + *************************************************************************/ +BOOL testResetDialInFlowDataPublishIntervalOverride( void ) +{ + BOOL result = FALSE; + + if ( TRUE == isTestingActivated() ) + { + result = TRUE; + dialInFlowDataPublishInterval.override = OVERRIDE_RESET; + dialInFlowDataPublishInterval.ovData = dialInFlowDataPublishInterval.ovInitData; + } + + return result; +} + +/************************************************************************* + * @brief testSetTargetDialInFlowRateOverride and testResetTargetDialInFlowRateOverride + * The testSetTargetDialInFlowRateOverride function overrides the target \n + * dialIn flow rate. \n + * The testResetTargetDialInFlowRateOverride function resets the override of the \n + * target dialIn flow rate. + * @details + * Inputs : none + * Outputs : targetDialInFlowRate + * @param value : override target dialIn flow rate (in mL/min) + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +DATA_OVERRIDE_FUNC( S32, testSetTargetDialInFlowRateOverride, testResetTargetDialInFlowRateOverride, targetDialInFlowRate ) + +/************************************************************************* + * @brief testSetMeasuredDialInFlowRateOverride and testResetMeasuredDialInFlowRateOverride + * The testResetMeasuredDialInFlowRateOverride function overrides the measured \n + * dialIn flow rate. \n + * The testResetOffButtonStateOverride function resets the override of the \n + * measured dialIn flow rate. + * @details + * Inputs : none + * Outputs : measuredDialInFlowRate + * @param value : override measured dialIn flow rate (in mL/min) + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +DATA_OVERRIDE_FUNC( F32, testSetMeasuredDialInFlowRateOverride, testResetMeasuredDialInFlowRateOverride, measuredDialInFlowRate ) + +/************************************************************************* + * @brief testSetMeasuredDialInPumpRotorSpeedOverride and testResetMeasuredDialInPumpRotorSpeedOverride + * The testSetMeasuredDialInPumpRotorSpeedOverride function overrides the measured \n + * dialIn pump rotor speed. \n + * The testResetMeasuredDialInPumpRotorSpeedOverride function resets the override of the \n + * measured dialIn pump rotor speed. + * @details + * Inputs : none + * Outputs : dialInPumpRotorSpeedRPM + * @param value : override measured dialIn pump rotor speed (in RPM) + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +DATA_OVERRIDE_FUNC( F32, testSetMeasuredDialInPumpRotorSpeedOverride, testResetMeasuredDialInPumpRotorSpeedOverride, dialInPumpRotorSpeedRPM ) + +/************************************************************************* + * @brief testSetMeasuredDialInPumpSpeedOverride and testResetMeasuredDialInPumpSpeedOverride + * The testSetMeasuredDialInPumpSpeedOverride function overrides the measured \n + * dialIn pump motor speed. \n + * The testResetMeasuredDialInPumpSpeedOverride function resets the override of the \n + * measured dialIn pump motor speed. + * @details + * Inputs : none + * Outputs : dialInPumpSpeedRPM + * @param value : override measured dialIn pump motor speed (in RPM) + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +DATA_OVERRIDE_FUNC( F32, testSetMeasuredDialInPumpSpeedOverride, testResetMeasuredDialInPumpSpeedOverride, dialInPumpSpeedRPM ) + +/************************************************************************* + * @brief testSetMeasuredDialInPumpMCSpeedOverride and testResetMeasuredDialInPumpMCSpeedOverride + * The testSetMeasuredDialInPumpMCSpeedOverride function overrides the measured \n + * dialIn pump motor speed. \n + * The testResetMeasuredDialInPumpMCSpeedOverride function resets the override of the \n + * measured dialIn pump motor speed. + * @details + * Inputs : none + * Outputs : adcDialInPumpMCSpeedRPM + * @param value : override measured dialIn pump speed (in RPM) + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +DATA_OVERRIDE_FUNC( F32, testSetMeasuredDialInPumpMCSpeedOverride, testResetMeasuredDialInPumpMCSpeedOverride, adcDialInPumpMCSpeedRPM ) + +/************************************************************************* + * @brief testSetMeasuredDialInPumpMCCurrentOverride and testResetMeasuredDialInPumpMCCurrentOverride + * The testSetMeasuredDialInPumpMCCurrentOverride function overrides the measured \n + * dialIn pump motor current. \n + * The testResetMeasuredDialInPumpMCCurrentOverride function resets the override of the \n + * measured dialIn pump motor current. + * @details + * Inputs : none + * Outputs : adcDialInPumpMCCurrentmA + * @param value : override measured dialIn pump current (in mA) + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +DATA_OVERRIDE_FUNC( F32, testSetMeasuredDialInPumpMCCurrentOverride, testResetMeasuredDialInPumpMCCurrentOverride, adcDialInPumpMCCurrentmA ) + + Index: firmware/App/Controllers/DialInFlow.h =================================================================== diff -u --- firmware/App/Controllers/DialInFlow.h (revision 0) +++ firmware/App/Controllers/DialInFlow.h (revision 96bff7c1a8c755eb28a89e131a737dc1c4a7d9fc) @@ -0,0 +1,58 @@ +/************************************************************************** + * + * Copyright (c) 2019-2019 Diality Inc. - All Rights Reserved. + * + * THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN + * WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER. + * + * @file DialInFlow.h + * + * @date 16-Dec-2019 + * @author S. Nash + * + * @brief DialInFlow header file. + * + **************************************************************************/ + +#ifndef __DIALYSATE_IN_FLOW_H__ +#define __DIALYSATE_IN_FLOW_H__ + +#include "Common.h" + +// ********** public definitions ********** + + +// ********** public function prototypes ********** + +void initDialInFlow( void ); +void execDialInFlowMonitor( void ); +void execDialInFlowController( void ); + +BOOL setDialInPumpTargetFlowRate( U32 flowRate, MOTOR_DIR_T dir ); +void signalDialInPumpHardStop( void ); + +SELF_TEST_STATUS_T execDialInFlowTest( void ); + +DATA_GET_PROTOTYPE( S32, getTargetDialInFlowRate ); +DATA_GET_PROTOTYPE( F32, getMeasuredDialInFlowRate); +DATA_GET_PROTOTYPE( F32, getMeasuredDialInPumpRotorSpeed ); +DATA_GET_PROTOTYPE( F32, getMeasuredDialInPumpSpeed ); +DATA_GET_PROTOTYPE( F32, getMeasuredDialInPumpMCSpeed ); +DATA_GET_PROTOTYPE( F32, getMeasuredDialInPumpMCCurrent ); + +BOOL testSetDialInFlowDataPublishIntervalOverride( U32 value ); +BOOL testResetDialInFlowDataPublishIntervalOverride( void ); +BOOL testSetTargetDialInFlowRateOverride( S32 value ); +BOOL testResetTargetDialInFlowRateOverride( void ); +BOOL testSetMeasuredDialInFlowRateOverride( F32 value ); +BOOL testResetMeasuredDialInFlowRateOverride( void ); +BOOL testSetMeasuredDialInPumpRotorSpeedOverride( F32 value ); +BOOL testResetMeasuredDialInPumpRotorSpeedOverride( void ); +BOOL testSetMeasuredDialInPumpSpeedOverride( F32 value ); +BOOL testResetMeasuredDialInPumpSpeedOverride( void ); +BOOL testSetMeasuredDialInPumpMCSpeedOverride( F32 value ); +BOOL testResetMeasuredDialInPumpMCSpeedOverride( void ); +BOOL testSetMeasuredDialInPumpMCCurrentOverride( F32 value ); +BOOL testResetMeasuredDialInPumpMCCurrentOverride( void ); + +#endif Index: firmware/App/Controllers/DialOutUF.c =================================================================== diff -u --- firmware/App/Controllers/DialOutUF.c (revision 0) +++ firmware/App/Controllers/DialOutUF.c (revision 96bff7c1a8c755eb28a89e131a737dc1c4a7d9fc) @@ -0,0 +1,837 @@ +/************************************************************************** + * + * Copyright (c) 2019-2019 Diality Inc. - All Rights Reserved. + * + * THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN + * WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER. + * + * @file DialOutUF.c + * + * @date 16-Dec-2019 + * @author S. Nash & L. Baloa + * + * @brief Monitor/Controller for dialysate outlet pump and load sensors. + * + **************************************************************************/ + +#ifndef _VECTORCAST_ + #include +#endif + +#include "can.h" +#include "etpwm.h" + +#include "Common.h" +#include "FPGA.h" +#include "InternalADC.h" +#include "OperationModes.h" +#include "SystemCommMessages.h" +#include "TaskGeneral.h" +#include "TaskPriority.h" +#include "Timers.h" +#include "DialOutUF.h" + +// ********** private definitions ********** + +#define DIAL_OUT_FLOW_DATA_PUB_INTERVAL ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) // interval (ms/task time) at which the dialOut flow data is published on the CAN bus + +#define MAX_DIAL_OUT_FLOW_RATE 500 // mL/min +#define MIN_DIAL_OUT_FLOW_RATE 100 // mL/min + +#define MAX_DIAL_OUT_PUMP_PWM_STEP_CHANGE 0.02 // duty cycle TODO - fixed or parameterized or set in motor controller? +#define MAX_DIAL_OUT_PUMP_PWM_DUTY_CYCLE 0.88 // controller will error if PWM duty cycle > 90%, so set max to 88% +#define MIN_DIAL_OUT_PUMP_PWM_DUTY_CYCLE 0.12 // controller will error if PWM duty cycle < 10%, so set min to 12% + +#define DOP_CONTROL_INTERVAL ( 500 / TASK_GENERAL_INTERVAL ) // interval (ms/task time) at which the dialOut pump is controlled +#define DOP_P_COEFFICIENT 0.0002 // P term for dialOut pump control +#define DOP_I_COEFFICIENT 0.00002 // I term for dialOut pump control +#define DOP_MAX_ERROR_SUM 10.0 // for anti-wind-up in I term +#define DOP_MIN_ERROR_SUM -10.0 +#define DOP_MAX_PWM_DC_DELTA 0.01 // prevents large steps in PWM duty cycle +#define DOP_MIN_PWM_DC_DELTA -0.01 + +#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 +#define DOP_MAX_CURR_WHEN_RUNNING_MA 1000.0 // motor controller current should not exceed this when pump should be running +#define DOP_MAX_CURR_ERROR_DURATION_MS 2000 // motor controller current errors persisting beyond this duration will trigger an alarm + +#define DOP_SPEED_ADC_TO_RPM_FACTOR 1.375 // conversion factor from ADC counts to RPM for dialOut pump motor +#define DOP_CURRENT_ADC_TO_MA_FACTOR 2.65 // conversion factor from ADC counts to mA for dialOut pump motor + +#define DOP_REV_PER_LITER 124.0 // rotor revolutions per liter +#define DOP_ML_PER_MIN_TO_PUMP_RPM_FACTOR ( DOP_REV_PER_LITER / ML_PER_LITER ) +#define DOP_GEAR_RATIO 32.0 // dialOut pump motor to dialOut pump gear ratio +#define DOP_MOTOR_RPM_TO_PWM_DC_FACTOR 0.0003717 // ~27 BP motor RPM = 1% PWM duty cycle +#define DOP_PWM_ZERO_OFFSET 0.1 // 10% PWM duty cycle = zero speed +#define DOP_PWM_FROM_ML_PER_MIN(rate) ( (rate) * DOP_ML_PER_MIN_TO_PUMP_RPM_FACTOR * DOP_GEAR_RATIO * DOP_MOTOR_RPM_TO_PWM_DC_FACTOR + DOP_PWM_ZERO_OFFSET ) + +#define DIAL_OUTPUMP_ADC_FULL_SCALE_V 3.0 // BP analog signals are 0-3V (while int. ADC ref V is 3.3V) +#define DIAL_OUTPUMP_ADC_MID_PT_BITS ( (F32)( INT_ADC_FULL_SCALE_BITS >> 1 ) * ( DIAL_OUTPUMP_ADC_FULL_SCALE_V / INT_ADC_REF_V ) ) +#define SIGN_FROM_12_BIT_VALUE(v) ( (S16)(v) - (S16)DIAL_OUTPUMP_ADC_MID_PT_BITS ) + +#define DIAL_OUT_FLOW_SAMPLE_FREQ ( MS_PER_SECOND / TASK_PRIORITY_INTERVAL ) +#define SIZE_OF_ROLLING_AVG (U32)( (F32)DIAL_OUT_FLOW_SAMPLE_FREQ * 0.8 ) // measured dialOut flow is filtered w/ moving average +#define MAX_FLOW_FILTER_INTERVAL 5 // slowest sample interval for filter is every 5th sample + +typedef enum DialOutPump_States +{ + DIAL_OUT_PUMP_OFF_STATE = 0, + DIAL_OUT_PUMP_RAMPING_UP_STATE, + DIAL_OUT_PUMP_RAMPING_DOWN_STATE, + DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE, + NUM_OF_DIAL_OUT_PUMP_STATES +} DIAL_OUT_PUMP_STATE_T; + +typedef enum DialOutUF_Self_Test_States +{ + DIAL_OUT_FLOW_SELF_TEST_STATE_START = 0, + DIAL_OUT_FLOW_TEST_STATE_IN_PROGRESS, + DIAL_OUT_FLOW_TEST_STATE_COMPLETE, + NUM_OF_DIAL_OUT_FLOW_SELF_TEST_STATES +} DIAL_OUT_FLOW_SELF_TEST_STATE_T; + +// CAN3 port pin assignments for pump stop and direction outputs +#define STOP_CAN3_PORT_MASK 0x00000002 // (Tx - re-purposed as output GPIO for dialOut pump stop signal) +#define DIR_CAN3_PORT_MASK 0x00000002 // (Rx - re-purposed as output GPIO for dialOut pump direction signal) +// dialOut pump stop and direction macros +#define SET_DOP_DIR() {canREG3->RIOC |= DIR_CAN3_PORT_MASK;} +#define SET_DOP_STOP() {canREG3->TIOC |= STOP_CAN3_PORT_MASK;} +#define CLR_DOP_DIR() {canREG3->RIOC &= ~DIR_CAN3_PORT_MASK;} +#define CLR_DOP_STOP() {canREG3->TIOC &= ~STOP_CAN3_PORT_MASK;} + +// ********** private data ********** + +static DIAL_OUT_PUMP_STATE_T dialOutPumpState = DIAL_OUT_PUMP_OFF_STATE; // current state of dialOut flow controller state machine +static U32 dialOutFlowDataPublicationTimerCounter = 0; // used to schedule dialOut flow data publication to CAN bus +static BOOL isDialOutPumpOn = FALSE; // dialOut pump is currently running +static F32 dialOutPumpPWMDutyCyclePct = 0.0; // initial dialOut pump PWM duty cycle +static F32 dialOutPumpPWMDutyCyclePctSet = 0.0; // currently set dialOut pump PWM duty cycle +static MOTOR_DIR_T dialOutPumpDirection = MOTOR_DIR_FORWARD; // requested dialOut flow direction +static MOTOR_DIR_T dialOutPumpDirectionSet = MOTOR_DIR_FORWARD; // currently set dialOut flow direction + +DATA_DECL( U32, DialOutUFDataPub, dialOutFlowDataPublishInterval, DIAL_OUT_FLOW_DATA_PUB_INTERVAL, DIAL_OUT_FLOW_DATA_PUB_INTERVAL ); // interval (in ms) at which to publish dialOut flow data to CAN bus +DATA_DECL( S32, TargetDialOutUFRate, targetDialOutUFRate, 0, 0 ); // requested dialOut flow rate +DATA_DECL( F32, MeasuredDialOutUFRate, measuredDialOutUFLoad, 0.0, 0.0 ); // measured dialOut flow rate +DATA_DECL( F32, MeasuredDialOutPumpRotorSpeed, dialOutPumpRotorSpeedRPM, 0.0, 0.0 );// measured dialOut pump rotor speed +DATA_DECL( F32, MeasuredDialOutPumpSpeed, dialOutPumpSpeedRPM, 0.0, 0.0 ); // measured dialOut pump motor speed +DATA_DECL( F32, MeasuredDialOutPumpMCSpeed, adcDialOutPumpMCSpeedRPM, 0.0, 0.0 ); // measured dialOut pump motor controller speed +DATA_DECL( F32, MeasuredDialOutPumpMCCurrent, adcDialOutPumpMCCurrentmA, 0.0, 0.0 );// measured dialOut pump motor controller current + +static U32 dopCurrErrorDurationCtr = 0; // used for tracking persistence of dop current errors + +static DIAL_OUT_FLOW_SELF_TEST_STATE_T dialOutPumpSelfTestState = DIAL_OUT_FLOW_SELF_TEST_STATE_START; // current dialOut pump self test state +static U32 dialOutPumpSelfTestTimerCount = 0; // timer counter for dialOut pump self test + +// ********** private function prototypes ********** + +static DIAL_OUT_PUMP_STATE_T handleDialOutPumpOffState( void ); +static DIAL_OUT_PUMP_STATE_T handleDialOutPumpRampingUpState( void ); +static DIAL_OUT_PUMP_STATE_T handleDialOutPumpRampingDownState( void ); +static DIAL_OUT_PUMP_STATE_T handleDialOutPumpControlToTargetState( void ); +static void stopDialOutPump( void ); +static void releaseDialOutPumpStop( void ); +static void setDialOutPumpDirection( MOTOR_DIR_T dir ); +static void publishDialOutUFData( void ); +static void checkDialOutPumpDirection( void ); +static void checkDialOutPumpMCCurrent( void ); +static DATA_GET_PROTOTYPE( U32, getPublishDialOutUFDataInterval ); + +/************************************************************************* + * @brief initDialOutUF + * The initDialOutUF function initializes the DialOutUF module. + * @details + * Inputs : none + * Outputs : DialOutUF module initialized. + * @param none + * @return none + *************************************************************************/ +void initDialOutUF( void ) +{ + stopDialOutPump(); + setDialOutPumpDirection( MOTOR_DIR_FORWARD ); +} + +/************************************************************************* + * @brief setDialOutPumpTargetFlowRate + * The setDialOutPumpTargetFlowRate function sets a new target flow rate and + * pump direction. + * @details + * Inputs : isDialOutPumpOn, dialOutPumpDirectionSet + * Outputs : targetDialOutUFRate, dialOutPumpdirection, dialOutPumpPWMDutyCyclePct + * @param flowRate : new target dialOut flow rate + * @param dir : new dialOut flow direction + * @return TRUE if new flow rate & dir are set, FALSE if not + *************************************************************************/ +BOOL setDialOutPumpTargetFlowRate( U32 flowRate, MOTOR_DIR_T dir ) +{ + BOOL result = FALSE; + + // direction change while pump is running is not allowed + if ( ( FALSE == isDialOutPumpOn ) || ( 0 == flowRate ) || ( dir == dialOutPumpDirectionSet ) ) + { + // verify flow rate + if ( flowRate <= MAX_DIAL_OUT_FLOW_RATE ) + { + targetDialOutUFRate.data = ( dir == MOTOR_DIR_FORWARD ? (S32)flowRate : (S32)flowRate * -1 ); + dialOutPumpDirection = dir; + // set PWM duty cycle target to an estimated initial target to ramp to based on target flow rate - then we'll control to flow when ramp completed + dialOutPumpPWMDutyCyclePct = DOP_PWM_FROM_ML_PER_MIN((F32)flowRate); // ~ 8% per 100 mL/min with a 10% zero offset added in (e.g. 100 mL/min = 8+10 = 18%) + + switch ( dialOutPumpState ) + { + case DIAL_OUT_PUMP_RAMPING_UP_STATE: // see if we need to reverse direction of ramp + if ( dialOutPumpPWMDutyCyclePct < dialOutPumpPWMDutyCyclePctSet ) + { + dialOutPumpState = DIAL_OUT_PUMP_RAMPING_DOWN_STATE; + } + break; + case DIAL_OUT_PUMP_RAMPING_DOWN_STATE: // see if we need to reverse direction of ramp + if ( dialOutPumpPWMDutyCyclePct > dialOutPumpPWMDutyCyclePctSet ) + { + dialOutPumpState = DIAL_OUT_PUMP_RAMPING_UP_STATE; + } + break; + case DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE: // start ramp to new target in appropriate direction + if ( dialOutPumpPWMDutyCyclePctSet > dialOutPumpPWMDutyCyclePct ) + { + dialOutPumpState = DIAL_OUT_PUMP_RAMPING_DOWN_STATE; + } + else + { + dialOutPumpState = DIAL_OUT_PUMP_RAMPING_UP_STATE; + } + break; + default: + // ok - not all states need to be handled here + break; + } + result = TRUE; + } + else // requested flow rate too high + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_DIAL_OUT_FLOW_SET_TOO_HIGH, flowRate ) + } + } + + return result; +} + +/************************************************************************* + * @brief signalDialOutPumpHardStop + * The signalDialOutPumpHardStop function stops the dialOut pump immediately. + * @details + * Inputs : none + * Outputs : DialOut pump stopped, set point reset, state changed to off + * @param none + * @return none + *************************************************************************/ +void signalDialOutPumpHardStop( void ) +{ + targetDialOutUFRate.data = 0; + stopDialOutPump(); + dialOutPumpState = DIAL_OUT_PUMP_OFF_STATE; + dialOutPumpPWMDutyCyclePct = 0.0; +} + +/************************************************************************* + * @brief execDialOutUFMonitor + * The execDialOutUFMonitor function executes the dialOut flow monitor. + * @details + * Inputs : none + * Outputs : measuredDialOutUFLoad, adcDialOutPumpMCSpeedRPM, adcDialOutPumpMCCurrentmA + * @param none + * @return none + *************************************************************************/ +void execDialOutUFMonitor( void ) +{ + U16 dopRPM = getIntADCReading( INT_ADC_DIAL_OUT_PUMP_SPEED ); + U16 dopmA = getIntADCReading( INT_ADC_DIAL_OUT_PUMP_MOTOR_CURRENT ); + F32 dopLoad = 0.0; // TODO - get load cell from FPGA (or dialin for now) + + adcDialOutPumpMCSpeedRPM.data = (F32)(SIGN_FROM_12_BIT_VALUE(dopRPM)) * DOP_SPEED_ADC_TO_RPM_FACTOR; + adcDialOutPumpMCCurrentmA.data = (F32)(SIGN_FROM_12_BIT_VALUE(dopmA)) * DOP_CURRENT_ADC_TO_MA_FACTOR; + + // don't start enforcing checks until out of init/POST mode + if ( getCurrentOperationMode() != MODE_INIT ) + { + checkDialOutPumpDirection(); + checkDialOutPumpMCCurrent(); + } + + // publish dialOut flow data on interval + publishDialOutUFData(); +} + +/************************************************************************* + * @brief execDialOutUFController + * The execDialOutUFController function executes the dialOut flow controller. + * @details + * Inputs : dialOutPumpState + * Outputs : dialOutPumpState + * @param none + * @return none + *************************************************************************/ +void execDialOutUFController( void ) +{ + switch ( dialOutPumpState ) + { + case DIAL_OUT_PUMP_OFF_STATE: + dialOutPumpState = handleDialOutPumpOffState(); + break; + + case DIAL_OUT_PUMP_RAMPING_UP_STATE: + dialOutPumpState = handleDialOutPumpRampingUpState(); + break; + + case DIAL_OUT_PUMP_RAMPING_DOWN_STATE: + dialOutPumpState = handleDialOutPumpRampingDownState(); + break; + + case DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE: + dialOutPumpState = handleDialOutPumpControlToTargetState(); + break; + + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_DIAL_OUT_FLOW_INVALID_DIAL_OUT_PUMP_STATE, dialOutPumpState ) + break; + } +} + +/************************************************************************* + * @brief handleDialOutPumpOffState + * The handleDialOutPumpOffState function handles the dialOut pump off state \n + * of the dialOut pump controller state machine. + * @details + * Inputs : targetDialOutUFRate, dialOutPumpDirection + * Outputs : dialOutPumpPWMDutyCyclePctSet, dialOutPumpDirectionSet, isDialOutPumpOn + * @param none + * @return next state + *************************************************************************/ +static DIAL_OUT_PUMP_STATE_T handleDialOutPumpOffState( void ) +{ + DIAL_OUT_PUMP_STATE_T result = DIAL_OUT_PUMP_OFF_STATE; + + // if we've been given a flow rate, setup ramp up and transition to ramp up state + if ( getTargetDialOutUFRate() != 0 ) + { + // set initial PWM duty cycle + dialOutPumpPWMDutyCyclePctSet = MAX_DIAL_OUT_PUMP_PWM_STEP_CHANGE; + etpwmSetCmpA( etpwmREG1, (U32)FLOAT_TO_INT_WITH_ROUND( dialOutPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) ); + // allow dialOut pump to run in requested direction + setDialOutPumpDirection( dialOutPumpDirection ); + releaseDialOutPumpStop(); + // start PWM for dialOut pump + etpwmStartTBCLK(); + isDialOutPumpOn = TRUE; + result = DIAL_OUT_PUMP_RAMPING_UP_STATE; + } + + return result; +} + +/************************************************************************* + * @brief handleDialOutPumpRampingUpState + * The handleDialOutPumpRampingUpState function handles the ramp up state \n + * of the dialOut pump controller state machine. + * @details + * Inputs : dialOutPumpPWMDutyCyclePctSet + * Outputs : dialOutPumpPWMDutyCyclePctSet + * @param none + * @return next state + *************************************************************************/ +static DIAL_OUT_PUMP_STATE_T handleDialOutPumpRampingUpState( void ) +{ + DIAL_OUT_PUMP_STATE_T result = DIAL_OUT_PUMP_RAMPING_UP_STATE; + + // have we been asked to stop the dialOut pump? + if ( 0 == getTargetDialOutUFRate() ) + { + // start ramp down to stop + dialOutPumpPWMDutyCyclePctSet -= MAX_DIAL_OUT_PUMP_PWM_STEP_CHANGE; + etpwmSetCmpA( etpwmREG1, (U32)FLOAT_TO_INT_WITH_ROUND( dialOutPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) ); + result = DIAL_OUT_PUMP_RAMPING_DOWN_STATE; + } + // have we reached end of ramp up? + else if ( dialOutPumpPWMDutyCyclePctSet >= dialOutPumpPWMDutyCyclePct ) + { + result = DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE; + } + // continue ramp up + else + { + dialOutPumpPWMDutyCyclePctSet += MAX_DIAL_OUT_PUMP_PWM_STEP_CHANGE; + etpwmSetCmpA( etpwmREG1, (U32)FLOAT_TO_INT_WITH_ROUND( dialOutPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) ); + } + + return result; +} + +/************************************************************************* + * @brief handleDialOutPumpRampingDownState + * The handleDialOutPumpRampingDownState function handles the ramp down state \n + * of the dialOut pump controller state machine. + * @details + * Inputs : dialOutPumpPWMDutyCyclePctSet + * Outputs : dialOutPumpPWMDutyCyclePctSet + * @param none + * @return next state + *************************************************************************/ +static DIAL_OUT_PUMP_STATE_T handleDialOutPumpRampingDownState( void ) +{ + DIAL_OUT_PUMP_STATE_T result = DIAL_OUT_PUMP_RAMPING_DOWN_STATE; + + // have we essentially reached zero speed + if ( dialOutPumpPWMDutyCyclePctSet < (MAX_DIAL_OUT_PUMP_PWM_STEP_CHANGE + DOP_PWM_ZERO_OFFSET) ) + { + stopDialOutPump(); + result = DIAL_OUT_PUMP_OFF_STATE; + } + // have we reached end of ramp down? + else if ( dialOutPumpPWMDutyCyclePctSet <= dialOutPumpPWMDutyCyclePct ) + { + result = DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE; + } + // continue ramp down + else + { + dialOutPumpPWMDutyCyclePctSet -= MAX_DIAL_OUT_PUMP_PWM_STEP_CHANGE; + etpwmSetCmpA( etpwmREG1, (U32)FLOAT_TO_INT_WITH_ROUND( dialOutPumpPWMDutyCyclePctSet * (F32)(etpwmREG1->TBPRD) ) ); + } + + return result; +} + +/************************************************************************* + * @brief handleDialOutPumpControlToTargetState + * The handleDialOutPumpControlToTargetState function handles the "control to \n + * target" state of the dialOut pump controller state machine. + * @details + * Inputs : none + * Outputs : dialOutPumpState + * @param none + * @return next state + *************************************************************************/ +static DIAL_OUT_PUMP_STATE_T handleDialOutPumpControlToTargetState( void ) +{ + DIAL_OUT_PUMP_STATE_T result = DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE; + + // TODO - control algorithm + + return result; +} + +/************************************************************************* + * @brief stopDialOutPump + * The stopDialOutPump function sets the dialOut pump stop signal. + * @details + * Inputs : none + * Outputs : dialOut pump stop signal activated, PWM duty cycle zeroed + * @param none + * @return none + *************************************************************************/ +static void stopDialOutPump( void ) +{ + isDialOutPumpOn = FALSE; + dialOutPumpPWMDutyCyclePctSet = 0.0; + etpwmSetCmpA( etpwmREG1, 0 ); + etpwmStopTBCLK(); + SET_DOP_STOP(); +} + +/************************************************************************* + * @brief releaseDialOutPumpStop + * The releaseDialOutPumpStop function clears the dialOut pump stop signal. + * @details + * Inputs : none + * Outputs : dialOut pump stop signal + * @param none + * @return none + *************************************************************************/ +static void releaseDialOutPumpStop( void ) +{ + CLR_DOP_STOP(); +} + +/************************************************************************* + * @brief setDialOutPumpDirection + * The setDialOutPumpDirection function sets the set dialOut pump direction to \n + * the given direction. + * @details + * Inputs : dialOutPumpState + * Outputs : dialOutPumpState + * @param dir : dialOut pump direction to set + * @return none + *************************************************************************/ +static void setDialOutPumpDirection( MOTOR_DIR_T dir ) +{ + switch ( dir ) + { + case MOTOR_DIR_FORWARD: + dialOutPumpDirectionSet = dir; + SET_DOP_DIR(); + break; + + case MOTOR_DIR_REVERSE: + dialOutPumpDirectionSet = dir; + CLR_DOP_DIR(); + break; + + default: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_DIAL_OUT_FLOW_INVALID_DIAL_OUT_PUMP_DIRECTION, dir ) + break; + } +} + +/************************************************************************* + * @brief getPublishDialOutUFDataInterval + * The getPublishDialOutUFDataInterval function gets the dialOut flow data \n + * publication interval. + * @details + * Inputs : dialOutFlowDataPublishInterval + * Outputs : none + * @param none + * @return the current dialOut flow data publication interval (in ms). + *************************************************************************/ +DATA_GET( U32, getPublishDialOutUFDataInterval, dialOutFlowDataPublishInterval ) + +/************************************************************************* + * @brief getTargetDialOutUFRate + * The getTargetDialOutUFRate function gets the current target dialOut flow \n + * rate. + * @details + * Inputs : targetDialOutUFRate + * Outputs : none + * @param none + * @return the current target dialOut flow rate (in mL/min). + *************************************************************************/ +DATA_GET( S32, getTargetDialOutUFRate, targetDialOutUFRate ) + +/************************************************************************* + * @brief getMeasuredDialOutUFLoad + * The getMeasuredDialOutUFLoad function gets the measured dialOut flow \n + * rate. + * @details + * Inputs : measuredDialOutUFLoad + * Outputs : none + * @param none + * @return the current dialOut flow rate (in mL/min). + *************************************************************************/ +DATA_GET( F32, getMeasuredDialOutUFLoad, measuredDialOutUFLoad ) + +/************************************************************************* + * @brief getMeasuredDialOutPumpRotorSpeed + * The getMeasuredDialOutPumpRotorSpeed function gets the measured dialOut flow \n + * rate. + * @details + * Inputs : dialOutPumpRotorSpeedRPM + * Outputs : none + * @param none + * @return the current dialOut flow rate (in mL/min). + *************************************************************************/ +DATA_GET( F32, getMeasuredDialOutPumpRotorSpeed, dialOutPumpRotorSpeedRPM ) + +/************************************************************************* + * @brief getMeasuredDialOutPumpSpeed + * The getMeasuredDialOutPumpSpeed function gets the measured dialOut flow \n + * rate. + * @details + * Inputs : dialOutPumpSpeedRPM + * Outputs : none + * @param none + * @return the current dialOut flow rate (in mL/min). + *************************************************************************/ +DATA_GET( F32, getMeasuredDialOutPumpSpeed, dialOutPumpSpeedRPM ) + +/************************************************************************* + * @brief getMeasuredDialOutPumpMCSpeed + * The getMeasuredDialOutPumpMCSpeed function gets the measured dialOut pump \n + * speed. + * @details + * Inputs : adcDialOutPumpMCSpeedRPM + * Outputs : none + * @param none + * @return the current dialOut pump speed (in RPM). + *************************************************************************/ +DATA_GET( F32, getMeasuredDialOutPumpMCSpeed, adcDialOutPumpMCSpeedRPM ) + +/************************************************************************* + * @brief getMeasuredDialOutPumpMCCurrent + * The getMeasuredDialOutPumpMCCurrent function gets the measured dialOut pump \n + * current. + * @details + * Inputs : adcDialOutPumpMCCurrentmA + * Outputs : none + * @param none + * @return the current dialOut pump current (in mA). + *************************************************************************/ +DATA_GET( F32, getMeasuredDialOutPumpMCCurrent, adcDialOutPumpMCCurrentmA ) + +/************************************************************************* + * @brief publishDialOutUFData + * The publishDialOutUFData function publishes dialOut flow data at the set \n + * interval. + * @details + * Inputs : target flow rate, measured flow rate, measured MC speed, \n + * measured MC current + * Outputs : DialOut flow data is published to CAN bus. + * @param none + * @return none + *************************************************************************/ +static void publishDialOutUFData( void ) +{ + // publish dialOut flow data on interval + if ( ++dialOutFlowDataPublicationTimerCounter > getPublishDialOutUFDataInterval() ) + { + S32 flowStPt = (S32)getTargetDialOutUFRate(); + F32 measLoad = getMeasuredDialOutUFLoad(); + F32 measRotSpd = getMeasuredDialOutPumpRotorSpeed(); + F32 measSpd = getMeasuredDialOutPumpSpeed(); + F32 measMCSpd = getMeasuredDialOutPumpMCSpeed(); + F32 measMCCurr = getMeasuredDialOutPumpMCCurrent(); +#ifdef DEBUG_ENABLED + // TODO - temporary debug code - remove later + S32 pwm = (S32)( 100.0 * dialOutPumpPWMDutyCyclePctSet ); + char debugStr[ 256 ]; + + sprintf( debugStr, "Target Flow:%5d, Meas. Flow:%5d, Speed:%5d RPM, Current:%5d mA, PWM:%5d \n", flowStPt, (S32)measLoad, (S32)measMCSpd, (S32)measMCCurr, pwm ); + sendDebugData( (U08*)debugStr, strlen(debugStr) ); +#endif + // TODO - broadcastDialOutUFData( flowStPt, measLoad, measRotSpd, measSpd, measMCSpd, measMCCurr ); + dialOutFlowDataPublicationTimerCounter = 0; + } +} + +/************************************************************************* + * @brief checkDialOutPumpDirection + * The checkDialOutPumpDirection function checks the set direction vs. \n + * the direction implied by the sign of the measured MC speed. + * @details + * Inputs : + * Outputs : + * @param none + * @return none + *************************************************************************/ +static void checkDialOutPumpDirection( void ) +{ + MOTOR_DIR_T dopMCDir; + + if ( DIAL_OUT_PUMP_CONTROL_TO_TARGET_STATE == dialOutPumpState ) + { + // check set direction vs. direction from sign of motor controller speed + dopMCDir = ( getMeasuredDialOutPumpMCSpeed() >= 0.0 ? MOTOR_DIR_FORWARD : MOTOR_DIR_REVERSE ); + if ( dialOutPumpDirectionSet != dopMCDir ) + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DIAL_OUT_PUMP_MC_DIRECTION_CHECK, (U32)dialOutPumpDirectionSet, (U32)dopMCDir ) + } + } +} + +/************************************************************************* + * @brief checkDialOutPumpMCCurrent + * The checkDialOutPumpMCCurrent function checks the measured MC current vs. \n + * the set state of the dialOut pump (stopped or running). + * @details + * Inputs : + * Outputs : + * @param none + * @return none + *************************************************************************/ +static void checkDialOutPumpMCCurrent( void ) +{ + F32 dopCurr; + + // dialOut pump should be off + if ( DIAL_OUT_PUMP_OFF_STATE == dialOutPumpState ) + { + dopCurr = fabs( getMeasuredDialOutPumpMCCurrent() ); + if ( dopCurr > DOP_MAX_CURR_WHEN_STOPPED_MA ) + { + dopCurrErrorDurationCtr += TASK_PRIORITY_INTERVAL; + if ( dopCurrErrorDurationCtr > DOP_MAX_CURR_ERROR_DURATION_MS ) + { + SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DIAL_OUT_PUMP_MC_CURRENT_CHECK, getMeasuredDialOutPumpMCCurrent() ); + } + } + else + { + dopCurrErrorDurationCtr = 0; + } + } + // dialOut pump should be running + else + { + dopCurr = fabs( getMeasuredDialOutPumpMCCurrent() ); + if ( ( dopCurr < DOP_MIN_CURR_WHEN_RUNNING_MA ) || ( dopCurr > DOP_MAX_CURR_WHEN_RUNNING_MA ) ) + { + dopCurrErrorDurationCtr += TASK_PRIORITY_INTERVAL; + if ( dopCurrErrorDurationCtr > DOP_MAX_CURR_ERROR_DURATION_MS ) + { + SET_ALARM_WITH_1_F32_DATA( ALARM_ID_DIAL_OUT_PUMP_MC_CURRENT_CHECK, getMeasuredDialOutPumpMCCurrent() ); + } + } + else + { + dopCurrErrorDurationCtr = 0; + } + } +} + +/************************************************************************* + * @brief execDialOutUFTest + * The execDialOutUFTest function executes the state machine for the \n + * DialOutUF self test. + * @details + * Inputs : none + * Outputs : none + * @param none + * @return the current state of the DialOutUF self test. + *************************************************************************/ +SELF_TEST_STATUS_T execDialOutUFTest( void ) +{ + SELF_TEST_STATUS_T result = SELF_TEST_STATUS_FAILED; + + // TODO - implement self test(s) + + return result; +} + + +/************************************************************************* + * TEST SUPPORT FUNCTIONS + *************************************************************************/ + + +/************************************************************************* + * @brief testSetDialOutUFDataPublishIntervalOverride + * The testSetDialOutUFDataPublishIntervalOverride function overrides the \n + * dialOut flow data publish interval. + * @details + * Inputs : none + * Outputs : dialOutFlowDataPublishInterval + * @param value : override dialOut flow data publish interval with (in ms) + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +BOOL testSetDialOutUFDataPublishIntervalOverride( U32 value ) +{ + BOOL result = FALSE; + + if ( TRUE == isTestingActivated() ) + { + U32 intvl = value / TASK_PRIORITY_INTERVAL; + + result = TRUE; + dialOutFlowDataPublishInterval.ovData = intvl; + dialOutFlowDataPublishInterval.override = OVERRIDE_KEY; + } + + return result; +} + +/************************************************************************* + * @brief testResetDialOutUFDataPublishIntervalOverride + * The testResetDialOutUFDataPublishIntervalOverride function resets the override \n + * of the dialOut flow data publish interval. + * @details + * Inputs : none + * Outputs : dialOutFlowDataPublishInterval + * @return TRUE if override reset successful, FALSE if not + *************************************************************************/ +BOOL testResetDialOutUFDataPublishIntervalOverride( void ) +{ + BOOL result = FALSE; + + if ( TRUE == isTestingActivated() ) + { + result = TRUE; + dialOutFlowDataPublishInterval.override = OVERRIDE_RESET; + dialOutFlowDataPublishInterval.ovData = dialOutFlowDataPublishInterval.ovInitData; + } + + return result; +} + +/************************************************************************* + * @brief testSetTargetDialOutUFRateOverride and testResetTargetDialOutUFRateOverride + * The testSetTargetDialOutUFRateOverride function overrides the target \n + * dialOut flow rate. \n + * The testResetTargetDialOutUFRateOverride function resets the override of the \n + * target dialOut flow rate. + * @details + * Inputs : none + * Outputs : targetDialOutUFRate + * @param value : override target dialOut flow rate (in mL/min) + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +DATA_OVERRIDE_FUNC( S32, testSetTargetDialOutUFRateOverride, testResetTargetDialOutUFRateOverride, targetDialOutUFRate ) + +/************************************************************************* + * @brief testSetMeasuredDialOutUFLoadOverride and testResetMeasuredDialOutUFLoadOverride + * The testSetMeasuredDialOutUFLoadOverride function overrides the measured \n + * dialOut load. \n + * The testResetMeasuredDialOutUFLoadOverride function resets the override of the \n + * measured dialOut load. + * @details + * Inputs : none + * Outputs : measuredDialOutUFLoad + * @param value : override measured dialOut flow rate (in mL/min) + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +DATA_OVERRIDE_FUNC( F32, testSetMeasuredDialOutUFLoadOverride, testResetMeasuredDialOutUFLoadOverride, measuredDialOutUFLoad ) + +/************************************************************************* + * @brief testSetMeasuredDialOutPumpRotorSpeedOverride and testResetMeasuredDialOutPumpRotorSpeedOverride + * The testSetMeasuredDialOutPumpRotorSpeedOverride function overrides the measured \n + * dialOut pump rotor speed. \n + * The testResetMeasuredDialOutPumpRotorSpeedOverride function resets the override of the \n + * measured dialOut pump rotor speed. + * @details + * Inputs : none + * Outputs : dialOutPumpRotorSpeedRPM + * @param value : override measured dialOut pump rotor speed (in RPM) + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +DATA_OVERRIDE_FUNC( F32, testSetMeasuredDialOutPumpRotorSpeedOverride, testResetMeasuredDialOutPumpRotorSpeedOverride, dialOutPumpRotorSpeedRPM ) + +/************************************************************************* + * @brief testSetMeasuredDialOutPumpSpeedOverride and testResetMeasuredDialOutPumpSpeedOverride + * The testSetMeasuredDialOutPumpSpeedOverride function overrides the measured \n + * dialOut pump motor speed. \n + * The testResetMeasuredDialOutPumpSpeedOverride function resets the override of the \n + * measured dialOut pump motor speed. + * @details + * Inputs : none + * Outputs : dialOutPumpSpeedRPM + * @param value : override measured dialOut pump motor speed (in RPM) + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +DATA_OVERRIDE_FUNC( F32, testSetMeasuredDialOutPumpSpeedOverride, testResetMeasuredDialOutPumpSpeedOverride, dialOutPumpSpeedRPM ) + +/************************************************************************* + * @brief testSetMeasuredDialOutPumpMCSpeedOverride and testResetMeasuredDialOutPumpMCSpeedOverride + * The testSetMeasuredDialOutPumpMCSpeedOverride function overrides the measured \n + * dialOut pump motor speed. \n + * The testResetMeasuredDialOutPumpMCSpeedOverride function resets the override of the \n + * measured dialOut pump motor speed. + * @details + * Inputs : none + * Outputs : adcDialOutPumpMCSpeedRPM + * @param value : override measured dialOut pump speed (in RPM) + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +DATA_OVERRIDE_FUNC( F32, testSetMeasuredDialOutPumpMCSpeedOverride, testResetMeasuredDialOutPumpMCSpeedOverride, adcDialOutPumpMCSpeedRPM ) + +/************************************************************************* + * @brief testSetMeasuredDialOutPumpMCCurrentOverride and testResetMeasuredDialOutPumpMCCurrentOverride + * The testSetMeasuredDialOutPumpMCCurrentOverride function overrides the measured \n + * dialOut pump motor current. \n + * The testResetMeasuredDialOutPumpMCCurrentOverride function resets the override of the \n + * measured dialOut pump motor current. + * @details + * Inputs : none + * Outputs : adcDialOutPumpMCCurrentmA + * @param value : override measured dialOut pump current (in mA) + * @return TRUE if override successful, FALSE if not + *************************************************************************/ +DATA_OVERRIDE_FUNC( F32, testSetMeasuredDialOutPumpMCCurrentOverride, testResetMeasuredDialOutPumpMCCurrentOverride, adcDialOutPumpMCCurrentmA ) + + Index: firmware/App/Controllers/DialOutUF.h =================================================================== diff -u --- firmware/App/Controllers/DialOutUF.h (revision 0) +++ firmware/App/Controllers/DialOutUF.h (revision 96bff7c1a8c755eb28a89e131a737dc1c4a7d9fc) @@ -0,0 +1,58 @@ +/************************************************************************** + * + * Copyright (c) 2019-2019 Diality Inc. - All Rights Reserved. + * + * THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN + * WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER. + * + * @file DialOutUF.h + * + * @date 16-Dec-2019 + * @author S. Nash & L. Baloa + * + * @brief DialOutUF header file. + * + **************************************************************************/ + +#ifndef __DIALYSATE_OUT_UF_H__ +#define __DIALYSATE_OUT_UF_H__ + +#include "Common.h" + +// ********** public definitions ********** + + +// ********** public function prototypes ********** + +void initDialOutUF( void ); +void execDialOutUFMonitor( void ); +void execDialOutUFController( void ); + +BOOL setDialOutPumpTargetFlowRate( U32 flowRate, MOTOR_DIR_T dir ); +void signalDialOutPumpHardStop( void ); + +SELF_TEST_STATUS_T execDialOutUFTest( void ); + +DATA_GET_PROTOTYPE( S32, getTargetDialOutUFRate ); +DATA_GET_PROTOTYPE( F32, getMeasuredDialOutUFLoad); +DATA_GET_PROTOTYPE( F32, getMeasuredDialOutPumpRotorSpeed ); +DATA_GET_PROTOTYPE( F32, getMeasuredDialOutPumpSpeed ); +DATA_GET_PROTOTYPE( F32, getMeasuredDialOutPumpMCSpeed ); +DATA_GET_PROTOTYPE( F32, getMeasuredDialOutPumpMCCurrent ); + +BOOL testSetDialOutUFDataPublishIntervalOverride( U32 value ); +BOOL testResetDialOutUFDataPublishIntervalOverride( void ); +BOOL testSetTargetDialOutUFRateOverride( S32 value ); +BOOL testResetTargetDialOutUFRateOverride( void ); +BOOL testSetMeasuredDialOutUFLoadOverride( F32 value ); +BOOL testResetMeasuredDialOutUFLoadOverride( void ); +BOOL testSetMeasuredDialOutPumpRotorSpeedOverride( F32 value ); +BOOL testResetMeasuredDialOutPumpRotorSpeedOverride( void ); +BOOL testSetMeasuredDialOutPumpSpeedOverride( F32 value ); +BOOL testResetMeasuredDialOutPumpSpeedOverride( void ); +BOOL testSetMeasuredDialOutPumpMCSpeedOverride( F32 value ); +BOOL testResetMeasuredDialOutPumpMCSpeedOverride( void ); +BOOL testSetMeasuredDialOutPumpMCCurrentOverride( F32 value ); +BOOL testResetMeasuredDialOutPumpMCCurrentOverride( void ); + +#endif Index: firmware/App/Services/AlarmMgmt.c =================================================================== diff -u -ref0b3f0ec00fadc50f95e0db1a6477fb4b076ea1 -r96bff7c1a8c755eb28a89e131a737dc1c4a7d9fc --- firmware/App/Services/AlarmMgmt.c (.../AlarmMgmt.c) (revision ef0b3f0ec00fadc50f95e0db1a6477fb4b076ea1) +++ firmware/App/Services/AlarmMgmt.c (.../AlarmMgmt.c) (revision 96bff7c1a8c755eb28a89e131a737dc1c4a7d9fc) @@ -55,7 +55,15 @@ { ALARM_PRIORITY_MEDIUM, 0, ALARM_ID_NO_ALARM, FALSE, TRUE , FALSE, FALSE, FALSE, FALSE, FALSE, FALSE }, // ALARM_ID_BLOOD_PUMP_MC_CURRENT_CHECK { ALARM_PRIORITY_MEDIUM, 0, ALARM_ID_NO_ALARM, FALSE, TRUE , FALSE, FALSE, FALSE, FALSE, FALSE, FALSE }, // ALARM_ID_BLOOD_PUMP_MC_SPEED_CHECK { ALARM_PRIORITY_MEDIUM, 0, ALARM_ID_NO_ALARM, FALSE, TRUE , FALSE, FALSE, FALSE, FALSE, FALSE, FALSE }, // ALARM_ID_BLOOD_PUMP_MC_DIRECTION_CHECK - { ALARM_PRIORITY_HIGH, 0, ALARM_ID_NO_ALARM, FALSE, TRUE , FALSE, FALSE, FALSE, FALSE, FALSE, FALSE } // ALARM_ID_BLOOD_PUMP_ROTOR_SPEED_CHECK + { ALARM_PRIORITY_HIGH, 0, ALARM_ID_NO_ALARM, FALSE, TRUE , FALSE, FALSE, FALSE, FALSE, FALSE, FALSE }, // ALARM_ID_BLOOD_PUMP_ROTOR_SPEED_CHECK + { ALARM_PRIORITY_MEDIUM, 0, ALARM_ID_NO_ALARM, FALSE, TRUE , FALSE, FALSE, FALSE, FALSE, FALSE, FALSE }, // ALARM_ID_DIAL_IN_PUMP_MC_CURRENT_CHECK + { ALARM_PRIORITY_MEDIUM, 0, ALARM_ID_NO_ALARM, FALSE, TRUE , FALSE, FALSE, FALSE, FALSE, FALSE, FALSE }, // ALARM_ID_DIAL_IN_PUMP_MC_SPEED_CHECK + { ALARM_PRIORITY_MEDIUM, 0, ALARM_ID_NO_ALARM, FALSE, TRUE , FALSE, FALSE, FALSE, FALSE, FALSE, FALSE }, // ALARM_ID_DIAL_IN_PUMP_MC_DIRECTION_CHECK + { ALARM_PRIORITY_HIGH, 0, ALARM_ID_NO_ALARM, FALSE, TRUE , FALSE, FALSE, FALSE, FALSE, FALSE, FALSE }, // ALARM_ID_DIAL_IN_PUMP_ROTOR_SPEED_CHECK + { ALARM_PRIORITY_MEDIUM, 0, ALARM_ID_NO_ALARM, FALSE, TRUE , FALSE, FALSE, FALSE, FALSE, FALSE, FALSE }, // ALARM_ID_DIAL_OUT_PUMP_MC_CURRENT_CHECK + { ALARM_PRIORITY_MEDIUM, 0, ALARM_ID_NO_ALARM, FALSE, TRUE , FALSE, FALSE, FALSE, FALSE, FALSE, FALSE }, // ALARM_ID_DIAL_OUT_PUMP_MC_SPEED_CHECK + { ALARM_PRIORITY_MEDIUM, 0, ALARM_ID_NO_ALARM, FALSE, TRUE , FALSE, FALSE, FALSE, FALSE, FALSE, FALSE }, // ALARM_ID_DIAL_OUT_PUMP_MC_DIRECTION_CHECK + { ALARM_PRIORITY_HIGH, 0, ALARM_ID_NO_ALARM, FALSE, TRUE , FALSE, FALSE, FALSE, FALSE, FALSE, FALSE } // ALARM_ID_DIAL_OUT_PUMP_ROTOR_SPEED_CHECK }; const ALARM_DATA_T blankAlarmData = { ALARM_DATA_TYPE_NONE, 0 }; Index: firmware/App/Services/AlarmMgmt.h =================================================================== diff -u -r620104f4a9e3148575703981a3063b9605b6e9b8 -r96bff7c1a8c755eb28a89e131a737dc1c4a7d9fc --- firmware/App/Services/AlarmMgmt.h (.../AlarmMgmt.h) (revision 620104f4a9e3148575703981a3063b9605b6e9b8) +++ firmware/App/Services/AlarmMgmt.h (.../AlarmMgmt.h) (revision 96bff7c1a8c755eb28a89e131a737dc1c4a7d9fc) @@ -31,6 +31,14 @@ ALARM_ID_BLOOD_PUMP_MC_SPEED_CHECK, ALARM_ID_BLOOD_PUMP_MC_DIRECTION_CHECK, ALARM_ID_BLOOD_PUMP_ROTOR_SPEED_CHECK, + ALARM_ID_DIAL_IN_PUMP_MC_CURRENT_CHECK, // 10 + ALARM_ID_DIAL_IN_PUMP_MC_SPEED_CHECK, + ALARM_ID_DIAL_IN_PUMP_MC_DIRECTION_CHECK, + ALARM_ID_DIAL_IN_PUMP_ROTOR_SPEED_CHECK, + ALARM_ID_DIAL_OUT_PUMP_MC_CURRENT_CHECK, + ALARM_ID_DIAL_OUT_PUMP_MC_SPEED_CHECK, // 15 + ALARM_ID_DIAL_OUT_PUMP_MC_DIRECTION_CHECK, + ALARM_ID_DIAL_OUT_PUMP_ROTOR_SPEED_CHECK, NUM_OF_ALARM_IDS } ALARM_ID_T; @@ -147,6 +155,12 @@ SW_FAULT_ID_MSG_QUEUES_IS_FULL_INVALID_QUEUE, // 35 SW_FAULT_ID_WATCHDOG_INVALID_SELF_TEST_STATE, SW_FAULT_ID_ALARM_MGMT_INVALID_FIFO_TO_RESET, + SW_FAULT_ID_DIAL_IN_FLOW_INVALID_DIAL_IN_PUMP_DIRECTION, + SW_FAULT_ID_DIAL_IN_FLOW_INVALID_DIAL_IN_PUMP_STATE, + SW_FAULT_ID_DIAL_IN_FLOW_SET_TOO_HIGH, // 40 + SW_FAULT_ID_DIAL_OUT_FLOW_INVALID_DIAL_OUT_PUMP_DIRECTION, + SW_FAULT_ID_DIAL_OUT_FLOW_INVALID_DIAL_OUT_PUMP_STATE, + SW_FAULT_ID_DIAL_OUT_FLOW_SET_TOO_HIGH, NUM_OF_SW_FAULT_IDS } SW_FAULT_ID_T;