/* DCLCLA.h - C2000 Digital Controller Library header file * */ //############################################################################# //! //! Copyright: Copyright (C) 2023 Texas Instruments Incorporated - //! All rights reserved not granted herein. //! Limited License. //! //! Texas Instruments Incorporated grants a world-wide, royalty-free, //! non-exclusive license under copyrights and patents it now or hereafter //! owns or controls to make, have made, use, import, offer to sell and sell //! ("Utilize") this software subject to the terms herein. With respect to the //! foregoing patent license, such license is granted solely to the extent that //! any such patent is necessary to Utilize the software alone. The patent //! license shall not apply to any combinations which include this software, //! other than combinations with devices manufactured by or for TI //! ("TI Devices"). //! No hardware patent is licensed hereunder. //! //! Redistributions must preserve existing copyright notices and reproduce this //! license (including the above copyright notice and the disclaimer and //! (if applicable) source code license limitations below) in the documentation //! and/or other materials provided with the distribution. //! //! Redistribution and use in binary form, without modification, are permitted //! provided that the following conditions are met: //! //! * No reverse engineering, decompilation, or disassembly of this software is //! permitted with respect to any software provided in binary form. //! * Any redistribution and use are licensed by TI for use only //! with TI Devices. //! * Nothing shall obligate TI to provide you with source code for the //! software licensed and provided to you in object code. //! //! If software source code is provided to you, modification and redistribution //! of the source code are permitted provided that the following conditions //! are met: //! //! * any redistribution and use of the source code, including any resulting //! derivative works, are licensed by TI for use only with TI Devices. //! * any redistribution and use of any object code compiled from the source //! code and any resulting derivative works, are licensed by TI for use //! only with TI Devices. //! //! Neither the name of Texas Instruments Incorporated nor the names of its //! suppliers may be used to endorse or promote products derived from this //! software without specific prior written permission. //############################################################################# #ifndef _C_DCLCLA_H #define _C_DCLCLA_H //! \file DCLCLA.h //! \brief Contains the public interface to the //! Digital Controller Library CLA functions #include "DCL.h" //--- Linear PID controller --------------------------------------------------- //! \brief Defines the DCL_PID_CLA controller structure //! typedef struct { float32_t Kp; //!< Proportional gain float32_t Ki; //!< Integral gain float32_t Kd; //!< Derivative gain float32_t Kr; //!< Set point weight float32_t c1; //!< D-term filter coefficient 1 float32_t c2; //!< D-term filter coefficient 2 float32_t d2; //!< D-term filter intermediate storage 1 float32_t d3; //!< D-term filter intermediate storage 2 float32_t i10; //!< I-term intermediate storage float32_t i14; //!< Intermediate saturation storage float32_t Umax; //!< Upper saturation limit float32_t Umin; //!< Lower saturation limit } DCL_PID_CLA; //! \brief Defines default values to initialize the DCL_PID structure //! #define PID_CLA_DEFAULTS { 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, \ 0.0f, 0.0f, 1.0f, -1.0f } //! \brief Executes an ideal form PID controller on the CLA //! \param[in] p Pointer to the DCL_PID_CLA structure //! \param[in] rk The controller set-point reference //! \param[in] yk The measured feedback value //! \param[in] lk External output clamp flag //! \return The control effort //! extern float32_t DCL_runPID_L1(DCL_PID_CLA *p, float32_t rk, float32_t yk, float32_t lk); //! \brief Executes a parallel form PID controller on the CLA //! \param[in] p Pointer to the DCL_PID_CLA structure //! \param[in] rk The controller set-point reference //! \param[in] yk The measured feedback value //! \param[in] lk External output clamp flag //! \return The control effort //! extern float32_t DCL_runPID_L2(DCL_PID_CLA *p, float32_t rk, float32_t yk, float32_t lk); //--- Linear PI controller ---------------------------------------------------- //! \brief Defines the DCL_PI_CLA controller structure //! typedef struct { float32_t Kp; //!< Proportional gain float32_t Ki; //!< Integral gain float32_t i10; //!< I storage float32_t Umax; //!< Upper control saturation limit float32_t Umin; //!< Lower control saturation limit float32_t i6; //!< Saturation storage float32_t i11; //!< I storage float32_t Imax; //!< Upper integrator saturation limit float32_t Imin; //!< Lower integrator saturation limit } DCL_PI_CLA; //! \brief Defines default values to initialize the PI_CLA structure //! #define PI_CLA_DEFAULTS { 1.0f, 0.0f, 0.0f, 1.0f, -1.0f, 1.0f, 1.0f, -1.0f } //! \brief Executes a series form PI controller on the CLA //! \param[in] p Pointer to the DCL_PI_CLA structure //! \param[in] rk The controller set-point reference //! \param[in] yk The measured feedback value //! \return The control effort //! extern float32_t DCL_runPI_L1(DCL_PI_CLA *p, float32_t rk, float32_t yk); //! \brief Executes a parallel form PI controller on the CLA //! \param[in] p Pointer to the DCL_PI_CLA structure //! \param[in] rk The controller set-point reference //! \param[in] yk The measured feedback value //! \return The control effort //! extern float32_t DCL_runPI_L2(DCL_PI_CLA *p, float32_t rk, float32_t yk); //! \brief Executes an inline series form PI controller on the CLA //! Implemented as inline C function //! \param[in] p Pointer to the DCL_PI structure //! \param[in] rk The controller set-point reference //! \param[in] yk The measured feedback value //! \return The control effort //! static inline float32_t DCL_runPI_L3(DCL_PI_CLA *p, float32_t rk, float32_t yk) { float32_t v2, v4, v5, v9; asm(" MSETFLG RNDF32=1"); v2 = p->Kp * (rk - yk); v4 = p->i10 + (p->Ki * p->i6 * v2); v5 = v2 + v4; v9 = (v5 > p->Umax) ? p->Umax : v5; v9 = (v9 < p->Umin) ? p->Umin : v9; p->i10 = v4; p->i6 = (v5 == v9) ? 1.0f : 0.0f; return(v9); } //! \brief Executes an parallel form PI controller on the CLA //! Implemented as inline C function //! \param[in] p Pointer to the DCL_PI structure //! \param[in] rk The controller set-point reference //! \param[in] yk The measured feedback value //! \return The control effort //! static inline float32_t DCL_runPI_L4(DCL_PI_CLA *p, float32_t rk, float32_t yk) { float32_t v1, v2, v4, v5, v9; asm(" MSETFLG RNDF32=1"); v1 = rk - yk; v2 = p->Kp * v1; v4 = (v1 * p->Ki * p->i6) + p->i10; p->i10 = v4; v5 = v2 + v4; v9 = (v5 > p->Umax) ? p->Umax : v5; v9 = (v9 < p->Umin) ? p->Umin : v9; p->i6 = (v5 == v9) ? 1.0f : 0.0f; return(v9); } //! \brief Executes a series form PI controller with Tustin integrator //! on the CLA. //! \param[in] p Pointer to the DCL_PI_CLA structure //! \param[in] rk The controller set-point reference //! \param[in] yk The measured feedback value //! \return The control effort //! static inline float32_t DCL_runPI_L5(DCL_PI_CLA *p, float32_t rk, float32_t yk) { float32_t v2, v4, v5, v8, v9; asm(" MSETFLG RNDF32=1"); v2 = (rk - yk) * p->Kp; v8 = v2 * p->Ki * p->i6; v4 = v8 + p->i11 + p->i10; v5 = v2 + v4; p->i10 = v4; p->i11 = v8; v9 = (v5 > p->Umax) ? p->Umax : v5; v9 = (v9 < p->Umin) ? p->Umin : v9; p->i6 = (v5 == v9) ? 1.0f : 0.0f; return(v9); } //--- Direct Form 1 - 1st order ----------------------------------------------- //! \brief Defines the DCL_DF11 controller structure //! typedef struct { float32_t b0; //!< b0 float32_t b1; //!< b1 float32_t a1; //!< a1 float32_t d1; //!< e(k-1) float32_t d2; //!< u(k-1) } DCL_DF11_CLA; //! \brief Defines default values to initialize the DCL_DF11_CLA structure //! #define DF11_CLA_DEFAULTS { 0.5f, 0.5f, 1.0f, 0.0f, 0.0f } //! \brief Executes a 1st order Direct Form 1 controller on the CLA //! \param[in] p Pointer to the DCL_DF11_CLA controller structure //! \param[in] ek The servo error //! \return The control effort //! extern float32_t DCL_runDF11_L1(DCL_DF11_CLA *p, float32_t ek); //--- Direct Form 1 - 3rd order ----------------------------------------------- //! \brief Defines the DCL_DF13_CLA controller structure //! typedef struct { // coefficients float32_t b0; //!< b0 float32_t b1; //!< b1 float32_t b2; //!< b2 float32_t b3; //!< b3 float32_t a0; //!< a0 float32_t a1; //!< a1 float32_t a2; //!< a2 float32_t a3; //!< a3 //data float32_t d0; //!< e(k) float32_t d1; //!< e(k-1) float32_t d2; //!< e(k-2) float32_t d3; //!< e(k-3) float32_t d4; //!< u(k) float32_t d5; //!< u(k-1) float32_t d6; //!< u(k-2) float32_t d7; //!< u(k-3) } DCL_DF13_CLA; //! \brief Defines default values to initialize the DCL_DF13_CLA structure //! #define DF13_CLA_DEFAULTS { 0.25f, 0.25f, 0.25f, 0.25f, 0.0f, 0.0f, 0.0f, 0.0f, \ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f } //! \brief Executes a full 3rd order Direct Form 1 controller on the CLA //! \param[in] p Pointer to the DCL_DF13_CLA controller structure //! \param[in] ek The servo error //! \return The control effort //! extern float32_t DCL_runDF13_L1(DCL_DF13_CLA *p, float32_t ek); //! \brief Executes an immediate 3rd order Direct Form 1 controller on the CLA //! \param[in] p Pointer to the DCL_DF13_CLA controller structure //! \param[in] ek The servo error //! \param[in] vk The partial pre-computed control effort //! \return The control effort //! extern float32_t DCL_runDF13_L2(DCL_DF13_CLA *p, float32_t ek, float32_t vk); //! \brief Executes a partial pre-computed 3rd order Direct Form 1 controller on the CLA //! \param[in] p Pointer to the DCL_DF13_CLA controller structure //! \param[in] ek The servo error //! \param[in] uk The controller output in the previous sample interval //! \return The control effort //! extern float32_t DCL_runDF13_L3(DCL_DF13_CLA *p, float32_t ek, float32_t uk); //! \brief Executes a full 3rd order Direct Form 1 controller on the CLA //! Implemented as inline C function //! Note: d0 not used //! \param[in] p Pointer to the DCL_DF13_CLA controller structure //! \param[in] ek The servo error //! \return The control effort //! static inline float32_t DCL_runDF13_L4(DCL_DF13_CLA *p, float32_t ek) { asm(" MSETFLG RNDF32=1"); p->d4 = (ek * p->b0) + (p->d1 * p->b1) + (p->d2 * p->b2) + (p->d3 * p->b3) - (p->d5 * p->a1) - (p->d6 * p->a2) - (p->d7 * p->a3); p->d3 = p->d2; p->d2 = p->d1; p->d1 = ek; p->d7 = p->d6; p->d6 = p->d5; p->d5 = p->d4; return(p->d4); } //! \brief Executes an immediate 3rd order Direct Form 1 controller on the CLA //! Implemented as inline C function //! \param[in] p Pointer to the DCL_DF13_CLA controller structure //! \param[in] ek The servo error //! \param[in] vk The partial pre-computed control effort //! \return The control effort //! static inline float32_t DCL_runDF13_L5(DCL_DF13_CLA *p, float32_t ek, float32_t vk) { asm(" MSETFLG RNDF32=1"); p->d4 = (ek * p->b0) + vk; return(p->d4); } //! \brief Executes a partial pre-computed 3rd order Direct Form 1 controller on the CLA //! Implemented as inline C function //! Note: d0 not used //! \param[in] p Pointer to the DCL_DF13_CLA controller structure //! \param[in] ek The servo error //! \param[in] uk The controller output in the previous sample interval //! \return The control effort //! static inline float32_t DCL_runDF13_L6(DCL_DF13_CLA *p, float32_t ek, float32_t uk) { float32_t v9; asm(" MSETFLG RNDF32=1"); v9 = (ek * p->b1) + (p->d1 * p->b2) + (p->d2 * p->b3) - (uk * p->a1) - (p->d5 * p->a2) - (p->d6 * p->a3); p->d2 = p->d1; p->d1 = ek; p->d6 = p->d5; p->d5 = uk; return(v9); } //--- Direct Form 2 - 2nd order ----------------------------------------------- //! \brief Defines the DCL_DF22_CLA controller structure //! typedef struct { float32_t b0; //!< b0 float32_t b1; //!< b1 float32_t b2; //!< b2 float32_t a1; //!< a1 float32_t a2; //!< a2 float32_t x1; //!< x1 float32_t x2; //!< x2 } DCL_DF22_CLA; //! \brief Defines default values to initialize the DCL_DF22_CLA structure //! #define DF22_CLA_DEFAULTS { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f } //! \brief Executes a full 2nd order Direct Form 2 controller on the CLA //! \param[in] p Pointer to the DCL_DF22_CLA controller structure //! \param[in] ek The servo error //! \return The control effort //! extern float32_t DCL_runDF22_L1(DCL_DF22_CLA *p, float32_t ek); //! \brief Executes an immediate 2nd order Direct Form 2 controller on the CLA //! \param[in] p Pointer to the DCL_DF22_CLA controller structure //! \param[in] ek The servo error //! \return The control effort //! extern float32_t DCL_runDF22_L2(DCL_DF22_CLA *p, float32_t ek); //! \brief Executes a partial pre-computed 2nd order Direct Form 2 controller on the CLA //! \param[in] p Pointer to the DCL_DF22_CLA controller structure //! \param[in] ek The servo error //! \param[in] uk The controller output in the previous sample interval //! extern void DCL_runDF22_L3(DCL_DF22_CLA *p, float32_t ek, float32_t uk); //! \brief Executes a full 2nd order Direct Form 2 controller on the CLA //! Implemented as inline C function //! \param[in] p Pointer to the DCL_DF22 controller structure //! \param[in] ek The servo error //! \return The control effort //! static inline float32_t DCL_runDF22_L4(DCL_DF22_CLA *p, float32_t ek) { float32_t v7; asm(" MSETFLG RNDF32=1"); v7 = (ek * p->b0) + p->x1; p->x1 = (ek * p->b1) + p->x2 - (v7 * p->a1); p->x2 = (ek * p->b2) - (v7 * p->a2); return(v7); } //--- Direct Form 2 - 3rd order ----------------------------------------------- //! \brief Defines the DCL_DF23_CLA controller structure //! typedef struct { float32_t b0; //!< b0 float32_t b1; //!< b1 float32_t b2; //!< b2 float32_t b3; //!< b3 float32_t a1; //!< a1 float32_t a2; //!< a2 float32_t a3; //!< a3 float32_t x1; //!< x1 float32_t x2; //!< x2 float32_t x3; //!< x3 } DCL_DF23_CLA; //! \brief Defines default values to initialize the DCL_DF23_CLA structure //! #define DF23_CLA_DEFAULTS { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f } //! \brief Executes a full 3rd order Direct Form 2 controller on the CLA //! \param[in] p Pointer to the DCL_DF23_CLA controller structure //! \param[in] ek The servo error //! \return The control effort //! extern float32_t DCL_runDF23_L1(DCL_DF23_CLA *p, float32_t ek); //! \brief Executes an immediate 3rd order Direct Form 2 controller on the CLA //! \param[in] p Pointer to the DCL_DF23_CLA controller structure //! \param[in] ek The servo error //! \return The control effort //! extern float32_t DCL_runDF23_L2(DCL_DF23_CLA *p, float32_t ek); //! \brief Executes a partial pre-computed 2nd order Direct Form 2 controller on the CLA //! \param[in] p Pointer to the DCL_DF23_CLA controller structure //! \param[in] ek The servo error //! \param[in] uk The controller output in the previous sample interval //! extern void DCL_runDF23_L3(DCL_DF23_CLA *p, float32_t ek, float32_t uk); //--- Direct Form 2 - clamp --------------------------------------------------- //! \brief Saturates a control variable and returns 1.0f if either limit is exceeded //! //! \details Can be used to saturate a pre-computed Direct Form 2 controller. //! If the immediate result is in range it can be used, otherwise //! it can be clamped and the next partial pre-computation skipped. //! An example of use with a pre-computed DF22 controller follows: //! //! \code //! uk = DCL_runDF22_L2(&arma2, rk); // immediate result from pre-computed controller //! f = DCL_runClamp_L1(&uk, 1.0f, -1.0f); // clamp immediate result to +/-1.0 //! // ...use uk here... //! if (0.5f > f) // if immediate result is in range... //! { //! DCL_runDF22_L3(&arma2, rk, uk); // ...pre-compute the next partial result //! } //! \endcode //! //! \param[in] data The address of the data variable //! \param[in] Umax The upper limit //! \param[in] Umin The lower limit //! \return Returns 0.0f if (Umin < data < Umax), else 1.0f //! extern float32_t DCL_runClamp_L1(float32_t *data, float32_t Umax, float32_t Umin); #endif // _C_DCLCLA_H /* end of file */