Index: firmware/App/Services/PIControllers.c =================================================================== diff -u -r35246359c5a9080c704e0a6f1563e99a337e2e91 -r64b37cc2955d04bbc77ea41940b1b1b30c06651b --- firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision 35246359c5a9080c704e0a6f1563e99a337e2e91) +++ firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision 64b37cc2955d04bbc77ea41940b1b1b30c06651b) @@ -15,7 +15,7 @@ * ***************************************************************************/ -#include "math.h" +#include #include "SystemCommMessages.h" #include "PIControllers.h" @@ -30,9 +30,9 @@ /// Enumeration of PI controller direction. typedef enum controller_Directions { - CONTROLLER_BIDIRECTIONAL = 0, ///< Controller runs bidirectional so it only covers positive and negative values - CONTROLLER_UNIDIRECTIONAL, ///< Controller run unidirectional so it only covers positive values - NUM_OF_CONTROLLELR_DIRECTIONS ///< Number of PI controllers 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. @@ -43,12 +43,13 @@ 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; @@ -60,11 +61,11 @@ /// 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, CONTROLLER_UNIDIRECTIONAL }, // PI_CONTROLLER_ID_RO_PUMP - { 0.0, 0.0, 3000, 300, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, CONTROLLER_BIDIRECTIONAL }, // PI_CONTROLLER_ID_DRAIN_PUMP - { 0.0, 0.0, 1.39, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, CONTROLLER_UNIDIRECTIONAL }, // PI_CONTROLLER_ID_PRIMARY_HEATER - { 0.0, 0.0, 0.50, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, CONTROLLER_UNIDIRECTIONAL }, // PI_CONTROLLER_ID_TRIMMER_HEATER - { 0.0, 0.0, 0.99, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, CONTROLLER_UNIDIRECTIONAL }, // I_CONTROLLER_ID_RO_PUMP_RAMP_UP + { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 50.0, CONTROLLER_UNIDIRECTIONAL }, // PI_CONTROLLER_ID_RO_PUMP + { 0.0, 0.0, 3000, 300, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 50.0, CONTROLLER_BIDIRECTIONAL }, // PI_CONTROLLER_ID_DRAIN_PUMP + { 0.0, 0.0, 1.39, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 20.0, CONTROLLER_UNIDIRECTIONAL }, // PI_CONTROLLER_ID_PRIMARY_HEATER + { 0.0, 0.0, 0.50, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 20.0, CONTROLLER_UNIDIRECTIONAL }, // PI_CONTROLLER_ID_TRIMMER_HEATER + { 0.0, 0.0, 0.99, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 50.0, CONTROLLER_UNIDIRECTIONAL }, // I_CONTROLLER_ID_RO_PUMP_RAMP_UP }; /*********************************************************************//** @@ -84,14 +85,14 @@ void initializePIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal, F32 kP, F32 kI, F32 controlMin, F32 controlMax ) { - PI_CONTROLLER_T *controller; - if ( controllerID < NUM_OF_PI_CONTROLLERS_IDS ) { + PI_CONTROLLER_T *controller; + 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; } @@ -175,18 +176,39 @@ controller->errorSignal = referenceSignal - measuredSignal; } - controller->errorSum += controller->errorSignal; - // anti-windup + // 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; - // calculate control signal - controlSignalBeforeWindup = ( controller->Kp * controller->errorSignal ) + ( controller->Ki * controller->errorSum ); + controlSignalBeforeWindup = ( controller->Ki * controller->errorSum ); controller->controlSignal = RANGE( controlSignalBeforeWindup, controller->uMin, controller->uMax ); - // handle anti-windup for i term + + // Handle anti-windup for i term windupError = controlSignalBeforeWindup - controller->controlSignal; if ( fabs( windupError ) > NEARLY_ZERO ) { controller->errorSum -= ( windupError / controller->Ki ); } + + // Add p term 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 @@ -252,10 +274,10 @@ default: SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_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_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) }