Index: PIControllers.c =================================================================== diff -u --- PIControllers.c (revision 0) +++ PIControllers.c (revision 5c486b49eeca18ec83b7d776d9052dd38800f7d0) @@ -0,0 +1,375 @@ +/************************************************************************** +* +* Copyright (c) 2024-2024 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) Vinayakam Mani +* @date (last) 02-Oct-2024 +* +* @author (original) Vinayakam Mani +* @date (original) 02-Oct-2024 +* +***************************************************************************/ + +#include + +#include "Messaging.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 +#ifdef _DD_ + { 0.0, 0.0, 4500, 300, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, CONTROLLER_BIDIRECTIONAL }, // PI_CONTROLLER_ID_FRESH_DIALYSATE_PUMP + { 0.0, 0.0, 4500, 300, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, CONTROLLER_BIDIRECTIONAL }, // PI_CONTROLLER_ID_SPENT_DIALYSATE_PUMP +#endif +#ifdef _RO_ + { 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, 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 +#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 + * @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 + { +#ifdef _DD_ + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) +#endif +#ifdef _RO_ + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_RO_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 + * @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 + { +#ifdef _DD_ + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) +#endif +#ifdef _RO_ + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_RO_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 ); + 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 + { +#ifdef _DD_ + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) +#endif +#ifdef _RO_ + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_RO_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_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 +#ifdef _RO_ + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_RO_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 +#ifdef _RO_ + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_RO_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 +#ifdef _RO_ + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_RO_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 +#ifdef _RO_ + // Invalid controller given + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_RO_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) +#endif + } +} + +/**@}*/ Index: PIControllers.h =================================================================== diff -u --- PIControllers.h (revision 0) +++ PIControllers.h (revision 5c486b49eeca18ec83b7d776d9052dd38800f7d0) @@ -0,0 +1,91 @@ +/************************************************************************** +* +* Copyright (c) 2024-2024 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.h +* +* @author (last) Vinayakam Mani +* @date (last) 02-Oct-2024 +* +* @author (original) Vinayakam Mani +* @date (original) 02-Oct-2024 +* +***************************************************************************/ + +#ifndef __PI_CONTROLLERS_H__ +#define __PI_CONTROLLERS_H__ + +#ifdef _TD_ +#include "TDCommon.h" +#endif +#ifdef _DD_ +#include "DDCommon.h" +#endif +#ifdef _RO_ +#include "ROCommon.h" +#endif + +/** + * @defgroup PIControllers PIControllers + * @brief PIControllers common module. Provides PI controllers for various actuators. + * + * @addtogroup PIControllers + * @{ + */ + +// ********** public definitions ********** + +/// Enumeration of PI controllers. +typedef enum ControllerList +{ +#ifdef _DD_ + PI_CONTROLLER_ID_FRESH_DIALYSATE_PUMP, ///< Fresh dialysate Pump controller + PI_CONTROLLER_ID_SPENT_DIALYSATE_PUMP, ///< Spent dialysate Pump controller +#endif +#ifdef _RO_ + PI_CONTROLLER_ID_RO_PUMP_FLOW = 0, ///< RO Pump controller to flow + PI_CONTROLLER_ID_RO_PUMP_MAX_PRES, ///< RO pump controller to maximum pressure +#endif + NUM_OF_PI_CONTROLLERS_IDS ///< Number of PI controllers +} PI_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_PROPORTIONAL_OUTPUT, ///< P portion of controller output signal + CONTROLLER_SIGNAL_INTEGRAL_OUTPUT, ///< I portion of controller output signal + CONTROLLER_SIGNAL_CONTROL, ///< Controller output signal + NUM_OF_CONTROLLER_SIGNAL ///< Number of PI controller signals +} PI_CONTROLLER_SIGNALS_ID; + +/// Data structure for PI control profiles. +typedef struct +{ + F32 Kp; ///< Proportional Value + F32 Ki; ///< Integral 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 +} PI_CONTROLLER_PROFILE_DATA_T; + +// ********** public function prototypes ********** + +void initializePIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal, + F32 kP, F32 kI, F32 controlMin, F32 controlMax ); +void resetPIController( PI_CONTROLLER_ID_T controllerID, F32 initialControlSignal ); +F32 runPIController( PI_CONTROLLER_ID_T controllerID, F32 referenceSignal, F32 measuredSignal ); +F32 getPIControllerSignals( PI_CONTROLLER_ID_T controllerID, PI_CONTROLLER_SIGNALS_ID signalID ); +void setPIControllerStepLimit( PI_CONTROLLER_ID_T controllerID, F32 stepLimit ); + +/**@}*/ + +#endif Index: Utilities.h =================================================================== diff -u -r2801d97e877dd78189aa891e80a2f7cf60a6a2b7 -r5c486b49eeca18ec83b7d776d9052dd38800f7d0 --- Utilities.h (.../Utilities.h) (revision 2801d97e877dd78189aa891e80a2f7cf60a6a2b7) +++ Utilities.h (.../Utilities.h) (revision 5c486b49eeca18ec83b7d776d9052dd38800f7d0) @@ -58,10 +58,9 @@ TIME_WINDOWED_COUNT_BATT_COMM_ERROR, ///< Battery communication error #endif #ifdef _DD_ - TIME_WINDOWED_COUNT_FPGA_CONDUCTIVITY_SENSOR_ERROR, ///< FPGA Conductivity sensor error - TIME_WINDOWED_COUNT_FPGA_TEMPERATURE_SENSOR_ERROR, ///< FPGA Temperature sensor error - TIME_WINDOWED_COUNT_FPGA_COMM_FAILURES, ///< FPGA communication failure (NAK, CRC, timeout) - TIME_WINDOWED_COUNT_FPGA_DRAIN_PUMP_HALL_SENSOR_ERROR, ///< FPGA Drain pump hall sensor failure + TIME_WINDOWED_COUNT_FPGA_COMM_FAILURES, ///< FPGA communication failure (NAK, CRC, timeout) + TIME_WINDOWED_COUNT_FPGA_FRESH_DIALYSATE_PUMP_HALL_SENSOR_ERROR,///< FPGA fresh dialysate pump hall sensor failure + TIME_WINDOWED_COUNT_FPGA_SPENT_DIALYSATE_PUMP_HALL_SENSOR_ERROR,///< FPGA spent dialysate pump hall sensor failure #endif NUM_OF_TIME_WINDOWED_COUNTS ///< Number of pressure sensors } TIME_WINDOWED_COUNT_T;