Index: firmware/App/Services/PIControllers.c =================================================================== diff -u -r30f049651877229042e3f8700c8596e5b9a1e0f4 -r24b2fe72608344e67ef37234085d15ad5e4fcc37 --- firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision 30f049651877229042e3f8700c8596e5b9a1e0f4) +++ firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision 24b2fe72608344e67ef37234085d15ad5e4fcc37) @@ -1,17 +1,17 @@ /************************************************************************** * -* Copyright (c) 2019-2020 Diality Inc. - All Rights Reserved. +* Copyright (c) 2019-2022 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 +* @file PIControllers.c * -* @author (last) Sean Nash -* @date (last) 13-Aug-2020 +* @author (last) Sean Nash +* @date (last) 02-Jul-2021 * -* @author (original) Sean -* @date (original) 27-Dec-2019 +* @author (original) Sean +* @date (original) 27-Dec-2019 * ***************************************************************************/ @@ -30,48 +30,57 @@ /// Minimum integral coefficient - cannot be zero. #define MIN_KI NEARLY_ZERO -/// Record for PI controller. +/// 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. + 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. + 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 SET_CONTROLLER( c, id ) ((c) = &piControllers[id]) ///< Macro to set a local controller pointer to a given piController. +/// Macro to set a local controller pointer to a given piController. +#define SET_CONTROLLER( c, id ) ((c) = &piControllers[id]) // ********** 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_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 +{ // Kp Ki uMax uMin ref meas err esw esum ctrl Ilimit controller type + { 0.0, 0.0, 0.90, 0.10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 50.0, CONTROLLER_BIDIRECTIONAL }, // PI_CONTROLLER_ID_ULTRAFILTRATION + { 0.0, 0.0, 0.90, 0.10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 100.0, CONTROLLER_UNIDIRECTIONAL }, // 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, 100.0, CONTROLLER_UNIDIRECTIONAL } // PI_CONTROLLER_ID_DIALYSATE_FLOW }; -/**@}*/ - /*********************************************************************//** * @brief * Initialize controller before operation. Make sure to call it before * first call to runController function. - * + * @details Inputs: piControllers + * @details Outputs: piControllers * @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, @@ -106,10 +115,10 @@ * @brief * Reset controller before new set point. Make sure to call it before first * call to runController function. - * + * @details Inputs: piControllers + * @details Outputs: piControllers * @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 ) @@ -135,11 +144,11 @@ /*********************************************************************//** * @brief * Call this function whenever a new measured signal sampled is acquired. - * + * @details Inputs: piControllers + * @details Outputs: piControllers * @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) @@ -148,28 +157,58 @@ 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; + controller->measuredSignal = measuredSignal; + // Calculate error signal - controller->errorSignal = fabs( referenceSignal ) - ( referenceSignal < 0.0 ? ( measuredSignal * -1.0 ) : measuredSignal ); - controller->errorSum += controller->errorSignal; - // Anti-windup - controller->errorSumBeforeWindUp = controller->errorSum; - // Calculate control signal - controlSignalBeforeWindup = ( controller->Kp * controller->errorSignal ) + ( controller->Ki * controller->errorSum ); - controller->controlSignal = RANGE( controlSignalBeforeWindup, controller->uMin, controller->uMax ); + if ( controller->direction == CONTROLLER_UNIDIRECTIONAL ) + { + controller->errorSignal = fabs( referenceSignal ) - ( referenceSignal < 0.0 ? ( measuredSignal * -1.0 ) : measuredSignal ); + } + else + { + controller->errorSignal = referenceSignal - measuredSignal; + } + + // Limit error sum step size + if ( fabs( controller->errorSignal ) > controller->maxErrorSumStep ) + { + if ( controller->errorSignal < 0.0 ) + { + controller->errorSum += ( controller->maxErrorSumStep * -1.0 ); + } + 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 @@ -183,10 +222,10 @@ /*********************************************************************//** * @brief * Returns the latest requested signal sample. - * + * @details Inputs: piControllers + * @details Outputs: piControllers * @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 )