Index: firmware/App/Common.h =================================================================== diff -u -r6311eb9b65fdeec7a285d25e07f3932ac0fb6cf1 -r5217f70ca5c74bd586dc14540e6404b43feea004 --- firmware/App/Common.h (.../Common.h) (revision 6311eb9b65fdeec7a285d25e07f3932ac0fb6cf1) +++ firmware/App/Common.h (.../Common.h) (revision 5217f70ca5c74bd586dc14540e6404b43feea004) @@ -24,7 +24,7 @@ #ifndef _VECTORCAST_ // #define RM46_EVAL_BOARD_TARGET 1 // #define SIMULATE_UI 1 - #define DEBUG_ENABLED 1 +// #define DEBUG_ENABLED 1 #ifdef DEBUG_ENABLED #include @@ -77,7 +77,7 @@ // **** Common Definitions **** -#define NEARLY_ZERO 0.00001 +#define NEARLY_ZERO 0.00000001 #define MASK_OFF_MSB 0x00FF #define MASK_OFF_LSB 0xFF00 #define MASK_OFF_MSW 0x0000FFFF Index: firmware/App/Controllers/BloodFlow.c =================================================================== diff -u -r22f7bcd7e26dc6be337fdb6adbc9281e8f60dbbc -r5217f70ca5c74bd586dc14540e6404b43feea004 --- firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 22f7bcd7e26dc6be337fdb6adbc9281e8f60dbbc) +++ firmware/App/Controllers/BloodFlow.c (.../BloodFlow.c) (revision 5217f70ca5c74bd586dc14540e6404b43feea004) @@ -46,8 +46,6 @@ #define BP_CONTROL_INTERVAL ( 500 / TASK_GENERAL_INTERVAL ) // interval (ms/task time) at which the blood pump is controlled #define BP_P_COEFFICIENT 0.0002 // P term for blood pump control #define BP_I_COEFFICIENT 0.00002 // I term for blood pump control -#define BP_MAX_ERROR_SUM 10.0 // for anti-wind-up in I term -#define BP_MIN_ERROR_SUM -10.0 #define BP_MAX_PWM_DC_DELTA 0.01 // prevents large steps in PWM duty cycle while controlling #define BP_MIN_PWM_DC_DELTA -0.01 @@ -167,8 +165,7 @@ // initialize blood flow PI controller initializePIController( PI_CONTROLLER_ID_BLOOD_FLOW, MIN_BLOOD_PUMP_PWM_DUTY_CYCLE, BP_P_COEFFICIENT, BP_I_COEFFICIENT, - MIN_BLOOD_PUMP_PWM_DUTY_CYCLE, MAX_BLOOD_PUMP_PWM_DUTY_CYCLE, - BP_MIN_ERROR_SUM, BP_MAX_ERROR_SUM ); + MIN_BLOOD_PUMP_PWM_DUTY_CYCLE, MAX_BLOOD_PUMP_PWM_DUTY_CYCLE ); } /************************************************************************* Index: firmware/App/Controllers/DialInFlow.c =================================================================== diff -u -r22f7bcd7e26dc6be337fdb6adbc9281e8f60dbbc -r5217f70ca5c74bd586dc14540e6404b43feea004 --- firmware/App/Controllers/DialInFlow.c (.../DialInFlow.c) (revision 22f7bcd7e26dc6be337fdb6adbc9281e8f60dbbc) +++ firmware/App/Controllers/DialInFlow.c (.../DialInFlow.c) (revision 5217f70ca5c74bd586dc14540e6404b43feea004) @@ -47,8 +47,6 @@ #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 @@ -168,8 +166,7 @@ // initialize dialysate inlet flow PI controller initializePIController( PI_CONTROLLER_ID_DIALYSATE_FLOW, MIN_DIAL_IN_PUMP_PWM_DUTY_CYCLE, DIP_P_COEFFICIENT, DIP_I_COEFFICIENT, - MIN_DIAL_IN_PUMP_PWM_DUTY_CYCLE, MAX_DIAL_IN_PUMP_PWM_DUTY_CYCLE, - DIP_MIN_ERROR_SUM, DIP_MAX_ERROR_SUM ); + MIN_DIAL_IN_PUMP_PWM_DUTY_CYCLE, MAX_DIAL_IN_PUMP_PWM_DUTY_CYCLE ); } /************************************************************************* Index: firmware/App/Services/AlarmMgmt.h =================================================================== diff -u -r22f7bcd7e26dc6be337fdb6adbc9281e8f60dbbc -r5217f70ca5c74bd586dc14540e6404b43feea004 --- firmware/App/Services/AlarmMgmt.h (.../AlarmMgmt.h) (revision 22f7bcd7e26dc6be337fdb6adbc9281e8f60dbbc) +++ firmware/App/Services/AlarmMgmt.h (.../AlarmMgmt.h) (revision 5217f70ca5c74bd586dc14540e6404b43feea004) @@ -40,6 +40,8 @@ ALARM_ID_DIAL_OUT_PUMP_MC_DIRECTION_CHECK, ALARM_ID_DIAL_OUT_PUMP_ROTOR_SPEED_CHECK, ALARM_ID_WATCHDOG_EXPIRED, + ALARM_ID_RTC_COMM_ERROR, + ALARM_ID_RTC_CONFIG_ERROR, // 20 NUM_OF_ALARM_IDS } ALARM_ID_T; @@ -139,32 +141,33 @@ SW_FAULT_ID_ALARM_MGMT_LAMP_INVALID_ALARM_STATE, SW_FAULT_ID_COMM_BUFFERS_ADD_TOO_MUCH_DATA, SW_FAULT_ID_COMM_BUFFERS_ADD_INVALID_BUFFER, - SW_FAULT_ID_COMM_BUFFERS_GET_TOO_MUCH_DATA, // 20 - SW_FAULT_ID_COMM_BUFFERS_GET_INVALID_BUFFER, - SW_FAULT_ID_COMM_BUFFERS_PEEK_TOO_MUCH_DATA, + SW_FAULT_ID_COMM_BUFFERS_GET_INVALID_BUFFER, // 20 SW_FAULT_ID_COMM_BUFFERS_PEEK_INVALID_BUFFER, SW_FAULT_ID_COMM_BUFFERS_COUNT_INVALID_BUFFER, - SW_FAULT_ID_FPGA_INVALID_IN_STATE, // 25 + SW_FAULT_ID_FPGA_INVALID_IN_STATE, SW_FAULT_ID_FPGA_INVALID_OUT_STATE, - SW_FAULT_ID_FPGA_WRITE_CMD_TOO_MUCH_DATA, + SW_FAULT_ID_FPGA_WRITE_CMD_TOO_MUCH_DATA, // 25 SW_FAULT_ID_FPGA_WRITE_RSP_TOO_MUCH_DATA, SW_FAULT_ID_FPGA_READ_CMD_TOO_MUCH_DATA, - SW_FAULT_ID_FPGA_READ_RSP_TOO_MUCH_DATA, // 30 + SW_FAULT_ID_FPGA_READ_RSP_TOO_MUCH_DATA, SW_FAULT_ID_MSG_QUEUES_ADD_QUEUE_FULL, - SW_FAULT_ID_MSG_QUEUES_ADD_INVALID_QUEUE, + SW_FAULT_ID_MSG_QUEUES_ADD_INVALID_QUEUE, // 30 SW_FAULT_ID_MSG_QUEUES_GET_INVALID_QUEUE, SW_FAULT_ID_MSG_QUEUES_IS_EMPTY_INVALID_QUEUE, - SW_FAULT_ID_MSG_QUEUES_IS_FULL_INVALID_QUEUE, // 35 + SW_FAULT_ID_MSG_QUEUES_IS_FULL_INVALID_QUEUE, SW_FAULT_ID_WATCHDOG_INVALID_SELF_TEST_STATE, - SW_FAULT_ID_ALARM_MGMT_INVALID_FIFO_TO_RESET, + SW_FAULT_ID_ALARM_MGMT_INVALID_FIFO_TO_RESET, // 35 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_IN_FLOW_SET_TOO_HIGH, 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_INVALID_DIAL_OUT_PUMP_STATE, // 40 SW_FAULT_ID_DIAL_OUT_FLOW_SET_TOO_HIGH, SW_FAULT_ID_BLOOD_FLOW_INVALID_FILTER_STATE, - SW_FAULT_ID_DIAL_IN_FLOW_INVALID_FILTER_STATE, // 45 + SW_FAULT_ID_DIAL_IN_FLOW_INVALID_FILTER_STATE, + SW_FAULT_ID_RTC_EXEC_INVALID_STATE, + SW_FAULT_ID_RTC_SELF_TEST_INVALID_STATE, // 45 + SW_FAULT_ID_RTC_TRANSACTION_SERVICE_INVALID_STATE, NUM_OF_SW_FAULT_IDS } SW_FAULT_ID_T; Index: firmware/App/Services/CommBuffers.c =================================================================== diff -u -rab432aeb5e140e5b17b149f980063843fcb76290 -r5217f70ca5c74bd586dc14540e6404b43feea004 --- firmware/App/Services/CommBuffers.c (.../CommBuffers.c) (revision ab432aeb5e140e5b17b149f980063843fcb76290) +++ firmware/App/Services/CommBuffers.c (.../CommBuffers.c) (revision 5217f70ca5c74bd586dc14540e6404b43feea004) @@ -32,9 +32,10 @@ // ********** private data ********** -static U32 commBufferByteCount[ NUM_OF_COMM_BUFFERS ][ DOUBLE_BUFFERS ]; // for each buffer, how many bytes does it contain? (also index to next available) -static U32 activeDoubleBuffer[ NUM_OF_COMM_BUFFERS ]; // for each buffer, which double buffer is being fed right now? +static volatile U32 commBufferByteCount[ NUM_OF_COMM_BUFFERS ][ DOUBLE_BUFFERS ]; // for each buffer, how many bytes does it contain? (also index to next available) +static volatile U32 activeDoubleBuffer[ NUM_OF_COMM_BUFFERS ]; // for each buffer, which double buffer is being fed right now? static U08 commBuffers[ NUM_OF_COMM_BUFFERS ][ DOUBLE_BUFFERS ][ COMM_BUFFER_LENGTH ]; // each is double buffered to avoid thread contention +static volatile BOOL bufferGetLock[ NUM_OF_COMM_BUFFERS ]; // prevent getter from accessing active buffer while add in progress // ********** private function prototypes ********** @@ -96,6 +97,7 @@ // add requires brief thread protection because there may be multiple sources for transmits trying to add data to a buffer. _disable_IRQ(); + bufferGetLock[ buffer ] = TRUE; activeBuffer = activeDoubleBuffer[ buffer ]; currentActiveBufCount = commBufferByteCount[ buffer ][ activeBuffer ]; @@ -113,13 +115,15 @@ commBufferByteCount[ buffer ][ activeBuffer ] += len; // release thread protection _enable_IRQ(); + bufferGetLock[ buffer ] = FALSE; // data successfully added to buffer result = TRUE; } else // buffer too full to add this much data { // release thread protection _enable_IRQ(); + bufferGetLock[ buffer ] = FALSE; SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_COMM_BUFFERS_ADD_TOO_MUCH_DATA, len ) } } @@ -156,7 +160,7 @@ // verify given buffer if ( buffer < NUM_OF_COMM_BUFFERS ) { - // verify size of get + // verify requested # of bytes to get are in the buffer if ( ( len <= ( COMM_BUFFER_LENGTH * DOUBLE_BUFFERS ) ) && ( len <= numberOfBytesInCommBuffer( buffer ) ) ) { U32 activeBuffer = activeDoubleBuffer[ buffer ]; @@ -179,10 +183,6 @@ result += remNumOfBytes; } } - else // invalid get size given - { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_COMM_BUFFERS_GET_TOO_MUCH_DATA, len ) - } } else // invalid buffer given { @@ -215,7 +215,7 @@ // verify given buffer if ( buffer < NUM_OF_COMM_BUFFERS ) { - // verify size of peek + // verify requested # of bytes to peek are in the buffer if ( ( len <= ( COMM_BUFFER_LENGTH * DOUBLE_BUFFERS ) ) && ( len <= numberOfBytesInCommBuffer( buffer ) ) ) { U32 activeBuffer = activeDoubleBuffer[ buffer ]; @@ -237,10 +237,6 @@ numOfBytesPeeked = bytesInInactiveBuffer + remNumOfBytes; } } - else // invalid peek size given - { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_COMM_BUFFERS_PEEK_TOO_MUCH_DATA, len ) - } } else // invalid buffer given { @@ -268,7 +264,17 @@ // verify given buffer if ( buffer < NUM_OF_COMM_BUFFERS ) { - result = commBufferByteCount[ buffer ][ 0 ] + commBufferByteCount[ buffer ][ 1 ]; + U32 activeBuffer = activeDoubleBuffer[ buffer ]; + U32 inactiveBuffer = GET_TOGGLE( activeBuffer, 0, 1 ); + + if ( FALSE == bufferGetLock[ buffer ] ) + { + result = commBufferByteCount[ buffer ][ inactiveBuffer ] + commBufferByteCount[ buffer ][ activeBuffer ]; + } + else + { + result = commBufferByteCount[ buffer ][ inactiveBuffer ]; + } } else // invalid buffer { Index: firmware/App/Services/PIControllers.c =================================================================== diff -u -rea6ff77291eee02f351953b76c6720cf860c8be7 -r5217f70ca5c74bd586dc14540e6404b43feea004 --- firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision ea6ff77291eee02f351953b76c6720cf860c8be7) +++ firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision 5217f70ca5c74bd586dc14540e6404b43feea004) @@ -21,14 +21,14 @@ // ********** private definitions ********** +#define MIN_KI NEARLY_ZERO + typedef struct { // -- PI's parameters -- F32 Kp; // Proportional Value F32 Ki; // Integral Value F32 uMax; // Maximum control signal F32 uMin; // Minimum control signal - F32 iMax; // Maximum error sum - F32 iMin; // Minimum error sum // -- PI's signals -- F32 referenceSignal; // reference signal F32 measuredSignal; // measured signal @@ -45,10 +45,10 @@ // PI Controllers -- definition static PI_CONTROLLER_T piControllers[ NUM_OF_PI_CONTROLLERS_IDS ] = -{ // Kp Ki uMax uMin iMax iMin ref meas err esw esum ctrl - { 0.0, 0.0, 1.0, 0.0, 10.0, -10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // PI_CONTROLLER_ID_LOAD_CELL - { 0.0, 0.0, 1.0, 0.0, 10.0, -10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // PI_CONTROLLER_ID_BLOOD_FLOW - { 0.0, 0.0, 1.0, 0.0, 10.0, -10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } // PI_CONTROLLER_ID_DIALYSATE_FLOW +{ // Kp Ki uMax uMin ref meas err esw esum ctrl + { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // PI_CONTROLLER_ID_LOAD_CELL + { 0.0, 0.0, 0.90, 0.10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // PI_CONTROLLER_ID_BLOOD_FLOW + { 0.0, 0.0, 0.90, 0.10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } // PI_CONTROLLER_ID_DIALYSATE_FLOW }; /************************************************************************* @@ -62,7 +62,7 @@ * @return none *************************************************************************/ void initializePIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal, - F32 kP, F32 kI, F32 controlMin, F32 controlMax, F32 iMin, F32 iMax ) + F32 kP, F32 kI, F32 controlMin, F32 controlMax ) { PI_CONTROLLER_T *controller; @@ -71,11 +71,16 @@ SET_CONTROLLER( controller, controllerID ); controller->Kp = kP; - controller->Ki = kI; + if ( fabs( kI ) > MIN_KI ) // ensure kI is not zero + { + controller->Ki = kI; + } + else + { + controller->Ki = ( kI < 0.0 ? MIN_KI * -1.0 : MIN_KI ); + } controller->uMin = controlMin; controller->uMax = controlMax; - controller->iMin = iMin; - controller->iMax = iMax; resetPIController( controllerID, initialControlSignal ); } } @@ -100,8 +105,8 @@ controller->controlSignal = RANGE( initialControlSignal, controller->uMin, controller->uMax ); controller->referenceSignal = 0.0; controller->errorSignal = 0.0; - controller->errorSumBeforeWindUp = 0.0; - controller->errorSum = 0.0; + controller->errorSum = controller->controlSignal / controller->Ki; + controller->errorSumBeforeWindUp = controller->errorSum; controller->measuredSignal = 0.0; } } @@ -123,19 +128,27 @@ if ( controllerID < NUM_OF_PI_CONTROLLERS_IDS ) { + F32 controlSignalBeforeWindup; + F32 windupError; + SET_CONTROLLER( controller, controllerID ); controller->referenceSignal = referenceSignal; controller->measuredSignal = measuredSignal; // calculate error signal controller->errorSignal = fabs( referenceSignal ) - ( referenceSignal < 0.0 ? ( measuredSignal * -1.0 ) : measuredSignal ); - controller->errorSumBeforeWindUp += controller->errorSignal; + controller->errorSum += controller->errorSignal; // anti-windup - controller->errorSum = RANGE( controller->errorSumBeforeWindUp, controller->iMin, controller->iMax ); + controller->errorSumBeforeWindUp = controller->errorSum; // calculate control signal - controller->controlSignal += ( controller->Kp * controller->errorSignal ) + ( controller->Ki * controller->errorSum ); - // limit control signal to valid range - controller->controlSignal = RANGE( controller->controlSignal, controller->uMin, controller->uMax ); + controlSignalBeforeWindup = ( controller->Kp * controller->errorSignal ) + ( controller->Ki * controller->errorSum ); + controller->controlSignal = RANGE( controlSignalBeforeWindup, controller->uMin, controller->uMax ); + // handle anti-windup for i term + windupError = controller->controlSignal - controlSignalBeforeWindup; + if ( fabs( windupError ) > NEARLY_ZERO ) + { + controller->errorSum -= ( windupError / controller->Ki ); + } result = controller->controlSignal; } Index: firmware/App/Services/PIControllers.h =================================================================== diff -u -rea6ff77291eee02f351953b76c6720cf860c8be7 -r5217f70ca5c74bd586dc14540e6404b43feea004 --- firmware/App/Services/PIControllers.h (.../PIControllers.h) (revision ea6ff77291eee02f351953b76c6720cf860c8be7) +++ firmware/App/Services/PIControllers.h (.../PIControllers.h) (revision 5217f70ca5c74bd586dc14540e6404b43feea004) @@ -45,7 +45,7 @@ // ********** public function prototypes ********** void initializePIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal, - F32 kP, F32 kI, F32 controlMin, F32 controlMax, F32 iMin, F32 iMax ); + F32 kP, F32 kI, F32 controlMin, F32 controlMax ); void resetPIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal ); F32 runPIController( PI_CONTROLLER_ID_T controllerID, F32 referenceSignal, F32 measuredSignal ); F32 getPIControllerSignals( PI_CONTROLLER_ID_T controllerID, PI_CONTROLLER_SIGNALS_ID signalID ); Index: firmware/App/Tasks/TaskGeneral.c =================================================================== diff -u -rd2faaae9f396a98c32594b139a8ee6d3f3a2763f -r5217f70ca5c74bd586dc14540e6404b43feea004 --- firmware/App/Tasks/TaskGeneral.c (.../TaskGeneral.c) (revision d2faaae9f396a98c32594b139a8ee6d3f3a2763f) +++ firmware/App/Tasks/TaskGeneral.c (.../TaskGeneral.c) (revision 5217f70ca5c74bd586dc14540e6404b43feea004) @@ -64,7 +64,7 @@ execBloodFlowController(); // control dialysate inlet pump - execDialInFlowController(); +// execDialInFlowController(); // manage alarm state execAlarmMgmt(); Index: firmware/App/Tasks/TaskPriority.c =================================================================== diff -u -rd2faaae9f396a98c32594b139a8ee6d3f3a2763f -r5217f70ca5c74bd586dc14540e6404b43feea004 --- firmware/App/Tasks/TaskPriority.c (.../TaskPriority.c) (revision d2faaae9f396a98c32594b139a8ee6d3f3a2763f) +++ firmware/App/Tasks/TaskPriority.c (.../TaskPriority.c) (revision 5217f70ca5c74bd586dc14540e6404b43feea004) @@ -53,7 +53,7 @@ execBloodFlowMonitor(); // monitor dialysate inlet pump and flow - execDialInFlowMonitor(); + //execDialInFlowMonitor(); // 2nd pass for FPGA execFPGAOut();