/* * Copyright (C) 2023 Texas Instruments Incorporated * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the * distribution. * * Neither the name of Texas Instruments Incorporated nor the names of * its contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPgResS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #ifndef _DCL_PI2_H_ #define _DCL_PI2_H_ #ifdef __cplusplus extern "C" { #endif /** * \addtogroup DCL_API_MODULE APIs for Digital Control Library * @{ * \file dcl_pi2.h * \brief Contains PI2 (double integrator) controller * with its related structures and functions */ #include "../dcl_common.h" //--- Linear PI2 controller -------------------------------------------------- //! \brief Defines the DCL_PI2 shadow parameter set //! used for updating controller parameter //! typedef struct dcl_pi2_sps { float32_t Kp; //!< Proportional gain float32_t Ki; //!< Integral gain float32_t Umax; //!< Upper saturation limit float32_t Umin; //!< Lower saturation limit } DCL_PI2_SPS; //! \brief Defines default values to initialize DCL_PI2_SPS //! #define PI2_SPS_DEFAULTS { 1.0f, 0.0f, 1.0f, -1.0f } //! \brief PI2 object for storing PI2 specific parameters //! PI2 - PI controller with double integrators //! typedef _DCL_VOLATILE struct dcl_pi2 { /* controller parameter */ float32_t Kp; //!< Proportional gain float32_t Ki; //!< Integral gain float32_t Umax; //!< Upper saturation limit float32_t Umin; //!< Lower saturation limit /* internal storage */ float32_t i6; //!< Integrator 1 storage float32_t i9; //!< Integrator 2 storage float32_t i12; //!< Saturation 1 storage float32_t i13; //!< Saturation 2 storage /* miscellaneous */ DCL_PI2_SPS *sps; //!< updates controller parameter DCL_CSS *css; //!< configuration & debugging } DCL_PI2, *PI2_Handle; //! \brief Defines default values to initialize DCL_PI2 //! #define PI2_DEFAULTS { 1.0f, 0.0f, 1.0f, -1.0f, 0.0f, 0.0f, 1.0f, 1.0f, \ &(DCL_PI2_SPS)PI2_SPS_DEFAULTS, &(DCL_CSS)DCL_CSS_DEFAULTS } //! \brief Macro for internal default values to initialize DCL_PI2 //! Example: DCL_PI2 pi2_ctrl = { //! .Kp = 1.0f, //! .Ki = 0.0f, //! .Umax = 1.0f, //! .Umin = -1.0f, //! PI2_INT_DEFAULTS //! }; #define PI2_INT_DEFAULTS .i6=0.0f, .i9=0.0f, .i12=1.0f, .i13=1.0f, \ .sps=&(DCL_PI2_SPS)PI2_SPS_DEFAULTS, .css=&(DCL_CSS)DCL_CSS_DEFAULTS //! \brief Initialize DCL_PI2 struct with default parameters //! Example: DCL_PI2 pi2_ctrl = DCL_initPI2(); //! //! \return A DCL_PI2* pointer //! #define DCL_initPI2() &(DCL_PI2)PI2_DEFAULTS //! \brief Initialize DCL_PI2 struct with input controller parameters //! Example: DCL_PI2* pi2_ctrl = DCL_initPI2asParam(1.0f,0.0f,1.0f,-1.0f); //! Note: input parameter needs to be in the same order as listed in PI2_SPS struct //! //! \return A DCL_PI2* pointer //! #define DCL_initPI2asParam(kp,ki,umax,umin) &(DCL_PI2){ .Kp=kp, .Ki=ki, \ .Umax=umax, .Umin=umin, PI2_INT_DEFAULTS } //! \brief Initialize DCL_PI2 struct with sps parameters //! Example: DCL_PI2_SPS pi2_sps = { .Kp = , .Ki = , ...}; //! DCL_PI2 pi2_ctrl; //! DCL_initPI2asSPS(&pi2_ctrl,&pi2_sps); //! //! \param[in] pi2_ptr DCL_PI2* pointer that needs to be initialized //! \param[in] sps_ptr DCL_PI2_SPS* pointer with assigned parameters //! \return Returns DCL_PI2* with set sps parameters, default parameter will be used //! if sps_ptr is not specified //! #define DCL_initPI2asSPS(pi2_ptr,sps_ptr) \ ({ \ DCL_PI2* new_pi = (pi2_ptr) ? pi2_ptr : DCL_initPI2(); \ DCL_PI2_SPS* new_sps = (sps_ptr) ? sps_ptr : &(DCL_PI2_SPS)PI2_SPS_DEFAULTS; \ if(sps_ptr) \ { \ *new_pi = (DCL_PI){ (new_sps)->Kp, (new_sps)->Ki, (new_sps)->Umax, (new_sps)->Umin,\ 0.0f, 0.0f, 1.0f, 1.0f, (DCL_PI_SPS*)new_sps, &(DCL_CSS)DCL_CSS_DEFAULTS }; \ } \ new_pi; \ }) //! \brief Resets PI2 internal storage data with interrupt protection //! //! \param[in] pi2 Pointer to the DCL_PI2 structure //! _DCL_CODE_ACCESS void DCL_resetPI2(DCL_PI2 *pi2) { dcl_interrupt_t ints = DCL_disableInts(); pi2->i6 = pi2->i9 = 0.0f; pi2->i12 = pi2->i13 = 1.0f; DCL_restoreInts(ints); } //! \brief Loads PI2 tuning parameter from its SPS parameter //! //! \param[in] pi2 Pointer to the DCL_PI2 controller structure //! _DCL_CODE_ACCESS void DCL_fupdatePI2(DCL_PI2 *pi2) { #ifdef DCL_ERROR_HANDLING_ENABLED uint32_t err_code = dcl_none; err_code |= (pi2->sps->Umax <= pi2->sps->Umin) ? dcl_param_invalid_err : dcl_none; err_code |= (pi2->css->t_sec <= 0.0f) ? dcl_param_range_err : dcl_none; err_code |= (pi2->sps->Kp < 0.0f) ? dcl_param_range_err : dcl_none; err_code |= (pi2->sps->Ki < 0.0f) ? dcl_param_range_err : dcl_none; if (err_code) { DCL_setError(pi2,err_code); DCL_getErrorInfo(pi2); DCL_runErrorHandler(pi2); } #endif pi2->Ki = pi2->sps->Ki; pi2->Kp = pi2->sps->Kp; pi2->Umax = pi2->sps->Umax; pi2->Umin = pi2->sps->Umin; } //! \brief Updates PI2 parameter from its SPS parameter with interrupt protection //! //! \param[in] pi2 Pointer to the DCL_PI2 controller structure //! \return 'true' if update is successful, otherwise 'false' //! _DCL_CODE_ACCESS _DCL_CODE_SECTION bool DCL_updatePI2(DCL_PI2 *pi2) { #ifdef DCL_ERROR_HANDLING_ENABLED uint32_t err_code = dcl_none; err_code |= (pi2->sps->Umax <= pi2->sps->Umin) ? dcl_param_invalid_err : dcl_none; err_code |= (pi2->css->t_sec <= 0.0f) ? dcl_param_range_err : dcl_none; err_code |= (pi2->sps->Kp < 0.0f) ? dcl_param_range_err : dcl_none; err_code |= (pi2->sps->Ki < 0.0f) ? dcl_param_range_err : dcl_none; if (err_code) { DCL_setError(pi2,err_code); DCL_getErrorInfo(pi2); DCL_runErrorHandler(pi2); } #endif if (!DCL_getUpdateStatus(pi2)) { dcl_interrupt_t ints = DCL_disableInts(); DCL_setUpdateStatus(pi2); pi2->Ki = pi2->sps->Ki; pi2->Kp = pi2->sps->Kp; pi2->Umax = pi2->sps->Umax; pi2->Umin = pi2->sps->Umin; DCL_clearUpdateStatus(pi2); DCL_restoreInts(ints); return true; } return false; } //! \brief A conditional update based on the pending-for-update flag. //! If the pending status is set, the function will update PI2 //! parameter from its SPS parameter and clear the status flag on completion. //! Note: Use DCL_setPendingStatus(pi2) to set the pending status. //! //! \param[in] pi2 Pointer to the DCL_PI2 controller structure //! \return 'true' if an update is applied, otherwise 'false' //! _DCL_CODE_ACCESS _DCL_CODE_SECTION bool DCL_pendingUpdatePI2(DCL_PI2 *pi2) { if (DCL_getPendingStatus(pi2) && DCL_updatePI2(pi2)) { DCL_clearPendingStatus(pi2); return true; } return false; } //! \brief Update SPS parameter with active param, userful when needing //! to update only few active param from SPS and keep rest the same //! //! \param[in] pi2 Pointer to the active DCL_PI controller structure //! _DCL_CODE_ACCESS void DCL_updatePI2SPS(DCL_PI2 *pi2) { pi2->sps->Kp = pi2->Kp; pi2->sps->Ki = pi2->Ki; pi2->sps->Umax = pi2->Umax; pi2->sps->Umin = pi2->Umin; } //! \brief Executes an inline series form PI2 controller on the FPU32 //! //! \param[in] pi2 Pointer to the DCL_PI2 structure //! \param[in] rk The controller set-point reference //! \param[in] yk The measured feedback //! \return The control effort //! _DCL_CODE_ACCESS _DCL_CODE_SECTION float32_t DCL_runPI2Series(DCL_PI2 *pi2, float32_t rk, float32_t yk) { float32_t v1, v2, v5, v8, v10, v11, v14; bool l1, l2, l3, l4, l5, l6; v1 = rk - yk; v2 = pi2->Kp * v1; v5 = (v1 * pi2->Ki * pi2->i12) + pi2->i6; pi2->i6 = v5; v8 = (v5 * pi2->i13) + pi2->i9; pi2->i9 = v8; v10 = v2 + v8; v11 = DCL_runSat(v10,pi2->Umax,pi2->Umin); v14 = v10 - v11; l1 = (v1 > 0.0f) ? true : false; l2 = (v14 > 0.0f) ? true : false; l3 = (v14 == 0.0f) ? true : false; l4 = (v5 > 0.0f) ? true : false; l5 = l3 || (l1 ^ l2); l6 = l3 || (l4 ^ l2); pi2->i12 = (l5) ? 1.0f : 0.0f; pi2->i13 = (l6) ? 1.0f : 0.0f; #ifdef DCL_TESTPOINTS_ENABLED pi2->css->tpt = v8; #endif return(v11); } /** @} */ #ifdef __cplusplus } #endif // extern "C" #endif // _DCL_PI2_H_