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; }