Index: PIDControllers.c =================================================================== diff -u --- PIDControllers.c (revision 0) +++ PIDControllers.c (revision 0ead56df33c1583486ca3a8f8eeeeadfa5f11eb5) @@ -0,0 +1,398 @@ +/************************************************************************** +* +* Copyright (c) 2024-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) “rkallala” +* @date (last) 09-Dec-2025 +* +* @author (original) Vinayakam Mani +* @date (original) 07-Oct-2024 +* +***************************************************************************/ + +#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 errorSumBeforeWindUp; ///< Error signal before windup correction. + F32 errorSum; ///< Error integral after windup correction. + 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 esw esum eprev ctrl Ilimit controller type, feedfwdEnabled, feedfwdValue +#ifdef _TD_ + { 0.0, 0.0, 0.0, 0.90, 0.10, 0.0, 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, 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; + 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 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.0; + controller->errorSignal = 0.0; + controller->errorSum = controller->controlSignal / controller->Ki; + controller->errorSumBeforeWindUp = controller->errorSum; + controller->measuredSignal = 0.0; + controller->feedForward = feedFowardSignal; + controller->errorPrevious = 0.0; + } + 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 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 ) + { + 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 d term to control signal + if ( controller->Kd != 0 ) + { + controller->controlSignal += ( controller->Kd * ( controller->errorSignal - controller->errorPrevious ) ); + + controller->errorPrevious = 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 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_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; + + case CONTROLLER_SIGNAL_ERROR_PREVIOUS: + output = controller->errorPrevious; + break; + + case CONTROLLER_SIGNAL_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_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 getPIDControllerSignals 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 setPIDControllerStepLimit( PID_CONTROLLER_ID_T controllerID, F32 stepLimit ) +{ + PID_CONTROLLER_T *controller; + + if ( controllerID < NUM_OF_PID_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 + } +} + +/**@}*/ Index: PIDControllers.h =================================================================== diff -u --- PIDControllers.h (revision 0) +++ PIDControllers.h (revision 0ead56df33c1583486ca3a8f8eeeeadfa5f11eb5) @@ -0,0 +1,91 @@ +/************************************************************************** +* +* Copyright (c) 2024-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.h +* +* @author (last) Michael Garthwaite +* @date (last) 11-Sep-2025 +* +* @author (original) Vinayakam Mani +* @date (original) 07-Oct-2024 +* +***************************************************************************/ + +#ifndef __PID_CONTROLLERS_H__ +#define __PID_CONTROLLERS_H__ + +#ifdef _TD_ +#include "TDCommon.h" +#endif +#ifdef _DD_ +#include "DDCommon.h" +#endif + +/** + * @defgroup PIDControllers PIDControllers + * @brief PIDControllers common module. Provides PID controllers for various actuators. + * + * @addtogroup PIDControllers + * @{ + */ + +// ********** public definitions ********** + +/// Enumeration of PID controllers. +typedef enum ControllerList +{ +#ifdef _TD_ + PID_CONTROLLER_ID_BLOOD_FLOW, ///< Blood pump controller to target flow rate +#endif +#ifdef _DD_ + PID_CONTROLLER_ID_RO_PUMP_FLOW, ///< RO Pump controller to flow +#endif + NUM_OF_PID_CONTROLLERS_IDS ///< Number of PID controllers +} PID_CONTROLLER_ID_T; + +/// Enumeration of PI controller signals. +typedef enum ControllerSignals +{ + CONTROLLER_SIGNAL_REFERENCE = 0, ///< Reference value + CONTROLLER_SIGNAL_MEASURED, ///< Measured value + CONTROLLER_SIGNAL_ERROR, ///< Error value + CONTROLLER_SIGNAL_ERROR_SUM, ///< Error sum before anti-windup + CONTROLLER_SIGNAL_ERROR_SUM_AFTER_WINDUP, ///< Error sum after anti-windup + CONTROLLER_SIGNAL_ERROR_PREVIOUS, ///< Previous error signal + CONTROLLER_SIGNAL_PROPORTIONAL_OUTPUT, ///< P portion of controller output signal + CONTROLLER_SIGNAL_INTEGRAL_OUTPUT, ///< I portion of controller output signal + CONTROLLER_SIGNAL_DERIVATIVE_OUTPUT, ///< D portion of controller output signal. + CONTROLLER_SIGNAL_FEEDFORWARD_OUTPUT, ///< Feed forward portion of controller output signal + CONTROLLER_SIGNAL_CONTROL, ///< Controller output signal + NUM_OF_CONTROLLER_SIGNAL ///< Number of PI controller signals +} PID_CONTROLLER_SIGNALS_ID; + +/// Data structure for PI control profiles. +typedef struct +{ + F32 Kp; ///< Proportional Value + F32 Ki; ///< Integral Value + F32 Kd; ///< Derivative Value + F32 uMin; ///< Minimum control signal + F32 uMax; ///< Maximum control signal + F32 maxErrorSumStep; ///< Maximum change in I (error sum) for a single control interval. + U32 controlInterval; ///< Control interval value +} PID_CONTROLLER_PROFILE_DATA_T; + +// ********** public function prototypes ********** + +void initializePIDController( PID_CONTROLLER_ID_T controllerID, F32 initialControlSignal, + F32 kP, F32 kI, F32 controlMin, F32 controlMax, + BOOL isFeedForwardEnabled, F32 feedFowardSignal ); +void resetPIDController( PID_CONTROLLER_ID_T controllerID, F32 initialControlSignal, F32 feedFowardSignal ); +F32 runPIDController( PID_CONTROLLER_ID_T controllerID, F32 referenceSignal, F32 measuredSignal ); +F32 getPIDControllerSignals( PID_CONTROLLER_ID_T controllerID, PID_CONTROLLER_SIGNALS_ID signalID ); +void setPIDControllerStepLimit( PID_CONTROLLER_ID_T controllerID, F32 stepLimit ); + +/**@}*/ + +#endif