/************************************************************************** * * Copyright (c) 2020-2023 Diality Inc. - All Rights Reserved. * * THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN * WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER. * * @file PIControllers.c * * @author (last) Michael Garthwaite * @date (last) 21-Nov-2022 * * @author (original) Sean * @date (original) 04-Feb-2020 * ***************************************************************************/ #include #include "SystemCommMessages.h" #include "PIControllers.h" /** * @addtogroup PIControllers * @{ */ // ********** private definitions ********** #define MAX_ILIMIT 1.0F ///< Controller max Ilimit/maxErrorSumStep. /// 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. // -- 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 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 MIN_KI NEARLY_ZERO ///< minimum integral coefficient - cannot be zero. #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 Ilimit controller type { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 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, 10.0, CONTROLLER_BIDIRECTIONAL }, // PI_CONTROLLER_ID_DRAIN_PUMP { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, CONTROLLER_UNIDIRECTIONAL }, // PI_CONTROLLER_ID_RO_PUMP_MAX_PRES }; /*********************************************************************//** * @brief * The initializePIController function initializes controller before operation. * Make sure to call it before first call to runController function. * @details Inputs: none * @details Outputs: PI controllers module initialized * @param controllerID ID filter number * @param initialControlSignal Value of the output on the first iteration * @param kP Coefficient for proportional * @param kI Coefficient for integral * @param controlMin Minimum control output * @param controlMax Maximum control output * @return none *************************************************************************/ void initializePIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal, F32 kP, F32 kI, F32 controlMin, F32 controlMax ) { 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 { controller->Ki = kI; } else { controller->Ki = ( kI < 0.0 ? MIN_KI * -1.0 : MIN_KI ); } controller->uMin = controlMin; controller->uMax = controlMax; resetPIController( controllerID, initialControlSignal ); } else { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) } } /*********************************************************************//** * @brief * The resetPIController functions resets controller before new set point. * Make sure to call it before first call to runController function. * @details Inputs: none * @details Outputs: Reset a PI controller * @param controllerID ID filter number * @param initialControlSignal Value of the output on the first iteration * @return none *************************************************************************/ void resetPIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal ) { PI_CONTROLLER_T *controller; if ( controllerID < NUM_OF_PI_CONTROLLERS_IDS ) { SET_CONTROLLER( controller, controllerID ); controller->controlSignal = RANGE( initialControlSignal, controller->uMin, controller->uMax ); controller->referenceSignal = 0.0; controller->errorSignal = 0.0; controller->errorSum = controller->controlSignal / controller->Ki; controller->errorSumBeforeWindUp = controller->errorSum; controller->measuredSignal = 0.0; } else { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) } } /*********************************************************************//** * @brief * The runPIController functions executes a PI controller with a new sampled measured signal. * @details Inputs: none * @details Outputs: Feeds new signal to PI controller * @param controllerID ID filter number * @param referenceSignal reference signal value * @param measuredSignal latest measured sample * @return value of the control signal *************************************************************************/ F32 runPIController(PI_CONTROLLER_ID_T controllerID, F32 referenceSignal, F32 measuredSignal) { PI_CONTROLLER_T *controller; 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 if ( controller->direction == CONTROLLER_UNIDIRECTIONAL ) { // Control should always be positive controller->errorSignal = fabs( referenceSignal ) - ( referenceSignal < 0.0F ? ( measuredSignal * -1.0F ) : measuredSignal ); } else { controller->errorSignal = referenceSignal - measuredSignal; } // Limit error sum step size if ( fabs( controller->errorSignal ) > controller->maxErrorSumStep ) { if ( controller->errorSignal < 0.0F ) { controller->errorSum += ( controller->maxErrorSumStep * -1.0F ); } 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 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 { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) } return result; } /*********************************************************************//** * @brief * The getPIControllerSignals function returns the latest requested signal sample. * @details Inputs: none * @details Outputs: none * @param controllerID ID filter number * @param signalID signal sample ID request * @return latest sample requested *************************************************************************/ F32 getPIControllerSignals( PI_CONTROLLER_ID_T controllerID, PI_CONTROLLER_SIGNALS_ID signalID ) { PI_CONTROLLER_T *controller; F32 output = 0.0; if ( controllerID < NUM_OF_PI_CONTROLLERS_IDS ) { SET_CONTROLLER( controller, controllerID ); switch( signalID ) { case CONTROLLER_SIGNAL_REFERENCE: output = controller->referenceSignal; break; case CONTROLLER_SIGNAL_MEASURED: output = controller->measuredSignal; break; case CONTROLLER_SIGNAL_ERROR: output = controller->errorSignal; break; case CONTROLLER_SIGNAL_ERROR_SUM: output = controller->errorSumBeforeWindUp; break; case CONTROLLER_SIGNAL_ERROR_SUM_AFTER_WINDUP: output = controller->errorSum; break; case CONTROLLER_SIGNAL_PROPORTIONAL_OUTPUT: output = controller->Kp * controller->errorSignal; break; case CONTROLLER_SIGNAL_INTEGRAL_OUTPUT: output = controller->Ki * controller->errorSum; break; case CONTROLLER_SIGNAL_CONTROL: output = controller->controlSignal; break; 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 } else { // Invalid controller given SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) } return output; } /*********************************************************************//** * @brief * The getPIControllerSignals function returns the latest requested signal sample. * @details Inputs: none * @details Outputs: maxErrorSumStep of controller ID * @param controllerID ID filter number * @param stepLimit maximum step limit * @return latest sample requested *************************************************************************/ void setPIControllerStepLimit( PI_CONTROLLER_ID_T controllerID, F32 stepLimit ) { PI_CONTROLLER_T *controller; if ( controllerID < NUM_OF_PI_CONTROLLERS_IDS ) { SET_CONTROLLER( controller, controllerID ); if ( ( stepLimit > NEARLY_ZERO ) && ( stepLimit < MAX_ILIMIT ) ) { controller->maxErrorSumStep = stepLimit; } else { SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_STEP_LIMIT, (U32)stepLimit ) } } else { // Invalid controller given SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) } } /**@}*/