Index: firmware/App/Services/PIControllers.c =================================================================== diff -u -rd91a24c730aeb5cd7e3eba9ef4eca78e442911f8 -r0ca8364b86f7b7d7841bc884eae5f674fd5618d4 --- firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision d91a24c730aeb5cd7e3eba9ef4eca78e442911f8) +++ firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision 0ca8364b86f7b7d7841bc884eae5f674fd5618d4) @@ -30,32 +30,43 @@ /// minimum integral coefficient - cannot be zero. #define MIN_KI NEARLY_ZERO +/// Enumeration of PI controller direction. +typedef enum controller_Directions +{ + CONTROLLER_BIDIRECTIONAL = 0, ///< Controller runs bidirectional so it covers positive and negative control signals + CONTROLLER_UNIDIRECTIONAL, ///< Controller run unidirectional so it only covers positive control signals + NUM_OF_CONTROLLELR_DIRECTIONS ///< Number of PI controllers directions +} PI_CONTROLLER_DIRECTIONS_T; + /// record for PI controller. typedef struct { // -- PI's parameters -- - F32 Kp; ///< Proportional Value. - F32 Ki; ///< Integral Value. - F32 uMax; ///< Maximum control signal. - F32 uMin; ///< Minimum control signal. + F32 Kp; ///< Proportional Value. + F32 Ki; ///< Integral Value. + F32 uMax; ///< Maximum control signal. + F32 uMin; ///< Minimum control signal. // -- PI's signals -- - F32 referenceSignal; ///< reference signal. - F32 measuredSignal; ///< measured signal. - F32 errorSignal; ///< reference - measured signal. - F32 errorSumBeforeWindUp; ///< error signal before windup correction. - F32 errorSum; ///< error integral after windup correction. - F32 controlSignal; ///< actual control signal. + F32 referenceSignal; ///< reference signal. + F32 measuredSignal; ///< measured signal. + F32 errorSignal; ///< reference - measured signal. + F32 errorSumBeforeWindUp; ///< error signal before windup correction. + F32 errorSum; ///< error integral after windup correction. + F32 controlSignal; ///< actual control signal. + F32 maxErrorSumStep; ///< maximum change in I (error sum) for a single control interval. + PI_CONTROLLER_DIRECTIONS_T direction; ///< PI controller control direction. } PI_CONTROLLER_T; + +/// macro to set a local controller pointer to a given piController. +#define SET_CONTROLLER( c, id ) ((c) = &piControllers[id]) -#define SET_CONTROLLER( c, id ) ((c) = &piControllers[id]) ///< macro to set a local controller pointer to a given piController. - // ********** private data ********** /// PI Controllers -- initial configurations. static PI_CONTROLLER_T piControllers[ NUM_OF_PI_CONTROLLERS_IDS ] = -{ // 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_ULTRAFILTRATION - { 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 +{ // Kp Ki uMax uMin ref meas err esw esum ctrl Ilimit controller type + { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, CONTROLLER_UNIDIRECTIONAL }, // PI_CONTROLLER_ID_ULTRAFILTRATION + { 0.0, 0.0, 0.90, 0.10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100.0, CONTROLLER_UNIDIRECTIONAL }, // 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, 100.0, CONTROLLER_UNIDIRECTIONAL } // PI_CONTROLLER_ID_DIALYSATE_FLOW }; /**@}*/ @@ -148,28 +159,54 @@ F32 result = 0.0; if ( controllerID < NUM_OF_PI_CONTROLLERS_IDS ) - { + { F32 controlSignalBeforeWindup; F32 windupError; SET_CONTROLLER( controller, controllerID ); controller->referenceSignal = referenceSignal; - controller->measuredSignal = measuredSignal; + controller->measuredSignal = measuredSignal; + // calculate error signal - controller->errorSignal = fabs( referenceSignal ) - ( referenceSignal < 0.0 ? ( measuredSignal * -1.0 ) : measuredSignal ); - controller->errorSum += controller->errorSignal; - // anti-windup - controller->errorSumBeforeWindUp = controller->errorSum; + if ( controller->direction == CONTROLLER_UNIDIRECTIONAL ) + { + controller->errorSignal = fabs( referenceSignal ) - ( referenceSignal < 0.0 ? ( measuredSignal * -1.0 ) : measuredSignal ); + } + else + { + controller->errorSignal = referenceSignal - measuredSignal; + } + + // limit error sum step size + if ( fabs( controller->errorSignal ) > controller->maxErrorSumStep ) + { + if ( controller->errorSignal < 0.0 ) + { + controller->errorSum += ( controller->maxErrorSumStep * -1.0 ); + } + else + { + controller->errorSum += controller->maxErrorSumStep; + } + } + else + { + controller->errorSum += controller->errorSignal; + } + // calculate control signal + controller->errorSumBeforeWindUp = controller->errorSum; controlSignalBeforeWindup = ( controller->Kp * controller->errorSignal ) + ( controller->Ki * controller->errorSum ); - controller->controlSignal = RANGE( controlSignalBeforeWindup, controller->uMin, controller->uMax ); + controller->controlSignal = RANGE( controlSignalBeforeWindup, controller->uMin, controller->uMax ); + // handle anti-windup for i term windupError = controlSignalBeforeWindup - controller->controlSignal; if ( fabs( windupError ) > NEARLY_ZERO ) { controller->errorSum -= ( windupError / controller->Ki ); - } + } + result = controller->controlSignal; } else