Index: firmware/App/Services/PIControllers.c =================================================================== diff -u -ra4236a98b2de7d007173862dfe6b114bce08f90f -r8e7158d8231435496fcf1d5649e51babf859ccc7 --- firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision a4236a98b2de7d007173862dfe6b114bce08f90f) +++ firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision 8e7158d8231435496fcf1d5649e51babf859ccc7) @@ -1,22 +1,21 @@ -/**********************************************************************//** - * - * Copyright (c) 2019-2020 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 source file. - * - **************************************************************************/ +/************************************************************************** +* +* 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 +* +***************************************************************************/ -#ifndef _VECTORCAST_ - #include "math.h" -#endif +#include #include "SystemCommMessages.h" #include "PIControllers.h" @@ -28,56 +27,60 @@ // ********** private definitions ********** -/// minimum integral coefficient - cannot be zero +/// minimum integral coefficient - cannot be zero. #define MIN_KI NEARLY_ZERO -/// record for PI controller +/// 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]) ///< macro to set a local controller pointer to a given piController +#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 - { 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 + * @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; @@ -95,17 +98,17 @@ } else { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) } } /*********************************************************************//** - * @brief resetPIController + * @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 *************************************************************************/ @@ -125,17 +128,17 @@ } else { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + 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 *************************************************************************/ @@ -168,38 +171,21 @@ controller->errorSum -= ( windupError / controller->Ki ); } result = controller->controlSignal; -//#ifdef DEBUG_ENABLED -// { -// // TODO - temporary debug code - remove later -// char debugFlowStr[ 100 ]; -// S32 nums = (S32)(measuredSignal); -// S32 decs = (S32)(fabs(measuredSignal-(S32)(measuredSignal))*100.0); -// S32 nume = (S32)controller->errorSignal; -// S32 dece = (S32)(fabs(controller->errorSignal-(S32)controller->errorSignal)*100.0); -// S32 numes = (S32)controller->errorSum; -// S32 deces = (S32)((controller->errorSum-(S32)(controller->errorSum))*100.0); -// S32 nump = (S32)controller->controlSignal; -// S32 decp = (S32)((controller->controlSignal-(S32)controller->controlSignal)*10000.0); -// -// sprintf( debugFlowStr, "%6d.%02d %6d.%02d %10d.%02d %3d.%04d\n", nums, decs, nume, dece, numes, deces, nump, decp ); -// sendDebugData( (U08*)debugFlowStr, strlen(debugFlowStr) ); -// } -//#endif } else { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + 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 *************************************************************************/ @@ -247,15 +233,17 @@ break; default: - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_SIGNAL, (U32)signalID ) + 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_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_HD_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) } return output; } +/**@}*/ +