/************************************************************************** * * Copyright (c) 2026-2026 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 PIDControllers.c * * @author (last) Michael Garthwaite * @date (last) 15-Apr-2026 * * @author (original) Michael Garthwaite * @date (original) 26-Mar-2026 * ***************************************************************************/ #include #include "Messaging.h" #include "PIDControllers.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) = &pidControllers[ 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 PID controllers directions } PID_CONTROLLER_DIRECTIONS_T; /// Record for PID controller. typedef struct { // -- PID's parameters -- F32 Kp; ///< Proportional Value. F32 Ki; ///< Integral Value. F32 Kd; ///< Derivative Value. F32 uMax; ///< Maximum control signal. F32 uMin; ///< Minimum control signal. // -- PID's signals -- F32 referenceSignal; ///< Reference signal. F32 measuredSignal; ///< Measured signal. F32 errorSignal; ///< Reference - measured signal. F32 errorSum; ///< Error integral. F32 errorPrevious; ///< Previous error signal. F32 controlSignal; ///< Actual control signal. F32 maxErrorSumStep; ///< Maximum change in I (error sum) for a single control interval. PID_CONTROLLER_DIRECTIONS_T direction; ///< PI controller control direction. BOOL isFeedForwardEnabled; ///< Eanble or disable Feed forward term F32 feedForward; ///< Feed forward term } PID_CONTROLLER_T; // ********** private data ********** /// PID Controllers - initial configurations. static PID_CONTROLLER_T pidControllers[ NUM_OF_PID_CONTROLLERS_IDS ] = { // Kp Ki Kd uMax uMin ref meas err esum eprev ctrl Ilimit controller type, feedfwdEnabled, feedfwdValue #ifdef _TD_ { 0.0, 0.0, 0.0, 0.9, 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, 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 }, // PID_CONTROLLER_ID_RO_PUMP #endif }; /*********************************************************************//** * @brief * The initializePIDController function initializes controller before operation. * Make sure to call it before first call to runController function. * @details \b Input: none * @details \b Outputs: PID controllers module initialized * @details \b Alarms: ALARM_ID_XX_SOFTWARE_FAULT when invalid PID 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 kD Coefficient for derivative * @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 initializePIDController( PID_CONTROLLER_ID_T controllerID, F32 initialControlSignal, F32 kP, F32 kI, F32 kD, F32 controlMin, F32 controlMax, BOOL isFeedForwardEnabled, F32 feedFowardSignal ) { if ( controllerID < NUM_OF_PID_CONTROLLERS_IDS ) { PID_CONTROLLER_T *controller; SET_CONTROLLER( controller, controllerID ); controller->Kp = kP; controller->Kd = kD; 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; resetPIDController( controllerID, initialControlSignal, feedFowardSignal ); } else { #ifdef _DD_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PID_CTRL_INVALID_CONTROLLER1, (U32)controllerID ) #endif #ifdef _TD_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_PID_CTRL_INVALID_CONTROLLER1, (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 resetPIDController( PID_CONTROLLER_ID_T controllerID, F32 initialControlSignal, F32 feedFowardSignal ) { PID_CONTROLLER_T *controller; if ( controllerID < NUM_OF_PID_CONTROLLERS_IDS ) { SET_CONTROLLER( controller, controllerID ); controller->controlSignal = RANGE( initialControlSignal, controller->uMin, controller->uMax ); controller->referenceSignal = 0.0F; controller->errorSignal = 0.0F; controller->errorSum = 0.0F; controller->measuredSignal = 0.0F; controller->feedForward = feedFowardSignal; controller->errorPrevious = 0.0F; } else { #ifdef _DD_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PID_CTRL_INVALID_CONTROLLER2, (U32)controllerID ) #endif #ifdef _DT_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_PID_CTRL_INVALID_CONTROLLER2, (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 runPIDController(PID_CONTROLLER_ID_T controllerID, F32 referenceSignal, F32 measuredSignal) { PID_CONTROLLER_T *controller; F32 result = 0.0; if ( controllerID < NUM_OF_PID_CONTROLLERS_IDS ) { 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; } // TODO Redo anti windup by enforcing a max |error sum| // If feed forward enabled, use new feed forward value as basis for control signal instead of previous control signal if ( FALSE != controller->isFeedForwardEnabled ) { controller->controlSignal = controller->feedForward; } // Update control signal controller->controlSignal += ( controller->Ki * controller->errorSum ); controller->controlSignal += ( controller->Kp * controller->errorSignal ); controller->controlSignal += ( controller->Kd * ( controller->errorSignal - controller->errorPrevious ) ); // Record error signal for D term next time controller->errorPrevious = controller->errorSignal; // Apply range limits to new control signal 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_PID_CTRL_INVALID_CONTROLLER3, (U32)controllerID ) #endif #ifdef _TD_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_PID_CTRL_INVALID_CONTROLLER3, (U32)controllerID ) #endif } return result; } /*********************************************************************//** * @brief * The getPIDControllerSignals 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 getPIDControllerSignals( PID_CONTROLLER_ID_T controllerID, PID_CONTROLLER_SIGNALS_ID signalID ) { PID_CONTROLLER_T *controller; F32 output = 0.0; if ( controllerID < NUM_OF_PID_CONTROLLERS_IDS ) { SET_CONTROLLER( controller, controllerID ); switch( signalID ) { case CONTROLLER_SIGNAL_PID_REFERENCE: output = controller->referenceSignal; break; case CONTROLLER_SIGNAL_PID_MEASURED: output = controller->measuredSignal; break; case CONTROLLER_SIGNAL_PID_ERROR: output = controller->errorSignal; break; case CONTROLLER_SIGNAL_PID_ERROR_SUM: output = controller->errorSum; break; case CONTROLLER_SIGNAL_PID_PROPORTIONAL_OUTPUT: output = controller->Kp * controller->errorSignal; break; case CONTROLLER_SIGNAL_PID_INTEGRAL_OUTPUT: output = controller->Ki * controller->errorSum; break; case CONTROLLER_SIGNAL_PID_FEEDFORWARD_OUTPUT: output = controller->feedForward; break; case CONTROLLER_SIGNAL_PID_CONTROL: output = controller->controlSignal; break; case CONTROLLER_SIGNAL_PID_ERROR_PREVIOUS: output = controller->errorPrevious; break; case CONTROLLER_SIGNAL_PID_DERIVATIVE_OUTPUT: output = controller->Kd * ( controller->errorSignal - controller->errorPrevious ); default: #ifdef _DD_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PID_CTRL_INVALID_SIGNAL, (U32)signalID ) #endif #ifdef _TD_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_PID_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_PID_CTRL_INVALID_CONTROLLER4, (U32)controllerID ) #endif #ifdef _TD_ SET_ALARM_WITH_2_U32_DATA( ALARM_ID_TD_SOFTWARE_FAULT, SW_FAULT_ID_PID_CTRL_INVALID_CONTROLLER4, (U32)controllerID ) #endif } return output; } /**@}*/