/************************************************************************** * * Copyright (c) 2024-2025 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) Sean Nash * @date (last) 10-Sep-2025 * * @author (original) Vinayakam Mani * @date (original) 07-Oct-2024 * ***************************************************************************/ #include #include "Messaging.h" #include "PIControllers.h" /** * @addtogroup PIControllers * @{ */ // ********** private definitions ********** #define MAX_ILIMIT 1.0F ///< Controller max Ilimit/maxErrorSumStep. #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. /// 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. BOOL isFeedForwardEnabled; ///< Eanble or disable Feed forward term F32 feedForward; ///< Feed forward term } PI_CONTROLLER_T; // ********** 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, feedfwdEnabled, feedfwdValue #ifdef _TD_ { 0.0, 0.0, 0.90, 0.10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100.0, CONTROLLER_UNIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_BLOOD_FLOW #endif #ifdef _DD_ { 0.0, 0.0, 2650, 350, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, CONTROLLER_BIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_D12_PUMP { 0.0, 0.0, 2650, 350, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.0, CONTROLLER_BIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_D48_PUMP { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 15.0, CONTROLLER_BIDIRECTIONAL, TRUE, 0.0 }, // PI_CONTROLLER_ID_D5_HEAT { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 5.0, CONTROLLER_BIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_D45_HEAT { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 25.0, CONTROLLER_UNIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_RO_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, FALSE, 0.0 }, // PI_CONTROLLER_ID_RO_PUMP_MAX_PRES { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, CONTROLLER_UNIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_BOOST_PUMP_FLOW { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, CONTROLLER_UNIDIRECTIONAL, FALSE, 0.0 }, // PI_CONTROLLER_ID_BOOST_PUMP_PRES #endif }; /*********************************************************************//** * @brief * The initializePIController function initializes controller before operation. * Make sure to call it before first call to runController function. * @details \b Input: none * @details \b Outputs: PI controllers module initialized * @details \b Alarms: ALARM_ID_XX_SOFTWARE_FAULT when invalid PI controller * Id is passed. * @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 * @param isFeedForwardEnabled Feed forward enabled if true, otherwise not. * @param feedFowardSignal feedforward value to be applied to the control output * @return none *************************************************************************/ void initializePIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal, F32 kP, F32 kI, F32 controlMin, F32 controlMax, BOOL isFeedForwardEnabled, F32 feedFowardSignal ) { 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; controller->isFeedForwardEnabled = isFeedForwardEnabled; resetPIController( controllerID, initialControlSignal, feedFowardSignal ); } else { #ifdef _DD_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) #endif } } /*********************************************************************//** * @brief * The resetPIController functions resets controller before new set point. * Make sure to call it before first call to runController function. * @details \b Inputs: none * @details \b Outputs: Reset a PI controller * @details \b Alarms: ALARM_ID_XX_SOFTWARE_FAULT when invalid PI controller * Id is passed. * @param controllerID ID filter number * @param initialControlSignal Value of the output on the first iteration * @param feedFowardSignal Value of feed froward calculation if exists. * @return none *************************************************************************/ void resetPIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal, F32 feedFowardSignal ) { 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; controller->feedForward = feedFowardSignal; } else { #ifdef _DD_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) #endif } } /*********************************************************************//** * @brief * The runPIController functions executes a PI controller with a new sampled measured signal. * @details \b Inputs: none * @details \b Outputs: Feeds new signal to PI controller * @details \b Alarms: ALARM_ID_XX_SOFTWARE_FAULT when invalid PI controller * Id is passed. * @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 ); if ( FALSE == controller->isFeedForwardEnabled ) { controller->controlSignal = RANGE( controlSignalBeforeWindup, controller->uMin, controller->uMax ); } else { controller->controlSignal = RANGE( controlSignalBeforeWindup, ( controller->uMin - controller->feedForward ), ( controller->uMax - controller->feedForward ) ); } // 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 controller->controlSignal += ( controller->Kp * controller->errorSignal ); // Add feed forward term to control signal if ( FALSE != controller->isFeedForwardEnabled ) { controller->controlSignal = controller->controlSignal + controller->feedForward; } //Re-apply range limits controller->controlSignal = RANGE( controller->controlSignal, controller->uMin, controller->uMax ); result = controller->controlSignal; } else { #ifdef _DD_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) #endif } return result; } /*********************************************************************//** * @brief * The getPIControllerSignals function returns the latest requested signal sample. * @details \b Inputs: none * @details \b Outputs: none * @details \b Alarms: ALARM_ID_XX_SOFTWARE_FAULT when invalid PI controller * Id is passed. * @details \b Alarms: ALARM_ID_XX_SOFTWARE_FAULT when invalid PI controller * signal is passed. * @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_FEEDFORWARD_OUTPUT: output = controller->feedForward; break; case CONTROLLER_SIGNAL_CONTROL: output = controller->controlSignal; break; default: #ifdef _DD_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_SIGNAL, (U32)signalID ) #endif break; } // End of switch } else { // Invalid controller given #ifdef _DD_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) #endif } return output; } /*********************************************************************//** * @brief * The getPIControllerSignals function returns the latest requested signal sample. * @details \b Inputs: none * @details \b Outputs: maxErrorSumStep of controller ID * @details \b Alarms: ALARM_ID_XX_SOFTWARE_FAULT when invalid PI controller * Id is passed. * @details \b Alarms: ALARM_ID_XX_SOFTWARE_FAULT when invalid PI controller * stemp limit is passed. * @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 { #ifdef _DD_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_STEP_LIMIT, (U32)stepLimit ) #endif } } else { #ifdef _DD_ // Invalid controller given SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) #endif } } /**@}*/