Index: firmware/App/Services/PIControllers.c =================================================================== diff -u -rd91a24c730aeb5cd7e3eba9ef4eca78e442911f8 -rc282822f36836a8127f447c8ac5b8a50e851be63 --- firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision d91a24c730aeb5cd7e3eba9ef4eca78e442911f8) +++ firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision c282822f36836a8127f447c8ac5b8a50e851be63) @@ -27,35 +27,46 @@ // ********** private definitions ********** -/// minimum integral coefficient - cannot be zero. +/// 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, 0.90, 0.10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 50.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 }; /**@}*/ @@ -84,7 +95,7 @@ SET_CONTROLLER( controller, controllerID ); controller->Kp = kP; - if ( fabs( kI ) > MIN_KI ) // ensure kI is not zero + if ( fabs( kI ) > MIN_KI ) // Ensure kI is not zero { controller->Ki = kI; } @@ -148,28 +159,58 @@ 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; - // calculate error signal - controller->errorSignal = fabs( referenceSignal ) - ( referenceSignal < 0.0 ? ( measuredSignal * -1.0 ) : measuredSignal ); - controller->errorSum += controller->errorSignal; - // anti-windup - controller->errorSumBeforeWindUp = controller->errorSum; - // calculate control signal - controlSignalBeforeWindup = ( controller->Kp * controller->errorSignal ) + ( controller->Ki * controller->errorSum ); - controller->controlSignal = RANGE( controlSignalBeforeWindup, controller->uMin, controller->uMax ); - // handle anti-windup for i term + controller->measuredSignal = measuredSignal; + + // Calculate error signal + 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 from i term + controller->errorSumBeforeWindUp = controller->errorSum; + controlSignalBeforeWindup = ( controller->Ki * controller->errorSum ); + 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 ); } + + // Add p term adjustment to control signal and re-apply range limits + controller->controlSignal += ( controller->Kp * controller->errorSignal ); + controller->controlSignal = RANGE( controller->controlSignal, controller->uMin, controller->uMax ); + result = controller->controlSignal; } else @@ -235,10 +276,10 @@ default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_SIGNAL, (U32)signalID ) break; - } // end of switch + } // End of switch } else - { // invalid controller given + { // Invalid controller given SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) }