Index: firmware/App/Services/PIControllers.c =================================================================== diff -u -r30f049651877229042e3f8700c8596e5b9a1e0f4 -r766708fceb0bdf1af8c7897df29d4f5036bfd3db --- firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision 30f049651877229042e3f8700c8596e5b9a1e0f4) +++ firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision 766708fceb0bdf1af8c7897df29d4f5036bfd3db) @@ -30,32 +30,43 @@ /// Minimum integral coefficient - cannot be zero. #define MIN_KI NEARLY_ZERO -/// Record for PI controller. +/// 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 runs 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; -#define SET_CONTROLLER( c, id ) ((c) = &piControllers[id]) ///< Macro to set a local controller pointer to a given piController. +/// Macro to set a local controller pointer to a given piController. +#define SET_CONTROLLER( c, id ) ((c) = &piControllers[id]) // ********** 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