Index: firmware/App/Services/PIControllers.c =================================================================== diff -u -rf308cc4c35eab630ebbbde405cfe47d049afeafb -rfbc0a281b094ff309dcbf83db1878818a9b384f1 --- firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision f308cc4c35eab630ebbbde405cfe47d049afeafb) +++ firmware/App/Services/PIControllers.c (.../PIControllers.c) (revision fbc0a281b094ff309dcbf83db1878818a9b384f1) @@ -8,238 +8,258 @@ * @file PIControllers.c * * @author (last) Quang Nguyen -* @date (last) 22-Jul-2020 +* @date (last) 24-Aug-2020 * * @author (original) Sean * @date (original) 04-Feb-2020 * ***************************************************************************/ - -#include "math.h" - -#include "SystemCommMessages.h" -#include "PIControllers.h" - -/** - * @addtogroup PIControllers - * @{ - */ - -// ********** private definitions ********** - -#define MIN_KI NEARLY_ZERO ///< minimum integral coefficient - cannot be 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. - // -- 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. -} PI_CONTROLLER_T; - -#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_RO_PUMP - { 0.0, 0.0, 0.89, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, // PI_CONTROLLER_ID_PRIMARY_HEATER - { 0.0, 0.0, 0.50, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } // PI_CONTROLLER_ID_TRIMMER_HEATER -}; - -/*********************************************************************//** - * @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 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 ) - { - 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 - { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) - } -} - -/*********************************************************************//** - * @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 - * - * @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 - { - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) - } -} - -/*********************************************************************//** - * @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 - * - * @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 - 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 ); - // handle anti-windup for i term - 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_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) - } - - return result; -} - -/*********************************************************************//** - * @brief - * Returns the latest requested signal sample. - * - * @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: - SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_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_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) - } - - return output; -} - -/**@}*/ + +#include "math.h" + +#include "SystemCommMessages.h" +#include "PIControllers.h" + +/** + * @addtogroup PIControllers + * @{ + */ + +// ********** private definitions ********** + +/// Enumeration of PI controller direction. +typedef enum controller_Directions +{ + CONTROLLER_BIDIRECTIONAL = 0, ///< Controller runs bidirectional so it only covers positive and negative values + CONTROLLER_UNIDIRECTIONAL, ///< Controller run unidirectional so it only covers positive values + 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. + 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 + { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, CONTROLLER_UNIDIRECTIONAL }, // PI_CONTROLLER_ID_RO_PUMP + { 0.0, 0.0, 3000, 300, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, CONTROLLER_BIDIRECTIONAL }, // PI_CONTROLLER_ID_DRAIN_PUMP + { 0.0, 0.0, 1.39, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, CONTROLLER_UNIDIRECTIONAL }, // PI_CONTROLLER_ID_PRIMARY_HEATER + { 0.0, 0.0, 0.50, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, CONTROLLER_UNIDIRECTIONAL }, // PI_CONTROLLER_ID_TRIMMER_HEATER + { 0.0, 0.0, 0.99, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, CONTROLLER_UNIDIRECTIONAL }, // I_CONTROLLER_ID_RO_PUMP_RAMP_UP +}; + +/*********************************************************************//** + * @brief + * The initializePIController function initializes controller before operation. + * Make sure to call it before first call to runController function. + * @details Inputs: none + * @details Outputs: PI controllers module initialized + * @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 ) + { + 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 + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + } +} + +/*********************************************************************//** + * @brief + * The resetPIController functions resets controller before new set point. + * Make sure to call it before first call to runController function. + * @details Inputs: none + * @details Outputs: Reset a PI controller + * @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 + { + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + } +} + +/*********************************************************************//** + * @brief + * The runPIController functions executes a PI controller with a new sampled measured signal. + * @details Inputs: none + * @details Outputs: Feeds new signal to PI controller + * @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 ) + { + // To make sure that the outcome will be positive + controller->errorSignal = fabs( referenceSignal ) - ( referenceSignal < 0.0 ? ( measuredSignal * -1.0 ) : measuredSignal ); + } + else + { + controller->errorSignal = referenceSignal - 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 ); + // handle anti-windup for i term + 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_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + } + + return result; +} + +/*********************************************************************//** + * @brief + * The getPIControllerSignals function returns the latest requested signal sample. + * @details Inputs: none + * @details Outputs: none + * @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: + SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_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_DG_SOFTWARE_FAULT, SW_FAULT_ID_PI_CTRL_INVALID_CONTROLLER, (U32)controllerID ) + } + + return output; +} + +/**@}*/