Index: firmware/App/Services/PIControllers.c =================================================================== diff -u -r5217f70ca5c74bd586dc14540e6404b43feea004 -r8e7158d8231435496fcf1d5649e51babf859ccc7 --- firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision 5217f70ca5c74bd586dc14540e6404b43feea004) +++ firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision 8e7158d8231435496fcf1d5649e51babf859ccc7) @@ -1,73 +1,86 @@ -/************************************************************************** - * - * Copyright (c) 2019-2019 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 - * - * @date 18-Dec-2019 - * @author L. Baloa - * - * @brief PIControllers service module. Creates a digital PI to be used as - * control loops - * - **************************************************************************/ +/************************************************************************** +* +* Copyright (c) 2019-2021 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) 13-Aug-2020 +* +* @author (original) Sean +* @date (original) 27-Dec-2019 +* +***************************************************************************/ -#include "math.h" +#include +#include "SystemCommMessages.h" #include "PIControllers.h" +/** + * @addtogroup PIControllers + * @{ + */ + // ********** private definitions ********** +/// minimum integral coefficient - cannot be zero. #define MIN_KI NEARLY_ZERO +/// 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 + 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 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. } PI_CONTROLLER_T; -#define SET_CONTROLLER( c, id ) ((c) = &piControllers[id]) +#define SET_CONTROLLER( c, id ) ((c) = &piControllers[id]) ///< macro to set a local controller pointer to a given piController. // ********** private data ********** -// PI Controllers -- definition - +/// PI Controllers -- initial configurations. static PI_CONTROLLER_T piControllers[ NUM_OF_PI_CONTROLLERS_IDS ] = { // Kp Ki uMax uMin ref meas err esw esum ctrl - { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // PI_CONTROLLER_ID_LOAD_CELL + { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // PI_CONTROLLER_ID_ULTRAFILTRATION { 0.0, 0.0, 0.90, 0.10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // PI_CONTROLLER_ID_BLOOD_FLOW { 0.0, 0.0, 0.90, 0.10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } // PI_CONTROLLER_ID_DIALYSATE_FLOW }; -/************************************************************************* - * @brief initializePIController - * Initialize controller before operation. Make sure to call it before \n +/**@}*/ + +/*********************************************************************//** + * @brief + * Initialize controller before operation. Make sure to call it before * first call to runController function. * - * @param controllerID - ID filter number - * @param initialControlSignal - Value of the output on the first iteration + * @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 ) { - PI_CONTROLLER_T *controller; - if ( controllerID < NUM_OF_PI_CONTROLLERS_IDS ) { + PI_CONTROLLER_T *controller; + SET_CONTROLLER( controller, controllerID ); controller->Kp = kP; @@ -83,15 +96,19 @@ controller->uMax = controlMax; resetPIController( controllerID, initialControlSignal ); } + else + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + } } -/************************************************************************* - * @brief resetPIController - * Reset controller before new set point. Make sure to call it before first \n +/*********************************************************************//** + * @brief + * Reset controller before new set point. Make sure to call it before first * call to runController function. * - * @param controllerID - ID filter number - * @param initialControlSignal - Value of the output on the first iteration + * @param controllerID ID filter number + * @param initialControlSignal Value of the output on the first iteration * * @return none *************************************************************************/ @@ -109,15 +126,19 @@ controller->errorSumBeforeWindUp = controller->errorSum; controller->measuredSignal = 0.0; } + else + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + } } -/************************************************************************* - * @brief runPIController +/*********************************************************************//** + * @brief * Call this function whenever a new measured signal sampled is acquired. * - * @param controllerID - ID filter number - * @param referenceSignal - reference signal value - * @param measuredSignal - latest measured sample + * @param controllerID ID filter number + * @param referenceSignal reference signal value + * @param measuredSignal latest measured sample * * @return value of the control signal *************************************************************************/ @@ -144,23 +165,27 @@ controlSignalBeforeWindup = ( controller->Kp * controller->errorSignal ) + ( controller->Ki * controller->errorSum ); controller->controlSignal = RANGE( controlSignalBeforeWindup, controller->uMin, controller->uMax ); // handle anti-windup for i term - windupError = controller->controlSignal - controlSignalBeforeWindup; + windupError = controlSignalBeforeWindup - controller->controlSignal; if ( fabs( windupError ) > NEARLY_ZERO ) { controller->errorSum -= ( windupError / controller->Ki ); } result = controller->controlSignal; } + else + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + } return result; } -/************************************************************************* - * @brief getPIControllerSignals +/*********************************************************************//** + * @brief * Returns the latest requested signal sample. * - * @param controllerID - ID filter number - * @param signalID - signal sample ID request + * @param controllerID ID filter number + * @param signalID signal sample ID request * * @return latest sample requested *************************************************************************/ @@ -208,11 +233,17 @@ break; default: - output = 0; + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_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_HD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + } return output; } +/**@}*/ +