dcl_df13.h

Go to the documentation of this file.

/*
 * Copyright (C) 2024 Texas Instruments Incorporated - https://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_DF13_H_
#define _DCL_DF13_H_
 
#ifdef __cplusplus
extern "C" {
#endif
 
#include "../dcl_common.h"
 
//--- Direct Form 1 - 3rd order ----------------------------------------------
 
typedef struct dcl_df13_sps {
    float32_t b0;   
    float32_t b1;   
    float32_t b2;   
    float32_t b3;   
    float32_t a1;   
    float32_t a2;   
    float32_t a3;   
    float32_t a0;   
} DCL_DF13_SPS;
 
#define DF13_SPS_DEFAULTS { 0.25f, 0.25f, 0.25f, 0.25f, 0.0f, 0.0f, 0.0f, 0.0f }
 
typedef _DCL_VOLATILE struct dcl_df13 
{
    /* compensator parameter */
    float32_t b0;   
    float32_t b1;   
    float32_t b2;   
    float32_t b3;   
    float32_t a1;   
    float32_t a2;   
    float32_t a3;   
 
    /* internal storage */
    float32_t d1;   
    float32_t d2;   
    float32_t d3;   
    float32_t d4;   
    float32_t d5;   
    float32_t d6;   
 
    float32_t a0;   
    float32_t d0;   
    float32_t d7;   
 
    /* miscellaneous */
    DCL_DF13_SPS *sps; 
    DCL_CSS *css;      
} DCL_DF13;
 
#define DF13_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, \
            &(DCL_DF13_SPS)DF13_SPS_DEFAULTS, &(DCL_CSS)DCL_CSS_DEFAULTS }
 
#define DF13_INT_DEFAULTS .d1=0.0f, .d2=0.0f, .d3=0.0f, .d4=0.0f, .d5=0.0f, \
                          .d6=0.0f, .a0=0.0f, .d0=0.0f, .d7=0.0f, \
                          .sps=&(DCL_DF13_SPS)DF13_SPS_DEFAULTS, .css=&(DCL_CSS)DCL_CSS_DEFAULTS 
 
#define DCL_initDF13() &(DCL_DF13)DF13_DEFAULTS
 
#define DCL_initDF13asParam(_b0,_b1,_b2,_b3,_a1,_a2,_a3) &(DCL_DF13){ .b0=_b0, .b1=_b1, \
                        .b2=_b2, .b3=_b3, .a1=_a1, .a2=_a2, .a3=_a3, DF13_INT_DEFAULTS }
 
#define DCL_initDF13asSPS(df_ptr,sps_ptr)                                                  \
({                                                                                         \
    DCL_DF13* new_df = (df_ptr) ? df_ptr : DCL_initDF13();                                 \
    DCL_DF13_SPS* new_sps = (sps_ptr) ? sps_ptr : &(DCL_DF13_SPS)DF13_SPS_DEFAULTS;        \
    if(sps_ptr)                                                                            \
    {                                                                                      \
        *new_df =(DCL_DF13){ (new_sps)->b0, (new_sps)->b1, (new_sps)->b2, (new_sps)->b3,   \
        (new_sps)->a1, (new_sps)->a2, (new_sps)->a3, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,   \
        0.0f, 0.0f, 0.0f, (DCL_DF13_SPS*)new_sps, &(DCL_CSS)DCL_CSS_DEFAULTS };            \
    }                                                                                      \
    new_df;                                                                                \
})
 
_DCL_CODE_ACCESS
void DCL_resetDF13(DCL_DF13 *df)
{
    dcl_interrupt_t ints;
    ints = DCL_disableInts();
    df->d1 = df->d2 = df->d3 = df->d4 = df->d5 = df->d6 = 0.0f;
    DCL_restoreInts(ints);
}
 
_DCL_CODE_ACCESS
void DCL_forceUpdateDF13(DCL_DF13 *df)
{
    df->b0 = df->sps->b0;
    df->b1 = df->sps->b1;
    df->b2 = df->sps->b2;
    df->b3 = df->sps->b3;
    df->a1 = df->sps->a1;
    df->a2 = df->sps->a2;
    df->a3 = df->sps->a3;    
}
 
_DCL_CODE_ACCESS
void DCL_updateDF13NoCheck(DCL_DF13 *df)
{
    dcl_interrupt_t ints;
    ints = DCL_disableInts();
    df->b0 = df->sps->b0;
    df->b1 = df->sps->b1;
    df->b2 = df->sps->b2;
    df->b3 = df->sps->b3;
    df->a1 = df->sps->a1;
    df->a2 = df->sps->a2;
    df->a3 = df->sps->a3;
    DCL_restoreInts(ints);
}
 
_DCL_CODE_ACCESS
bool DCL_updateDF13(DCL_DF13 *df)
{
    if (DCL_getUpdateStatus(df))
    {
        DCL_updateDF13NoCheck(df);
        DCL_clearUpdateStatus(df);
        return true;
    }
    return false;
}
 
_DCL_CODE_ACCESS
bool DCL_isStableDF13(DCL_DF13 *df)
{
    return(DCL_isStablePn3(1.0f, df->sps->a1, df->sps->a2, df->sps->a3));
}
 
_DCL_CODE_ACCESS
void DCL_loadDF13asZPK(DCL_DF13 *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) + cimagf(zpk->z3)) ? dcl_none : dcl_param_invalid_err;
    err_code |= DCL_isZero(cimagf(zpk->p1) + cimagf(zpk->p2) + cimagf(zpk->p3)) ? dcl_none : dcl_param_invalid_err;
    if (err_code)
    {
        DCL_setError(df,err_code);
        DCL_getErrorInfo(df);
        DCL_runErrorHandler(df);
    }
#endif
 
    float32_t beta2 = -(float32_t) crealf(zpk->z1 + zpk->z2 + zpk->z3);
    float32_t beta1 = (float32_t) crealf((zpk->z1 * zpk->z2) + (zpk->z2 * zpk->z3) + (zpk->z1 * zpk->z3));
    float32_t beta0 = -(float32_t) crealf(zpk->z1 * zpk->z2 * zpk->z3);
 
    float32_t alpha2 = -(float32_t) crealf(zpk->p1 + zpk->p2 + zpk->p3);
    float32_t alpha1 = (float32_t) crealf((zpk->p1 * zpk->p2) + (zpk->p2 * zpk->p3) + (zpk->p1 * zpk->p3));
    float32_t alpha0 = -(float32_t) crealf(zpk->p1 * zpk->p2 * zpk->p3);
 
    float32_t T = df->css->T;
 
    float32_t a0p = 8.0f + (alpha2 * 4.0f * T) + (alpha1 * 2.0f * T * T) + (alpha0 * T * T * T);
 
    df->sps->b0 = zpk->K * (8.0f + (beta2 * 4.0f * T) + (beta1 * 2.0f * T * T) + (beta0 * T * T * T)) / a0p;
    df->sps->b1 = zpk->K * (-24.0f - (beta2 * 4.0f * T) + (beta1 * 2.0f * T * T) + (3.0f * beta0 * T * T * T)) / a0p;
    df->sps->b2 = zpk->K * (24.0f - (beta2 * 4.0f * T) - (beta1 * 2.0f * T * T) + (3.0f * beta0 * T * T * T)) / a0p;
    df->sps->b3 = zpk->K * (-8.0f + (beta2 * 4.0f * T) - (beta1 * 2.0f * T * T) + (beta0 * T * T * T)) / a0p;
 
    df->sps->a1 = (-24.0f - (alpha2 * 4.0f * T) + (alpha1 * 2.0f * T * T) + (3.0f * alpha0 * T * T * T)) / a0p;
    df->sps->a2 = (24.0f - (alpha2 * 4.0f * T) - (alpha1 * 2.0f * T * T) + (3.0f * alpha0 * T * T * T)) / a0p;
    df->sps->a3 = (-8.0f + (alpha2 * 4.0f * T) - (alpha1 * 2.0f * T * T) + (alpha0 * T * T * T)) / a0p;
}
 
_DCL_CRIT_ACCESS
float32_t DCL_runDF13(DCL_DF13 *df, float32_t ek)
{
    float32_t v4 = (ek * df->b0) + (df->d1 * df->b1) + (df->d2 * df->b2) + (df->d3 * df->b3) - (df->d4 * df->a1) - (df->d5 * df->a2) - (df->d6 * df->a3);
    df->d3 = df->d2;
    df->d2 = df->d1;
    df->d1 = ek;
    df->d6 = df->d5;
    df->d5 = df->d4;
    df->d4 = v4;
 
    return(v4);
}
 
_DCL_CRIT_ACCESS
float32_t DCL_runDF13PartialCompute(DCL_DF13 *df, float32_t ek)
{
    float32_t v4 = (ek * df->b0) + (df->d1 * df->b1) + (df->d2 * df->b2) + (df->d3 * df->b3) - (df->d4 * df->a1) - (df->d5 * df->a2) - (df->d6 * df->a3);
    return(v4);
}
 
_DCL_CRIT_ACCESS
void DCL_runDF13PartialUpdate(DCL_DF13 *df, float32_t ek, float32_t uk)
{
    df->d3 = df->d2;
    df->d2 = df->d1;
    df->d1 = ek;
    df->d6 = df->d5;
    df->d5 = df->d4;
    df->d4 = uk;
}
 
/*  
 *  \details    Previously, C28 version of DCL did not implement a function
 *              to run DF13 with clamp. Instead, user had to implement a routine 
 *              similar to the following: 
 *
 *  \code{.c}   extern DCL_DF13 df;
 *              extern float32_t ek, uk;
 *              float32_t vk = 0;
 *
 *              float32_t uk = DCL_runDF13_C2/C5(df, ek, vk);
 *              bool is_clamped = DCL_runClamp_C1/C2(&uk, Umax, Umin)
 *              if (!is_clamped)
 *              {
 *                  vk = DCL_runDF13_C3/C6(df, ek, uk);
 *              }     
 *  
 *              // A new function DCL_runDF13Clamp() was implemented to replace it,
 *              // therefore, user may change the aformentioned routine to just:
 *              uk = DCL_runDF13Clamp(df, ek, Umax, Umin);
 *  \endcode
 */
_DCL_CRIT_ACCESS
float32_t DCL_runDF13Clamp(DCL_DF13 *df, float32_t ek, float32_t Umax, float32_t Umin)
{
    float32_t uk = DCL_runDF13PartialCompute(df, ek);
    bool is_clamped = DCL_runClamp(&uk, Umax, Umin);
    if(!is_clamped) DCL_runDF13PartialUpdate(df, ek, uk);
    return(uk);
}
 
_DCL_CRIT_ACCESS 
float32_t DCL_runDF13_C5(DCL_DF13 *p, float32_t ek, float32_t vk)
{
    p->d4 = (ek * p->b0) + vk;
 
    return(p->d4);
}
 
_DCL_CRIT_ACCESS 
float32_t DCL_runDF13_C6(DCL_DF13 *p, float32_t ek, float32_t uk)
{
    float32_t v9;
 
    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);
}
 
#ifdef __cplusplus
}
#endif // extern "C"
 
#endif // _DCL_DF13_H_