438 lines
15 KiB
C
438 lines
15 KiB
C
|
/*
|
||
|
|
* Copyright (C) 2023 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 EXPgResS 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_PI_H_
|
||
|
|
#define _DCL_PI_H_
|
||
|
|
|
||
|
|
#ifdef __cplusplus
|
||
|
|
extern "C" {
|
||
|
|
#endif
|
||
|
|
|
||
|
|
/**
|
||
|
|
* \addtogroup DCL_API_MODULE APIs for Digital Control Library
|
||
|
|
* @{
|
||
|
|
*
|
||
|
|
* \file dcl_pi.h
|
||
|
|
* \brief Contains PI controller with its related structures and functions
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "../dcl_common.h"
|
||
|
|
|
||
|
|
//--- Linear PI controller ---------------------------------------------------
|
||
|
|
|
||
|
|
//! \brief Defines the DCL_PI shadow parameter set
|
||
|
|
//! used for updating controller parameter
|
||
|
|
//!
|
||
|
|
typedef struct dcl_pi_sps
|
||
|
|
{
|
||
|
|
float32_t Kp; //!< Proportional gain
|
||
|
|
float32_t Ki; //!< Integral gain
|
||
|
|
float32_t Umax; //!< Upper saturation limit
|
||
|
|
float32_t Umin; //!< Lower saturation limit
|
||
|
|
} DCL_PI_SPS;
|
||
|
|
|
||
|
|
//! \brief Defines default values to initialize DCL_PI_SPS
|
||
|
|
//!
|
||
|
|
#define PI_SPS_DEFAULTS { 1.0f, 0.0f, 1.0f, -1.0f }
|
||
|
|
|
||
|
|
//! \brief PI object for storing PI specific parameters
|
||
|
|
//!
|
||
|
|
typedef _DCL_VOLATILE struct dcl_pi
|
||
|
|
{
|
||
|
|
/* controller parameter */
|
||
|
|
float32_t Kp; //!< Proportional gain
|
||
|
|
float32_t Ki; //!< Integral gain
|
||
|
|
float32_t Umax; //!< Upper control saturation limit
|
||
|
|
float32_t Umin; //!< Lower control saturation limit
|
||
|
|
|
||
|
|
/* internal storage */
|
||
|
|
float32_t i6; //!< Saturation storage
|
||
|
|
float32_t i10; //!< I path feedback value
|
||
|
|
float32_t i11; //!< Tustin integrator storage
|
||
|
|
|
||
|
|
/* miscellaneous */
|
||
|
|
DCL_PI_SPS *sps; //!< updates controller parameter
|
||
|
|
DCL_CSS *css; //!< configuration & debugging
|
||
|
|
} DCL_PI, *PI_Handle;
|
||
|
|
|
||
|
|
//! \brief Defines default values to initialize DCL_PI
|
||
|
|
//!
|
||
|
|
#define PI_DEFAULTS { 1.0f, 0.0f, 1.0f, -1.0f, 1.0f, 0.0f, 0.0f, \
|
||
|
|
&(DCL_PI_SPS)PI_SPS_DEFAULTS, &(DCL_CSS)DCL_CSS_DEFAULTS }
|
||
|
|
|
||
|
|
//! \brief Macro for internal default values to initialize DCL_PI
|
||
|
|
//! Example: DCL_PI pi_ctrl = {
|
||
|
|
//! .Kp = 1.0f,
|
||
|
|
//! .Ki = 0.0f,
|
||
|
|
//! .Umax = 1.0f,
|
||
|
|
//! .Umin = -1.0f,
|
||
|
|
//! PI_INT_DEFAULTS
|
||
|
|
//! };
|
||
|
|
#define PI_INT_DEFAULTS .i6=1.0f, .i10=0.0f, .i11=0.0f, \
|
||
|
|
.sps=&(DCL_PI_SPS)PI_SPS_DEFAULTS, .css=&(DCL_CSS)DCL_CSS_DEFAULTS
|
||
|
|
|
||
|
|
//! \brief Initialize DCL_PI struct with default parameters
|
||
|
|
//! Example: DCL_PI* pi_ctrl = DCL_initPI();
|
||
|
|
//!
|
||
|
|
//! \return A DCL_PI* pointer
|
||
|
|
//!
|
||
|
|
#define DCL_initPI() &(DCL_PI)PI_DEFAULTS
|
||
|
|
|
||
|
|
//! \brief Initialize DCL_PI struct with input controller parameters
|
||
|
|
//! Example: DCL_PI* pi_ctrl = DCL_initPIasParam(1.0f,0.0f,1.0f,-1.0f);
|
||
|
|
//! Note: input parameter needs to be in the same order as listed in PI_SPS struct
|
||
|
|
//!
|
||
|
|
//! \return A DCL_PI* pointer
|
||
|
|
//!
|
||
|
|
#define DCL_initPIasParam(kp,ki,umax,umin) &(DCL_PI){ .Kp=kp, .Ki=ki, \
|
||
|
|
.Umax=umax, .Umin=umin, PI_INT_DEFAULTS }
|
||
|
|
|
||
|
|
//! \brief Initialize DCL_PI struct with sps parameters
|
||
|
|
//! Example: DCL_PI_SPS pi_sps = { .Kp = , .Ki = , ...};
|
||
|
|
//! DCL_PI pi_ctrl;
|
||
|
|
//! DCL_initPIasSPS(&pi_ctrl,&pi_sps);
|
||
|
|
//!
|
||
|
|
//! \param[in] pi_ptr DCL_PI* pointer that needs to be initialized
|
||
|
|
//! \param[in] sps_ptr DCL_PI_SPS* pointer with assigned parameters
|
||
|
|
//! \return Returns DCL_PI* with set sps parameters, default parameter will be used
|
||
|
|
//! if sps_ptr is not specified
|
||
|
|
//!
|
||
|
|
#define DCL_initPIasSPS(pi_ptr,sps_ptr) \
|
||
|
|
({ \
|
||
|
|
DCL_PI* new_pi = (pi_ptr) ? pi_ptr : DCL_initPI(); \
|
||
|
|
DCL_PI_SPS* new_sps = (sps_ptr) ? sps_ptr : &(DCL_PI_SPS)PI_SPS_DEFAULTS; \
|
||
|
|
if(sps_ptr) \
|
||
|
|
{ \
|
||
|
|
*new_pi = (DCL_PI){ (new_sps)->Kp, (new_sps)->Ki, (new_sps)->Umax, (new_sps)->Umin,\
|
||
|
|
1.0f, 0.0f, 0.0f, (DCL_PI_SPS*)new_sps, &(DCL_CSS)DCL_CSS_DEFAULTS }; \
|
||
|
|
} \
|
||
|
|
new_pi; \
|
||
|
|
})
|
||
|
|
|
||
|
|
//! \brief Resets PI internal storage data with interrupt protection
|
||
|
|
//!
|
||
|
|
//! \param[in] pi Pointer to the DCL_PI structure
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS
|
||
|
|
void DCL_resetPI(DCL_PI *pi)
|
||
|
|
{
|
||
|
|
dcl_interrupt_t ints = DCL_disableInts();
|
||
|
|
pi->i6 = 1.0f;
|
||
|
|
pi->i10 = pi->i11 = 0.0f;
|
||
|
|
DCL_restoreInts(ints);
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Loads PI tuning parameter from its SPS parameter
|
||
|
|
//!
|
||
|
|
//! \param[in] pi Pointer to the DCL_PI controller structure
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS
|
||
|
|
void DCL_fupdatePI(DCL_PI *pi)
|
||
|
|
{
|
||
|
|
|
||
|
|
#ifdef DCL_ERROR_HANDLING_ENABLED
|
||
|
|
uint32_t err_code = dcl_none;
|
||
|
|
err_code |= (pi->sps->Umax <= pi->sps->Umin) ? dcl_param_invalid_err : dcl_none;
|
||
|
|
err_code |= (pi->css->t_sec <= 0.0f) ? dcl_param_range_err : dcl_none;
|
||
|
|
err_code |= (pi->sps->Kp < 0.0f) ? dcl_param_range_err : dcl_none;
|
||
|
|
err_code |= (pi->sps->Ki < 0.0f) ? dcl_param_range_err : dcl_none;
|
||
|
|
if (err_code)
|
||
|
|
{
|
||
|
|
DCL_setError(pi,err_code);
|
||
|
|
DCL_getErrorInfo(pi);
|
||
|
|
DCL_runErrorHandler(pi);
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
pi->Kp = pi->sps->Kp;
|
||
|
|
pi->Ki = pi->sps->Ki;
|
||
|
|
pi->Umax = pi->sps->Umax;
|
||
|
|
pi->Umin = pi->sps->Umin;
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Updates PI parameter from its SPS parameter with interrupt protection
|
||
|
|
//!
|
||
|
|
//! \param[in] pi Pointer to the DCL_PI controller structure
|
||
|
|
//! \return 'true' if update is successful, otherwise 'false'
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS _DCL_CODE_SECTION
|
||
|
|
bool DCL_updatePI(DCL_PI *pi)
|
||
|
|
{
|
||
|
|
|
||
|
|
#ifdef DCL_ERROR_HANDLING_ENABLED
|
||
|
|
uint32_t err_code = dcl_none;
|
||
|
|
err_code |= (pi->sps->Umax <= pi->sps->Umin) ? dcl_param_invalid_err : dcl_none;
|
||
|
|
err_code |= (pi->css->t_sec <= 0.0f) ? dcl_param_range_err : dcl_none;
|
||
|
|
err_code |= (pi->sps->Kp < 0.0f) ? dcl_param_range_err : dcl_none;
|
||
|
|
err_code |= (pi->sps->Ki < 0.0f) ? dcl_param_range_err : dcl_none;
|
||
|
|
if (err_code)
|
||
|
|
{
|
||
|
|
DCL_setError(pi,err_code);
|
||
|
|
DCL_getErrorInfo(pi);
|
||
|
|
DCL_runErrorHandler(pi);
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
if (!DCL_getUpdateStatus(pi))
|
||
|
|
{
|
||
|
|
dcl_interrupt_t ints = DCL_disableInts();
|
||
|
|
DCL_setUpdateStatus(pi);
|
||
|
|
pi->Kp = pi->sps->Kp;
|
||
|
|
pi->Ki = pi->sps->Ki;
|
||
|
|
pi->Umax = pi->sps->Umax;
|
||
|
|
pi->Umin = pi->sps->Umin;
|
||
|
|
DCL_clearUpdateStatus(pi);
|
||
|
|
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 PI
|
||
|
|
//! parameter from its SPS parameter and clear the status flag on completion.
|
||
|
|
//! Note: Use DCL_setPendingStatus(pi) to set the pending status.
|
||
|
|
//!
|
||
|
|
//! \param[in] pi Pointer to the DCL_PI controller structure
|
||
|
|
//! \return 'true' if an update is applied, otherwise 'false'
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS _DCL_CODE_SECTION
|
||
|
|
bool DCL_pendingUpdatePI(DCL_PI *pi)
|
||
|
|
{
|
||
|
|
if (DCL_getPendingStatus(pi) && DCL_updatePI(pi))
|
||
|
|
{
|
||
|
|
DCL_clearPendingStatus(pi);
|
||
|
|
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] pi Pointer to the active DCL_PI controller structure
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS
|
||
|
|
void DCL_updatePISPS(DCL_PI *pi)
|
||
|
|
{
|
||
|
|
pi->sps->Kp = pi->Kp;
|
||
|
|
pi->sps->Ki = pi->Ki;
|
||
|
|
pi->sps->Umax = pi->Umax;
|
||
|
|
pi->sps->Umin = pi->Umin;
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Configures a series PI controller in "zero-pole-gain" form
|
||
|
|
//! Note: Sampling period pi->css->t_sec are used in the calculation.
|
||
|
|
//! New settings take effect after DCL_updatePI().
|
||
|
|
//! Only z1 considered in DCL_ZPK3, other poles & zeros ignored.
|
||
|
|
//! Zero frequency assumed to be in radians/sec.
|
||
|
|
//!
|
||
|
|
//! \param[in] pi Pointer to the DCL_PI controller structure
|
||
|
|
//! \param[in] zpk Pointer to the ZPK3 structure
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS
|
||
|
|
void DCL_loadSeriesPIasZPK(DCL_PI *pi, DCL_ZPK3 *zpk)
|
||
|
|
{
|
||
|
|
#ifdef DCL_ERROR_HANDLING_ENABLED
|
||
|
|
uint32_t err_code = dcl_none;
|
||
|
|
err_code |= (zpk->K < 0.0f) ? dcl_param_range_err : dcl_none;
|
||
|
|
err_code |= (crealf(zpk->z1) > (1.0f / (2.0f * pi->css->t_sec))) ? dcl_param_warn_err : dcl_none;
|
||
|
|
if (err_code)
|
||
|
|
{
|
||
|
|
DCL_setError(pi,err_code);
|
||
|
|
DCL_getErrorInfo(pi);
|
||
|
|
DCL_runErrorHandler(pi);
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
float32_t t_sec = pi->css->t_sec;
|
||
|
|
float32_t z1 = (float32_t) crealf(zpk->z1);
|
||
|
|
pi->sps->Kp = zpk->K * (1.0f + (t_sec * z1) / 2.0f);
|
||
|
|
pi->sps->Ki = (-2.0f * t_sec * z1) / (2.0f + (t_sec * z1));
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Configures a parallel PI controller in "zero-pole-gain" form
|
||
|
|
//! Note: Sampling period pi->css->t_sec are used in the calculation.
|
||
|
|
//! New settings take effect after DCL_updatePI().
|
||
|
|
//! Zero frequency assumed to be in radians/sec.
|
||
|
|
//!
|
||
|
|
//! \param[in] pi Pointer to the active DCL_PI controller structure
|
||
|
|
//! \param[in] zpk Pointer to the ZPK3 structure
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS
|
||
|
|
void DCL_loadParallelPIasZPK(DCL_PI *pi, DCL_ZPK3 *zpk)
|
||
|
|
{
|
||
|
|
#ifdef DCL_ERROR_HANDLING_ENABLED
|
||
|
|
uint32_t err_code = dcl_none;
|
||
|
|
err_code |= (zpk->K < 0.0f) ? dcl_param_range_err : dcl_none;
|
||
|
|
err_code |= (crealf(zpk->z1) > (1.0f / (2.0f * pi->css->t_sec))) ? dcl_param_warn_err : dcl_none;
|
||
|
|
if (err_code)
|
||
|
|
{
|
||
|
|
DCL_setError(pi,err_code);
|
||
|
|
DCL_getErrorInfo(pi);
|
||
|
|
DCL_runErrorHandler(pi);
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
float32_t t_sec = pi->css->t_sec;
|
||
|
|
float32_t z1 = (float32_t) crealf(zpk->z1);
|
||
|
|
pi->sps->Kp = zpk->K * (1.0f + (t_sec * z1) / 2.0f);
|
||
|
|
pi->sps->Ki = -zpk->K * t_sec * z1;
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Executes an inline series form PI controller
|
||
|
|
//!
|
||
|
|
//! \param[in] pi 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
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS _DCL_CODE_SECTION
|
||
|
|
float32_t DCL_runPISeries(DCL_PI *pi, float32_t rk, float32_t yk)
|
||
|
|
{
|
||
|
|
float32_t v2, v4, v5, v9;
|
||
|
|
|
||
|
|
v2 = pi->Kp * (rk - yk);
|
||
|
|
v4 = pi->i10 + (pi->Ki * pi->i6 * v2);
|
||
|
|
v5 = v2 + v4;
|
||
|
|
v9 = DCL_runSat(v5, pi->Umax, pi->Umin);
|
||
|
|
pi->i6 = (v5 == v9) ? 1.0f : 0.0f;
|
||
|
|
pi->i10 = v4;
|
||
|
|
|
||
|
|
#ifdef DCL_TESTPOINTS_ENABLED
|
||
|
|
pi->css->tpt = v5;
|
||
|
|
#endif
|
||
|
|
|
||
|
|
return(v9);
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Executes a parallel form PI controller
|
||
|
|
//! Implemented as inline C function
|
||
|
|
//!
|
||
|
|
//! \param[in] pi 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
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS _DCL_CODE_SECTION
|
||
|
|
float32_t DCL_runPIParallel(DCL_PI *pi, float32_t rk, float32_t yk)
|
||
|
|
{
|
||
|
|
float32_t v1, v2, v4, v5, v9;
|
||
|
|
|
||
|
|
v1 = rk - yk;
|
||
|
|
v2 = pi->Kp * v1;
|
||
|
|
v4 = (v1 * pi->Ki * pi->i6) + pi->i10;
|
||
|
|
pi->i10 = v4;
|
||
|
|
v5 = v2 + v4;
|
||
|
|
v9 = DCL_runSat(v5, pi->Umax, pi->Umin);
|
||
|
|
pi->i6 = (v5 == v9) ? 1.0f : 0.0f;
|
||
|
|
|
||
|
|
#ifdef DCL_TESTPOINTS_ENABLED
|
||
|
|
pi->css->tpt = v5;
|
||
|
|
#endif
|
||
|
|
|
||
|
|
return(v9);
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Executes a parallel form PI controller with
|
||
|
|
//! enhanced anti-windup logic incorporating an
|
||
|
|
//! addintional integrator saturation clamp
|
||
|
|
//!
|
||
|
|
//! \param[in] pi Pointer to the DCL_PI structure
|
||
|
|
//! \param[in] rk The controller set-point reference
|
||
|
|
//! \param[in] yk The measured feedback value
|
||
|
|
//! \param[in] Imax Upper integrator saturation limit
|
||
|
|
//! \param[in] Imin Lower integrator saturation limit
|
||
|
|
//! \return The control effort
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS _DCL_CODE_SECTION
|
||
|
|
float32_t DCL_runPIParallelEnhanced(DCL_PI *pi, float32_t rk, float32_t yk, float32_t Imax, float32_t Imin)
|
||
|
|
{
|
||
|
|
float32_t v1, v5, v7, v8;
|
||
|
|
bool l11, l14, l17, l18, l19;
|
||
|
|
|
||
|
|
v1 = rk - yk;
|
||
|
|
v5 = (v1 * pi->Ki * pi->i6) + pi->i10;
|
||
|
|
pi->i10 = v5;
|
||
|
|
v7 = (v1 * pi->Kp) + v5;
|
||
|
|
v8 = DCL_runSat(v7, pi->Umax, pi->Umin);
|
||
|
|
l17 = (v7 == v8) ? true : false;
|
||
|
|
l11 = (DCL_runSat(v5,Imax,Imin) == v5) ? true : false;
|
||
|
|
l19 = (v5 > 0) ? true : false;
|
||
|
|
l14 = (v1 > 0) ? true : false;
|
||
|
|
l18 = l17 && (!l11 || (l19 ^ l14));
|
||
|
|
pi->i6 = (l18) ? 1.0f : 0.0f;
|
||
|
|
|
||
|
|
#ifdef DCL_TESTPOINTS_ENABLED
|
||
|
|
pi->css->tpt = v7;
|
||
|
|
#endif
|
||
|
|
|
||
|
|
return(v8);
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Executes a series form PI controller with Tustin integrator
|
||
|
|
//!
|
||
|
|
//! \param[in] pi 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
|
||
|
|
//!
|
||
|
|
_DCL_CODE_ACCESS _DCL_CODE_SECTION
|
||
|
|
float32_t DCL_runPISeriesTustin(DCL_PI *pi, float32_t rk, float32_t yk)
|
||
|
|
{
|
||
|
|
float32_t v2, v4, v5, v8, v9;
|
||
|
|
|
||
|
|
v2 = (rk - yk) * pi->Kp;
|
||
|
|
v8 = v2 * pi->Ki * pi->i6;
|
||
|
|
v4 = v8 + pi->i11 + pi->i10;
|
||
|
|
v5 = v2 + v4;
|
||
|
|
pi->i10 = v4;
|
||
|
|
pi->i11 = v8;
|
||
|
|
v9 = DCL_runSat(v5, pi->Umax, pi->Umin);
|
||
|
|
pi->i6 = (v5 == v9) ? 1.0f : 0.0f;
|
||
|
|
|
||
|
|
#ifdef DCL_TESTPOINTS_ENABLED
|
||
|
|
pi->css->tpt = v4;
|
||
|
|
#endif
|
||
|
|
|
||
|
|
return(v9);
|
||
|
|
}
|
||
|
|
|
||
|
|
/** @} */
|
||
|
|
|
||
|
|
#ifdef __cplusplus
|
||
|
|
}
|
||
|
|
#endif // extern "C"
|
||
|
|
|
||
|
|
#endif // _DCL_PI_H_
|