dcl_pi2.h

Go to the documentation of this file.

/*
 *  Copyright (C) 2024 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 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_PI2_H_
#define _DCL_PI2_H_
 
#ifdef __cplusplus
extern "C" {
#endif
 
#include "../dcl_common.h"
 
//--- Linear PI2 controller --------------------------------------------------
 
typedef struct dcl_pi2_sps
{
    float32_t Kp;       
    float32_t Ki;       
    float32_t Umax;     
    float32_t Umin;     
} DCL_PI2_SPS;
 
#define PI2_SPS_DEFAULTS { 1.0f, 0.0f, 1.0f, -1.0f }
 
typedef _DCL_VOLATILE struct dcl_pi2
{
    /* controller parameter */
    float32_t Kp;       
    float32_t Ki;       
    float32_t Umax;     
    float32_t Umin;     
 
    /* internal storage */    
    float32_t i6;       
    float32_t i9;       
    float32_t i12;      
    float32_t i13;      
 
    /* miscellaneous */
    DCL_PI2_SPS *sps;   
    DCL_CSS *css;       
} 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 }
 
#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           
 
#define DCL_initPI2() &(DCL_PI2)PI2_DEFAULTS
 
#define DCL_initPI2asParam(kp,ki,umax,umin) &(DCL_PI2){ .Kp=kp, .Ki=ki, \
                                .Umax=umax, .Umin=umin, PI2_INT_DEFAULTS }
 
#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;                                                                                \
})
 
_DCL_CODE_ACCESS
void DCL_resetPI2(DCL_PI2 *pi2)
{
    dcl_interrupt_t ints;
    ints = DCL_disableInts();
    pi2->i6 = pi2->i9 = 0.0f;
    pi2->i12 = pi2->i13 = 1.0f;
    DCL_restoreInts(ints);
}
 
_DCL_CODE_ACCESS
void DCL_forceUpdatePI2(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 <= 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;
}
 
_DCL_CODE_ACCESS
void DCL_updatePI2NoCheck(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 <= 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
 
    dcl_interrupt_t ints;
    ints = DCL_disableInts();
    pi2->Ki = pi2->sps->Ki;
    pi2->Kp = pi2->sps->Kp;
    pi2->Umax = pi2->sps->Umax;
    pi2->Umin = pi2->sps->Umin;
    DCL_restoreInts(ints);
}
 
_DCL_CODE_ACCESS
bool DCL_updatePI2(DCL_PI2 *pi2)
{
    if (DCL_getUpdateStatus(pi2))
    {
        DCL_updatePI2NoCheck(pi2);
        DCL_clearUpdateStatus(pi2);
        return true;
    }
    return false;
}
 
_DCL_CRIT_ACCESS
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_