306 lines
11 KiB
C
306 lines
11 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_DF11_H_
|
||
|
|
#define _DCL_DF11_H_
|
||
|
|
|
||
|
|
#ifdef __cplusplus
|
||
|
|
extern "C" {
|
||
|
|
#endif
|
||
|
|
|
||
|
|
/**
|
||
|
|
* \addtogroup DCL_API_MODULE APIs for Digital Control Library
|
||
|
|
* @{
|
||
|
|
*
|
||
|
|
* \file dcl_df11.h
|
||
|
|
* \brief Contains direct form 1 1st order DF11 compensator
|
||
|
|
* with its related structures and functions
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "../dcl_common.h"
|
||
|
|
|
||
|
|
//--- Direct Form 1 - 1st order ----------------------------------------------
|
||
|
|
|
||
|
|
//! \brief Defines the DCL_DF11 shadow parameter set
|
||
|
|
//! used for updating compensator parameter
|
||
|
|
//!
|
||
|
|
typedef struct dcl_df11_sps
|
||
|
|
{
|
||
|
|
float32_t b0; //!< pos. coefficient to e(k)
|
||
|
|
float32_t b1; //!< pos. coefficient to e(k-1)
|
||
|
|
float32_t a1; //!< neg. coefficient to u(k-1)
|
||
|
|
} DCL_DF11_SPS;
|
||
|
|
|
||
|
|
#define DF11_SPS_DEFAULTS { 0.5f, 0.5f, 1.0f }
|
||
|
|
|
||
|
|
//! \brief DF11 object for storing df11 specific parameters
|
||
|
|
//!
|
||
|
|
typedef _DCL_VOLATILE struct dcl_df11
|
||
|
|
{
|
||
|
|
/* compensator parameter */
|
||
|
|
float32_t b0; //!< pos. coefficient to e(k)
|
||
|
|
float32_t b1; //!< pos. coefficient to e(k-1)
|
||
|
|
float32_t a1; //!< neg. coefficient to u(k-1)
|
||
|
|
|
||
|
|
/* internal storage */
|
||
|
|
float32_t d1; //!< e(k-1)
|
||
|
|
float32_t d2; //!< u(k-1)
|
||
|
|
|
||
|
|
/* miscellaneous */
|
||
|
|
DCL_DF11_SPS *sps; //!< Pointer to the shadow parameter set
|
||
|
|
DCL_CSS *css; //!< Pointer to the common support structure
|
||
|
|
} DCL_DF11, *DF11_Handle;
|
||
|
|
|
||
|
|
//! \brief Defines default values to initialize DCL_DF11
|
||
|
|
//!
|
||
|
|
#define DF11_DEFAULTS { 0.5f, 0.5f, 1.0f, 0.0f, 0.0f, \
|
||
|
|
&(DCL_DF11_SPS)DF11_SPS_DEFAULTS, &(DCL_CSS)DCL_CSS_DEFAULTS }
|
||
|
|
|
||
|
|
//! \brief Macro for internal default values to initialize DCL_DF11
|
||
|
|
//! Example: DCL_DF11 df11_ctrl = {
|
||
|
|
//! .b0 = 0.5f,
|
||
|
|
//! .b1 = 0.5f,
|
||
|
|
//! .a1 = 1.0f,
|
||
|
|
//! DF11_INT_DEFAULTS
|
||
|
|
//! };
|
||
|
|
#define DF11_INT_DEFAULTS .d1=0.0f, .d2=0.0f, .sps=&(DCL_DF11_SPS)DF11_SPS_DEFAULTS, .css=&(DCL_CSS)DCL_CSS_DEFAULTS
|
||
|
|
|
||
|
|
//! \brief Initialize DCL_DF11 struct with default parameters
|
||
|
|
//! Example: DCL_DF11* df11_ctrl = DCL_initDF11();
|
||
|
|
//!
|
||
|
|
//! \return A DCL_DF11* pointer
|
||
|
|
//!
|
||
|
|
#define DCL_initDF11() &(DCL_DF11)DF11_DEFAULTS
|
||
|
|
|
||
|
|
//! \brief Initialize DCL_DF11 struct with input compensator parameters
|
||
|
|
//! Example: DCL_DF11* DF11_ctrl = DCL_initDF11asParam(0.5f,0.5f,1.0f);
|
||
|
|
//! Note: input parameter needs to be in the same order as listed in DF11_SPS struct
|
||
|
|
//!
|
||
|
|
//! \return A DCL_DF11* pointer
|
||
|
|
//!
|
||
|
|
#define DCL_initDF11asParam(_b0,_b1,_a1) &(DCL_DF11){ .b0=_b0, .b1=_b1, .a1=_a1, \
|
||
|
|
DF11_INT_DEFAULTS }
|
||
|
|
|
||
|
|
//! \brief Initialize DCL_DF11 struct with sps parameters
|
||
|
|
//! Example: DCL_DF11_SPS df_sps = { .b0 = , .b1 = , ...}; //initial parameter
|
||
|
|
//! DCL_DF11 df_ctrl;
|
||
|
|
//! DCL_initDF11asSPS(&df_ctrl,&df_sps);
|
||
|
|
//!
|
||
|
|
//! \param[in] df_ptr DCL_DF11* pointer that needs to be initialized
|
||
|
|
//! \param[in] sps_ptr DCL_DF11_SPS* pointer with assigned parameters
|
||
|
|
//! \return DCL_DF11* Returns DCL_DF11* with set sps parameters, default parameter will be used
|
||
|
|
//! if sps_ptr is not specified
|
||
|
|
//!
|
||
|
|
#define DCL_initDF11asSPS(df_ptr,sps_ptr) \
|
||
|
|
({ \
|
||
|
|
DCL_DF11* new_df = (df_ptr) ? df_ptr : DCL_initDF11(); \
|
||
|
|
DCL_DF11_SPS* new_sps = (sps_ptr) ? sps_ptr : &(DCL_DF11_SPS)DF11_SPS_DEFAULTS;\
|
||
|
|
if(sps_ptr) \
|
||
|
|
{ \
|
||
|
|
*new_df = (DCL_DF11){ (new_sps)->b0, (new_sps)->b1, (new_sps)->a1, \
|
||
|
|
0.0f, 0.0f, (DCL_DF11_SPS*)new_sps, &(DCL_CSS)DCL_CSS_DEFAULTS }; \
|
||
|
|
} \
|
||
|
|
new_df; \
|
||
|
|
})
|
||
|
|
|
||
|
|
//! \brief Resets DF11 internal storage data with interrupt protection
|
||
|
|
//!
|
||
|
|
//! \param[in] df Pointer to the DCL_DF11 structure
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS
|
||
|
|
void DCL_resetDF11(DCL_DF11 *df)
|
||
|
|
{
|
||
|
|
dcl_interrupt_t ints = DCL_disableInts();
|
||
|
|
df->d1 = df->d2 = 0.0f;
|
||
|
|
DCL_restoreInts(ints);
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Loads DF11 tuning parameter from its SPS parameter
|
||
|
|
//!
|
||
|
|
//! \param[in] df Pointer to the active DCL_DF11 controller structure
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS
|
||
|
|
void DCL_fupdateDF11(DCL_DF11 *df)
|
||
|
|
{
|
||
|
|
df->b0 = df->sps->b0;
|
||
|
|
df->b1 = df->sps->b1;
|
||
|
|
df->a1 = df->sps->a1;
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Updates DF11 parameter from its SPS parameter with interrupt protection
|
||
|
|
//!
|
||
|
|
//! \param[in] df Pointer to the DCL_DF11 controller structure
|
||
|
|
//! \return 'true' if update is successful, otherwise 'false'
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS _DCL_CODE_SECTION
|
||
|
|
bool DCL_updateDF11(DCL_DF11 *df)
|
||
|
|
{
|
||
|
|
if (!DCL_getUpdateStatus(df))
|
||
|
|
{
|
||
|
|
dcl_interrupt_t ints = DCL_disableInts();
|
||
|
|
DCL_setUpdateStatus(df);
|
||
|
|
df->b0 = df->sps->b0;
|
||
|
|
df->b1 = df->sps->b1;
|
||
|
|
df->a1 = df->sps->a1;
|
||
|
|
DCL_clearUpdateStatus(df);
|
||
|
|
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 DF11
|
||
|
|
//! parameter from its SPS parameter and clear the status flag on completion.
|
||
|
|
//! Note: Use DCL_setPendingStatus(df) to set the pending status.
|
||
|
|
//!
|
||
|
|
//! \param[in] df Pointer to the DCL_DF11 controller structure
|
||
|
|
//! \return 'true' if an update is applied, otherwise 'false'
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS _DCL_CODE_SECTION
|
||
|
|
bool DCL_pendingUpdateDF11(DCL_DF11 *df)
|
||
|
|
{
|
||
|
|
if (DCL_getPendingStatus(df) && DCL_updateDF11(df))
|
||
|
|
{
|
||
|
|
DCL_clearPendingStatus(df);
|
||
|
|
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] df Pointer to the active DCL_DF11 controller structure
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS
|
||
|
|
void DCL_updateDF11SPS(DCL_DF11 *df)
|
||
|
|
{
|
||
|
|
df->sps->b0 = df->b0;
|
||
|
|
df->sps->b1 = df->b1;
|
||
|
|
df->sps->a1 = df->a1;
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Determines stability of the shadow DF11 compensator
|
||
|
|
//!
|
||
|
|
//! \param[in] df Pointer to the DCL_DF11 controller structure
|
||
|
|
//! \return 'true' if the pole has magnitude less than 1, 'false' otherwise
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS
|
||
|
|
bool DCL_isStableDF11(DCL_DF11 *df)
|
||
|
|
{
|
||
|
|
return(DCL_isStablePn1(df->sps->a1));
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Loads the DF11 shadow coefficients from a ZPK3 description
|
||
|
|
//! Note: Sampling period df->css->t_sec are used in the calculation.
|
||
|
|
//! New settings take effect after DCL_updateDF11().
|
||
|
|
//! Only real z1 & p1 considered, all other roots ignored.
|
||
|
|
//!
|
||
|
|
//! \param[in] df Pointer to the DCL_DF11 controller structure
|
||
|
|
//! \param[in] zpk Pointer to the ZPK3 structure
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS
|
||
|
|
void DCL_loadDF11asZPK(DCL_DF11 *df, DCL_ZPK3 *zpk)
|
||
|
|
{
|
||
|
|
#ifdef DCL_ERROR_HANDLING_ENABLED
|
||
|
|
uint32_t err_code = dcl_none;
|
||
|
|
err_code |= DCL_isZero(cimagf(zpk->z1)) ? dcl_none : dcl_param_warn_err;
|
||
|
|
err_code |= DCL_isZero(cimagf(zpk->p1)) ? dcl_none : dcl_param_warn_err;
|
||
|
|
if (err_code)
|
||
|
|
{
|
||
|
|
DCL_setError(df,err_code);
|
||
|
|
DCL_getErrorInfo(df);
|
||
|
|
DCL_runErrorHandler(df);
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
float32_t t_sec = df->css->t_sec;
|
||
|
|
float32_t a0p = 2.0f - (float32_t) crealf(zpk->p1) * t_sec;
|
||
|
|
df->sps->b0 = zpk->K * (2.0f - (float32_t) crealf(zpk->z1) * t_sec) / a0p;
|
||
|
|
df->sps->b1 = zpk->K * (-2.0f - (float32_t) crealf(zpk->z1) * t_sec) / a0p;
|
||
|
|
df->sps->a1 = (-2.0f - (float32_t) crealf(zpk->p1) * t_sec) / a0p;
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Loads compensator coefficients to emulate series form PI
|
||
|
|
//! Note: Sampling period df->css->t_sec are used in the calculation.
|
||
|
|
//! New settings take effect after DCL_updateDF11().
|
||
|
|
//!
|
||
|
|
//! \param[in] df Pointer to the DCL_DF11 controller structure
|
||
|
|
//! \param[in] Kp Proportional gain
|
||
|
|
//! \param[in] Ki Integral gain
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS
|
||
|
|
void DCL_loadDF11asPI(DCL_DF11 *df, float32_t Kp, float32_t Ki)
|
||
|
|
{
|
||
|
|
#ifdef DCL_ERROR_HANDLING_ENABLED
|
||
|
|
uint32_t err_code = dcl_none;
|
||
|
|
err_code |= (Kp < 0.0f) ? dcl_param_invalid_err : dcl_none;
|
||
|
|
err_code |= (Ki < 0.0f) ? dcl_param_invalid_err : dcl_none;
|
||
|
|
if (err_code)
|
||
|
|
{
|
||
|
|
DCL_setError(df,err_code);
|
||
|
|
DCL_getErrorInfo(df);
|
||
|
|
DCL_runErrorHandler(df);
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
float32_t t_sec = df->css->t_sec;
|
||
|
|
df->sps->b0 = Kp * ((Ki * t_sec) + 2.0f) / 2.0f;
|
||
|
|
df->sps->b1 = Kp * ((Ki * t_sec) - 2.0f) / 2.0f;
|
||
|
|
df->sps->a1 = -1.0f;
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Executes a 1st order Direct Form 1 controller
|
||
|
|
//!
|
||
|
|
//! \param[in] df Pointer to the DCL_DF11 controller structure
|
||
|
|
//! \param[in] ek The servo error
|
||
|
|
//! \return uk The control effort
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS _DCL_CODE_SECTION
|
||
|
|
float32_t DCL_runDF11(DCL_DF11 *df, float32_t ek)
|
||
|
|
{
|
||
|
|
df->d2 = (ek * df->b0) + (df->d1 * df->b1) - (df->d2 * df->a1);
|
||
|
|
df->d1 = ek;
|
||
|
|
|
||
|
|
return(df->d2);
|
||
|
|
}
|
||
|
|
|
||
|
|
/** @} */
|
||
|
|
|
||
|
|
#ifdef __cplusplus
|
||
|
|
}
|
||
|
|
#endif // extern "C"
|
||
|
|
|
||
|
|
#endif // _DCL_DF11_H_
|