438 lines
16 KiB
C
438 lines
16 KiB
C
/*
|
|
* Copyright (C) 2023 Texas Instruments Incorporated - http://www.ti.com/
|
|
*
|
|
*
|
|
* 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 EXPRESS 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_DF22_H_
|
|
#define _DCL_DF22_H_
|
|
|
|
#ifdef __cplusplus
|
|
extern "C" {
|
|
#endif
|
|
|
|
/**
|
|
* \addtogroup DCL_API_MODULE APIs for Digital Control Library
|
|
* @{
|
|
*
|
|
* \file dcl_df22.h
|
|
* \brief Contains direct form 2 2nd order DF22 compensator
|
|
* with its related structures and functions
|
|
*/
|
|
|
|
#include "../dcl_common.h"
|
|
|
|
//--- Direct Form 2 - 2nd order ----------------------------------------------
|
|
|
|
//! \brief Defines DCL_DF22 shadow parameter set
|
|
//! used for updating compensator parameter
|
|
//!
|
|
typedef struct dcl_df22_sps
|
|
{
|
|
float32_t b0; //!< pos. coefficient to e(k)
|
|
float32_t b1; //!< pos. coefficient to e(k-1)
|
|
float32_t b2; //!< pos. coefficient to e(k-2)
|
|
float32_t a1; //!< neg. coefficient to u(k-1)
|
|
float32_t a2; //!< neg. coefficient to u(k-2)
|
|
} DCL_DF22_SPS;
|
|
|
|
#define DF22_SPS_DEFAULTS { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f }
|
|
|
|
//! \brief DCL_DF22 object for storing df22 specific parameters
|
|
//!
|
|
typedef _DCL_VOLATILE struct dcl_df22
|
|
{
|
|
/* compensator parameter */
|
|
float32_t b0; //!< pos. coefficient to e(k)
|
|
float32_t b1; //!< pos. coefficient to e(k-1)
|
|
float32_t b2; //!< pos. coefficient to e(k-2)
|
|
float32_t a1; //!< neg. coefficient to u(k-1)
|
|
float32_t a2; //!< neg. coefficient to u(k-2)
|
|
|
|
/* internal storage */
|
|
float32_t x1; //!< x1 = b1*e(k-1) - a1*u(k-1) + x2
|
|
float32_t x2; //!< x2 = b2*e(k-2) - a2*u(k-2)
|
|
|
|
/* miscellaneous */
|
|
DCL_DF22_SPS *sps; //!< updates compensator parameter
|
|
DCL_CSS *css; //!< configuration & debugging
|
|
} DCL_DF22, *DF22_Handle;
|
|
|
|
//! \brief Defines default values to initialize DCL_DF22
|
|
//!
|
|
#define DF22_DEFAULTS { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, \
|
|
&(DCL_DF22_SPS)DF22_SPS_DEFAULTS, &(DCL_CSS)DCL_CSS_DEFAULTS }
|
|
|
|
//! \brief Macro for internal default values to initialize DCL_DF22
|
|
//! Example: DCL_DF22 df22_ctrl = {
|
|
//! .b0 = 1.00f,
|
|
//! .b1 = 0.0f,
|
|
//! ...
|
|
//! .a2 = 0.0f,
|
|
//! DF22_INT_DEFAULTS
|
|
//! };
|
|
#define DF22_INT_DEFAULTS .x1=0.0f, .x2=0.0f, .sps=&(DCL_DF22_SPS)DF22_SPS_DEFAULTS, \
|
|
.css=&(DCL_CSS)DCL_CSS_DEFAULTS
|
|
|
|
//! \brief Initialize DCL_DF22 struct with default parameters
|
|
//! Example: DCL_DF22* df22_ctrl = DCL_initDF22();
|
|
//!
|
|
//! \return A DCL_DF22* pointer
|
|
//!
|
|
#define DCL_initDF22() &(DCL_DF22)DF22_DEFAULTS
|
|
|
|
//! \brief Initialize DCL_DF22 struct with input compensator parameters
|
|
//! Example: DCL_DF22 DF22_ctrl = DCL_initD22asParam(1.0f,0.0f,0.0f,0.0f,0.0f);
|
|
//! Note: input parameter needs to be in the same order as listed in DF22_SPS struct.
|
|
//!
|
|
//! \return A DCL_DF22* pointer
|
|
//!
|
|
#define DCL_initDF22asParam(_b0,_b1,_b2,_a1,_a2) &(DCL_DF22){ .b0=_b0, .b1=_b1, \
|
|
.b2=_b2, .a1=_a1, .a2=_a2, .a3=_a3, DF22_INT_DEFAULTS }
|
|
|
|
//! \brief Initialize DCL_DF22 struct using sps parameters
|
|
//! Example: DCL_DF22_SPS df_sps = { .b0 = , .b1 = , ...}; //initial parameter
|
|
//! DCL_DF22 df_ctrl;
|
|
//! DCL_initDF22asSPS(&df_ctrl,&df_sps);
|
|
//!
|
|
//! \param[in] df_ptr DCL_DF22* pointer that needs to be initialized
|
|
//! \param[in] sps_ptr DCL_DF22_SPS* pointer with assigned parameters
|
|
//! \return Returns DCL_DF22* with set sps parameters, default parameter will be used
|
|
//! if sps_ptr is not specified
|
|
//!
|
|
#define DCL_initDF22asSPS(df_ptr,sps_ptr) \
|
|
({ \
|
|
DCL_DF22* new_df = (df_ptr) ? df_ptr : DCL_initDF22(); \
|
|
DCL_DF22_SPS* new_sps = (sps_ptr) ? sps_ptr : &(DCL_DF22_SPS)DF22_SPS_DEFAULTS; \
|
|
if(sps_ptr) \
|
|
{ \
|
|
*new_df = (DCL_DF22){ (new_sps)->b0, (new_sps)->b1, (new_sps)->b2, (new_sps)->a1,\
|
|
(new_sps)->a2, 0.0f, 0.0f, (DCL_DF22_SPS*)new_sps, &(DCL_CSS)DCL_CSS_DEFAULTS }; \
|
|
} \
|
|
new_df; \
|
|
})
|
|
|
|
//! \brief Resets DF22 internal storage data with interrupt protection
|
|
//!
|
|
//! \param[in] df Pointer to the DCL_DF22 controller structure
|
|
//!
|
|
_DCL_CODE_ACCESS
|
|
void DCL_resetDF22(DCL_DF22 *df)
|
|
{
|
|
dcl_interrupt_t ints;
|
|
ints = DCL_disableInts();
|
|
df->x1 = df->x2 = 0.0f;
|
|
DCL_restoreInts(ints);
|
|
}
|
|
|
|
//! \brief Loads DF22 tuning parameter from its SPS parameter without interrupt protection
|
|
//!
|
|
//! \param[in] df Pointer to the active DCL_DF22 controller structure
|
|
//!
|
|
_DCL_CODE_ACCESS
|
|
void DCL_forceUpdateDF22(DCL_DF22 *df)
|
|
{
|
|
df->b0 = df->sps->b0;
|
|
df->b1 = df->sps->b1;
|
|
df->b2 = df->sps->b2;
|
|
df->a1 = df->sps->a1;
|
|
df->a2 = df->sps->a2;
|
|
}
|
|
|
|
//! \brief Loads DF22 tuning parameter from its SPS parameter with interrupt protection
|
|
//!
|
|
//! \param[in] df Pointer to the DCL_DF22 controller structure
|
|
//!
|
|
_DCL_CODE_ACCESS _DCL_CODE_SECTION
|
|
void DCL_updateDF22NoCheck(DCL_DF22 *df)
|
|
{
|
|
dcl_interrupt_t ints;
|
|
ints = DCL_disableInts();
|
|
df->b0 = df->sps->b0;
|
|
df->b1 = df->sps->b1;
|
|
df->b2 = df->sps->b2;
|
|
df->a1 = df->sps->a1;
|
|
df->a2 = df->sps->a2;
|
|
DCL_restoreInts(ints);
|
|
}
|
|
|
|
//! \brief A conditional update based on the update flag.
|
|
//! If the update status is set, the function will update DF22
|
|
//! parameter from its SPS parameter and clear the status flag on completion.
|
|
//! Note: Use DCL_setUpdateStatus(df) to set the update status.
|
|
//!
|
|
//! \param[in] df Pointer to the DCL_DF22 controller structure
|
|
//! \return 'true' if an update is applied, otherwise 'false'
|
|
//!
|
|
_DCL_CODE_ACCESS _DCL_CODE_SECTION
|
|
bool DCL_updateDF22(DCL_DF22 *df)
|
|
{
|
|
if (DCL_setUpdateStatus(df))
|
|
{
|
|
DCL_updateDF22NoCheck(df);
|
|
DCL_clearUpdateStatus(df);
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
//! \brief Determines stability of the shadow compensator
|
|
//!
|
|
//! \param[in] df Pointer to the DCL_DF22 controller structure
|
|
//! \return 'true' if both poles have magnitude less than 1, 'false' otherwise
|
|
//!
|
|
_DCL_CODE_ACCESS
|
|
bool DCL_isStableDF22(DCL_DF22 *df)
|
|
{
|
|
return(DCL_isStablePn2(1.0f, df->sps->a1, df->sps->a2));
|
|
}
|
|
|
|
//! \brief Loads the DF22 shadow coefficients from a ZPK3 description
|
|
//! Note: Sampling period df->css->T are used in the calculation.
|
|
//! New settings take effect after DCL_updateDF22().
|
|
//! Only z1, z2, p1 & p2 are considered, z3 & p3 are ignored.
|
|
//!
|
|
//! \param[in] df Pointer to the DCL_DF22 controller structure
|
|
//! \param[in] zpk Pointer to the DCL_ZPK3 structure
|
|
//!
|
|
_DCL_CODE_ACCESS
|
|
void DCL_loadDF22asZPK(DCL_DF22 *df, DCL_ZPK3 *zpk)
|
|
{
|
|
|
|
#ifdef DCL_ERROR_HANDLING_ENABLED
|
|
uint32_t err_code = dcl_none;
|
|
err_code |= DCL_isZero(cimagf(zpk->z1) + cimagf(zpk->z2)) ? dcl_none : dcl_param_invalid_err;
|
|
err_code |= DCL_isZero(cimagf(zpk->p1) + cimagf(zpk->p2)) ? dcl_none : dcl_param_invalid_err;
|
|
DCL_setError(df,err_code);
|
|
if (err_code)
|
|
{
|
|
DCL_getErrorInfo(df);
|
|
DCL_runErrorHandler(df);
|
|
}
|
|
#endif
|
|
|
|
float32_t beta1 = -(float32_t) crealf(zpk->z1 + zpk->z2);
|
|
float32_t beta0 = (float32_t) crealf(zpk->z1 * zpk->z2);
|
|
float32_t alpha1 = -(float32_t) crealf(zpk->p1 + zpk->p2);
|
|
float32_t alpha0 = (float32_t) crealf(zpk->p1 * zpk->p2);
|
|
|
|
float32_t T = df->css->T;
|
|
float32_t a0p = 4.0f + (alpha1 * 2.0f * T) + (alpha0 * T * T);
|
|
|
|
df->sps->b0 = zpk->K * (4.0f + (beta1 * 2.0f * T) + (beta0 * T * T)) / a0p;
|
|
df->sps->b1 = zpk->K * (-8.0f + (2.0f * beta0 * T * T)) / a0p;
|
|
df->sps->b2 = zpk->K * (4.0f - (beta1 * 2.0f * T) + (beta0 * T * T)) / a0p;
|
|
df->sps->a1 = (-8.0f + (2.0f * alpha0 * T * T)) / a0p;
|
|
df->sps->a2 = (4.0f - (alpha1 * 2.0f * T) + (alpha0 * T * T)) / a0p;
|
|
}
|
|
|
|
//! \brief Loads the DF22 shadow coefficients from damping ratio and un-damped
|
|
//! natural frequency using sample rate in CSS.
|
|
//! Note: Sampling period df->css->T are used in the calculation.
|
|
//! New settings take effect after DCL_updateDF22().
|
|
//!
|
|
//! \param[in] df Pointer to the DCL_DF22 controller structure
|
|
//! \param[in] z The damping ratio
|
|
//! \param[in] wn The un-damped natural frequency in rad/s
|
|
//!
|
|
_DCL_CODE_ACCESS
|
|
void DCL_loadDF22asZwn(DCL_DF22 *df, float32_t z, float32_t wn)
|
|
{
|
|
|
|
#ifdef DCL_ERROR_HANDLING_ENABLED
|
|
uint32_t err_code = dcl_none;
|
|
err_code |= (z >= 0.0f) ? dcl_none : dcl_param_invalid_err;
|
|
err_code |= (wn >= 0.0f) ? dcl_none : dcl_param_invalid_err;
|
|
if (err_code)
|
|
{
|
|
DCL_setError(df,err_code);
|
|
DCL_getErrorInfo(df);
|
|
DCL_runErrorHandler(df);
|
|
}
|
|
#endif
|
|
|
|
float32_t T = df->css->T;
|
|
float32_t v1 = wn * wn * T * T;
|
|
float32_t a2p = 1.0f / (4.0f + (4.0f * z * wn * T) + v1);
|
|
df->sps->b0 = v1 * a2p;
|
|
df->sps->b1 = 2.0f * df->sps->b0;
|
|
df->sps->b2 = df->sps->b0;
|
|
df->sps->a1 = ((2.0f * v1) - 8.0f) * a2p;
|
|
df->sps->a2 = (4.0f - (4.0f * z * wn * T) + v1) * a2p;
|
|
}
|
|
|
|
//! \brief Loads the shadow DF22 compensator coefficients to emulate a series form PID.
|
|
//! Note: Sampling period df->css->T are used in the calculation.
|
|
//! New settings take effect after DCL_updateDF22().
|
|
//!
|
|
//! \param[in] df Pointer to the DCL_DF22 controller structure
|
|
//! \param[in] Kp Proportional gain
|
|
//! \param[in] Ki Integral gain
|
|
//! \param[in] Kd Derivative gain
|
|
//! \param[in] fc Derivative path filter bandwidth in Hz
|
|
//!
|
|
_DCL_CODE_ACCESS
|
|
void DCL_loadDF22asSeriesPID(DCL_DF22 *df, float32_t Kp, float32_t Ki, float32_t Kd, float32_t fc)
|
|
{
|
|
|
|
#ifdef DCL_ERROR_HANDLING_ENABLED
|
|
uint32_t err_code = dcl_none;
|
|
err_code |= (Kp < 0.0f) ? dcl_param_range_err : dcl_none;
|
|
err_code |= (Ki < 0.0f) ? dcl_param_range_err : dcl_none;
|
|
err_code |= (Kd < 0.0f) ? dcl_param_range_err : dcl_none;
|
|
err_code |= ((fc < 0.0f) || (fc > (1.0f / (2.0f * df->css->T)))) ? dcl_param_range_err : dcl_none;
|
|
if (err_code)
|
|
{
|
|
DCL_setError(df,err_code);
|
|
DCL_getErrorInfo(df);
|
|
DCL_runErrorHandler(df);
|
|
}
|
|
#endif
|
|
|
|
float32_t T = df->css->T;
|
|
float32_t tau = 1 / (2.0f * CONST_PI * fc);
|
|
float32_t c1 = 2.0f / (T + (2.0f * tau));
|
|
float32_t c2 = c1 * (T - (2.0f * tau)) / 2.0f;
|
|
float32_t Kdp = Kd * c1;
|
|
df->sps->b0 = Kp * (1 + Ki + Kdp);
|
|
df->sps->b1 = Kp * (c2 - 1 + Ki*c2 - 2*Kdp);
|
|
df->sps->b2 = Kp * (-c2 + Kdp);
|
|
df->sps->a1 = c2 - 1;
|
|
df->sps->a2 = -c2;
|
|
}
|
|
|
|
//! \brief Loads the shadow DF22 compensator coefficients to emulate a parallel form PID.
|
|
//! Note: Sampling period df->css->T are used in the calculation.
|
|
//! New settings take effect after DCL_updateDF22().
|
|
//!
|
|
//! \param[in] df Pointer to the DCL_DF22 controller structure
|
|
//! \param[in] Kp Proportional gain
|
|
//! \param[in] Ki Integral gain
|
|
//! \param[in] Kd Derivative gain
|
|
//! \param[in] fc Derivative path filter bandwidth in Hz
|
|
//!
|
|
_DCL_CODE_ACCESS
|
|
void DCL_loadDF22asParallelPID(DCL_DF22 *df, float32_t Kp, float32_t Ki, float32_t Kd, float32_t fc)
|
|
{
|
|
|
|
#ifdef DCL_ERROR_HANDLING_ENABLED
|
|
uint32_t err_code = dcl_none;
|
|
err_code |= (Kp < 0.0f) ? dcl_param_range_err : dcl_none;
|
|
err_code |= (Ki < 0.0f) ? dcl_param_range_err : dcl_none;
|
|
err_code |= (Kd < 0.0f) ? dcl_param_range_err : dcl_none;
|
|
err_code |= (fc < 0.0f) ? dcl_param_range_err : dcl_none;
|
|
err_code |= (fc > (1.0f / (2.0f * df->css->T))) ? dcl_param_warn_err : dcl_none;
|
|
if (err_code)
|
|
{
|
|
DCL_setError(df,err_code);
|
|
DCL_getErrorInfo(df);
|
|
DCL_runErrorHandler(df);
|
|
}
|
|
#endif
|
|
|
|
float32_t T = df->css->T;
|
|
float32_t tau = 1.0f / (2.0f * CONST_PI * fc);
|
|
float32_t c1 = 2.0f / (T + (2.0f * tau));
|
|
float32_t c2 = c1 * (T - (2.0f * tau)) / 2.0f;
|
|
float32_t Kdp = Kd * c1;
|
|
df->sps->b0 = Kp + Ki + Kdp;
|
|
df->sps->b1 = (Kp * (c2 - 1)) + (Ki * c2) - (2.0f * Kdp);
|
|
df->sps->b2 = (-c2 * Kp) + Kdp;
|
|
df->sps->a1 = c2 - 1;
|
|
df->sps->a2 = -c2;
|
|
}
|
|
|
|
//! \brief Executes a 2nd order Direct Form 2 controller
|
|
//!
|
|
//! \param[in] df Pointer to the DCL_DF22 controller structure
|
|
//! \param[in] ek The servo error
|
|
//! \return The control effort
|
|
//!
|
|
_DCL_CODE_ACCESS _DCL_CODE_SECTION
|
|
float32_t DCL_runDF22(DCL_DF22 *df, float32_t ek)
|
|
{
|
|
float32_t v7 = (ek * df->b0) + df->x1;
|
|
df->x1 = (ek * df->b1) + df->x2 - (v7 * df->a1);
|
|
df->x2 = (ek * df->b2) - (v7 * df->a2);
|
|
|
|
return(v7);
|
|
}
|
|
|
|
//! \brief Immediate computation to obtain DF22 servo error
|
|
//! without updating the controller
|
|
//!
|
|
//! \param[in] df Pointer to the DCL_DF22 controller structure
|
|
//! \param[in] ek The servo error
|
|
//! \return The control effort
|
|
//!
|
|
_DCL_CODE_ACCESS
|
|
float32_t DCL_runDF22PartialCompute(DCL_DF22 *df, float32_t ek)
|
|
{
|
|
return((ek * df->b0) + df->x1);
|
|
}
|
|
|
|
//! \brief Update DF22 controller based on pre-computed control effort
|
|
//!
|
|
//! \param[in] df Pointer to the DCL_DF22 controller structure
|
|
//! \param[in] ek The servo error
|
|
//! \param[in] uk The controller output in the previous sample interval
|
|
//!
|
|
_DCL_CODE_ACCESS
|
|
void DCL_runDF22PartialUpdate(DCL_DF22 *df, float32_t ek, float32_t uk)
|
|
{
|
|
df->x1 = (ek * df->b1) + df->x2 - (uk * df->a1);
|
|
df->x2 = (ek * df->b2) - (uk * df->a2);
|
|
}
|
|
|
|
//! \brief Executes a 2nd order Direct Form 2 controller with clamp
|
|
//!
|
|
//! \param[in] df Pointer to the DCL_DF22 controller structure
|
|
//! \param[in] ek The servo error
|
|
//! \param[in] Umax Upper saturation limit
|
|
//! \param[in] Umin Lower saturation limit
|
|
//! \return The control effort
|
|
//!
|
|
_DCL_CODE_ACCESS _DCL_CODE_SECTION
|
|
float32_t DCL_runDF22Clamp(DCL_DF22 *df, float32_t ek, float32_t Umax, float32_t Umin)
|
|
{
|
|
float32_t uk = DCL_runDF22PartialCompute(df, ek);
|
|
bool is_clamped = DCL_runClamp(&uk, Umax, Umin);
|
|
if(!is_clamped) DCL_runDF22PartialUpdate(df, ek, uk);
|
|
return(uk);
|
|
}
|
|
|
|
/** @} */
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif // extern "C"
|
|
|
|
#endif // _DCL_DF22_H_
|