External/c2000ware-core-sdk
8
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
main
c2000ware-core-sdk/libraries/control/DCL/c28/include/DCLF32.h
Deshpande 0ad1c07e52 Added C2000Ware v5.01.00.00 contents
2023-12-13 16:46:16 +05:30

1931 lines
66 KiB
C
Raw Permalink Blame History

/* DCLF32.h - C2000 Digital Control Library
*
*/
//#############################################################################
//!
//! Copyright: Copyright (C) 2023 Texas Instruments Incorporated -
//! All rights reserved not granted herein.
//! Limited License.
//!
//! Texas Instruments Incorporated grants a world-wide, royalty-free,
//! non-exclusive license under copyrights and patents it now or hereafter
//! owns or controls to make, have made, use, import, offer to sell and sell
//! ("Utilize") this software subject to the terms herein. With respect to the
//! foregoing patent license, such license is granted solely to the extent that
//! any such patent is necessary to Utilize the software alone. The patent
//! license shall not apply to any combinations which include this software,
//! other than combinations with devices manufactured by or for TI
//! ("TI Devices").
//! No hardware patent is licensed hereunder.
//!
//! Redistributions must preserve existing copyright notices and reproduce this
//! license (including the above copyright notice and the disclaimer and
//! (if applicable) source code license limitations below) in the documentation
//! and/or other materials provided with the distribution.
//!
//! Redistribution and use in binary form, without modification, are permitted
//! provided that the following conditions are met:
//!
//! * No reverse engineering, decompilation, or disassembly of this software is
//! permitted with respect to any software provided in binary form.
//! * Any redistribution and use are licensed by TI for use only
//! with TI Devices.
//! * Nothing shall obligate TI to provide you with source code for the
//! software licensed and provided to you in object code.
//!
//! If software source code is provided to you, modification and redistribution
//! of the source code are permitted provided that the following conditions
//! are met:
//!
//! * any redistribution and use of the source code, including any resulting
//! derivative works, are licensed by TI for use only with TI Devices.
//! * any redistribution and use of any object code compiled from the source
//! code and any resulting derivative works, are licensed by TI for use
//! only with TI Devices.
//!
//! Neither the name of Texas Instruments Incorporated nor the names of its
//! suppliers may be used to endorse or promote products derived from this
//! software without specific prior written permission.
//#############################################################################
#ifndef _C_DCLF32_H
#define _C_DCLF32_H
#ifdef __cplusplus
extern "C" {
#endif
//! \file DCLF32.h
//! \brief Contains the public interface to the FPU32
//! Digital Controller Library functions
#ifdef __TMS320C28XX__
#include <math.h>
#endif
#include "DCL.h"
#include <complex.h>
#include <stdint.h>
//--- Linear PID controller --------------------------------------------------
#pragma CODE_SECTION(DCL_updatePID,"dclfuncs")
#pragma CODE_SECTION(DCL_runPID_C2,"dclfuncs")
#pragma CODE_SECTION(DCL_runPID_C4,"dclfuncs")
//! \brief Defines the DCL_PID shadow parameter set
//!
typedef struct dcl_pid_sps {
float32_t Kp; //!< Proportional gain
float32_t Ki; //!< Integral gain
float32_t Kd; //!< Derivative gain
float32_t Kr; //!< Set point weight
float32_t c1; //!< D path filter coefficient 1
float32_t c2; //!< D path filter coefficient 2
float32_t Umax; //!< Upper saturation limit
float32_t Umin; //!< Lower saturation limit
} DCL_PID_SPS;
//! \brief Defines default values to initialize the DCL_PID shadow structure
//!
#define PID_SPS_DEFAULTS { 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 1.0f, -1.0f }
//! \brief Defines the DCL_PID active controller structure
//!
typedef struct dcl_pid {
float32_t Kp; //!< Proportional gain
float32_t Ki; //!< Integral gain
float32_t Kd; //!< Derivative gain
float32_t Kr; //!< Set point weight
float32_t c1; //!< D path filter coefficient 1
float32_t c2; //!< D path filter coefficient 2
float32_t d2; //!< D path filter intermediate storage 1
float32_t d3; //!< D path filter intermediate storage 2
float32_t i10; //!< I path intermediate storage
float32_t i14; //!< Intermediate saturation storage
float32_t Umax; //!< Upper saturation limit
float32_t Umin; //!< Lower saturation limit
DCL_PID_SPS *sps; //!< Pointer to the shadow parameter set
DCL_CSS *css; //!< Pointer to the common support structure
} DCL_PID;
//! \brief Defines default values to initialize the DCL_PID structure
//!
#define PID_DEFAULTS { 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, \
0.0f, 1.0f, 1.0f, -1.0f, \
NULL_ADDR, NULL_ADDR }
//! \brief Resets PID internal storage data
//! \param[in] p Pointer to the DCL_PID structure
//! \return None
//!
static inline void DCL_resetPID(DCL_PID *p)
{
uint16_t v;
v = DCL_DISABLE_INTS;
p->d2 = p->d3 = p->i10 = 0.0f;
p->i14 = 1.0f;
DCL_RESTORE_INTS(v);
}
//! \brief Loads PID tuning parameters
//! \param[in] p Pointer to the active DCL_PID controller structure
//! \return None
//!
static inline void DCL_updatePID(DCL_PID *p)
{
uint16_t v;
#ifdef DCL_ERROR_HANDLING_ENABLED
float32_t tau = (2.0f - p->sps->c1 * p->css->T) / (2.0f * p->sps->c1);
float32_t ec2 = p->sps->c1 * (p->css->T - 2.0f * tau) / 2.0f;
p->css->err |= !F32_IS_VALUE(p->sps->c2, ec2) ? ERR_PARAM_INVALID : ERR_NONE;
p->css->err |= (p->sps->Umax <= p->sps->Umin) ? ERR_PARAM_INVALID : ERR_NONE;
p->css->err |= (p->css->T <= 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
p->css->err |= ((p->sps->Kp < 0.0f) || (p->sps->Ki < 0.0f) || (p->sps->Kd < 0.0f) || (p->sps->Kr < 0.0f)) ? ERR_PARAM_RANGE : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
if (p->css->sts & STS_UPDATE_PENDING)
{
v = DCL_DISABLE_INTS;
p->Kp = p->sps->Kp;
p->Ki = p->sps->Ki;
p->Kd = p->sps->Kd;
p->Kr = p->sps->Kr;
p->c1 = p->sps->c1;
p->c2 = p->sps->c2;
p->Umax = p->sps->Umax;
p->Umin = p->sps->Umin;
DCL_RESTORE_INTS(v);
DCL_CLEAR_UPDATE_REQUEST(p);
}
}
//! \brief Loads PID tuning parameters
//! Implemented as assembly module
//! \param[in] p Pointer to the active DCL_PID controller structure
//! \return None
//!
extern void DCL_fupdatePID(DCL_PID *p);
//! \brief Loads the derivative path filter shadow coefficients
//! Note: new coefficients take effect when DCL_updatePID() is called
//! \param[in] p Pointer to the DCL_PID structure
//! \param[in] fc The desired filter bandwidth in Hz
//! \return None
//!
static inline void DCL_setPIDfilterBW(DCL_PID *p, float32_t fc)
{
float32_t tau;
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= ((fc >= 1.0f / (2.0f * p->css->T)) || (fc <= 0.0f)) ? ERR_PARAM_RANGE : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
tau = 1.0f / (2.0f * CONST_PI_32 * fc);
p->sps->c1 = 2.0f / (p->css->T + 2.0f * tau);
p->sps->c2 = (p->css->T - 2.0f * tau) / (p->css->T + 2.0f * tau);
}
//! \brief Loads the PID derivative path filter active coefficients
//! Note: new coefficients take effect immediately. SPS &
//! CSS contents are unaffected.
//! \param[in] p Pointer to the DCL_PID structure
//! \param[in] fc The desired filter bandwidth in Hz
//! \param[in] T The controller update rate in seconds
//! \return None
//!
static inline void DCL_setActivePIDfilterBW(DCL_PID *p, float32_t fc, float32_t T)
{
float32_t tau;
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= ((fc >= 1.0f / (2.0f * T)) || (fc <= 0.0f)) ? ERR_PARAM_RANGE : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
tau = 1.0f / (2.0f * CONST_PI_32 * fc);
p->c1 = 2.0f / (T + 2.0f * tau);
p->c2 = (T - 2.0f * tau) / (T + 2.0f * tau);
}
//! \brief Returns the active derivative path filter bandwidth in Hz
//! \param[in] p Pointer to the DCL_PID structure
//! \return The filter bandwidth in Hz
//!
static inline float32_t DCL_getPIDfilterBW(DCL_PID *p)
{
float32_t tau = ((2.0f - p->c1 * p->css->T) / (2.0f * p->c1));
return(1.0f / (2.0f * CONST_PI_32 * tau));
}
//! \brief Configures a series PID controller in ZPK form
//! Note: parameters take effect after call to DCL_updatePID()
//! Only z1, z2 & p2 considered. p1 = 0 assumed.
//! \param[in] p Pointer to the active DCL_PID controller structure
//! \param[in] q Pointer to the DCL_ZPK3 structure
//! \return None
//!
static inline void DCL_loadSeriesPIDasZPK(DCL_PID *p, DCL_ZPK3 *q)
{
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= (!F32_IS_VALUE(cimagf(q->z1) + cimagf(q->z2), 0.0f)) ? ERR_PARAM_INVALID : ERR_NONE;
p->css->err |= (!F32_IS_VALUE(cimagf(q->p2), 0.0f)) ? ERR_PARAM_INVALID : ERR_NONE;
p->css->err |= (crealf(q->p2) > DCL_C2_LIMIT_32) ? ERR_PARAM_INVALID : ERR_NONE;
p->css->err |= (q->K < 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
float32_t beta1 = -(float32_t) crealf(q->z1 + q->z2);
float32_t beta0 = (float32_t) crealf(q->z1 * q->z2);
float32_t alpha1 = -(float32_t) crealf(q->p1 + q->p2);
float32_t alpha0 = (float32_t) crealf(q->p1 * q->p2);
float32_t T = p->css->T;
float32_t a0p = 4.0f + (alpha1 * 2.0f * T) + (alpha0 * T * T);
float32_t b0 = q->K * (4.0f + (beta1 * 2.0f * T) + (beta0 * T * T)) / a0p;
float32_t b1 = q->K * (-8.0f + (2.0f * beta0 * T * T)) / a0p;
float32_t b2 = q->K * (4.0f - (beta1 * 2.0f * T) + (beta0 * T * T)) / a0p;
float32_t a2 = (4.0f - (alpha1 * 2.0f * T) + (alpha0 * T * T)) / a0p;
float32_t c2 = -a2;
float32_t tau = (p->css->T / 2.0f) * (1.0f - c2) / (1.0f + c2);
p->sps->c1 = 2.0f / (p->css->T + 2.0f*tau);
p->sps->c2 = c2;
float32_t det = (c2 + 1.0f);
det *= det;
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= (F32_IS_ZERO(det)) ? ERR_PARAM_INVALID : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
float32_t k1 = (c2*b0 - b1 - (2.0f + c2)*b2) / det;
float32_t k2 = (c2 + 1.0f) * (b0 + b1 + b2) / det;
float32_t k3 = (c2*c2*b0 - c2*b1 + b2) / det;
p->sps->Kp = k1;
p->sps->Ki = k2 / k1;
p->sps->Kd = k3 / (k1*p->sps->c1);
#ifdef DCL_TESTPOINTS_ENABLED
p->css->tpt = det;
#endif
}
//! \brief Configures a parallel PID controller in ZPK form
//! Note: parameters take effect after call to DCL_updatePID()
//! Only z1, z2 & p2 considered. p1 = 0 assumed.
//! \param[in] p Pointer to the active DCL_PID controller structure
//! \param[in] q Pointer to the DCL_ZPK3 structure
//! \return None
//!
static inline void DCL_loadParallelPIDasZPK(DCL_PID *p, DCL_ZPK3 *q)
{
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= (!F32_IS_VALUE(cimagf(q->z1) + cimagf(q->z2), 0.0f)) ? ERR_PARAM_INVALID : ERR_NONE;
p->css->err |= (!F32_IS_VALUE(cimagf(q->p2), 0.0f)) ? ERR_PARAM_INVALID : ERR_NONE;
p->css->err |= (crealf(q->p2) > DCL_C2_LIMIT_32) ? ERR_PARAM_INVALID : ERR_NONE;
p->css->err |= (q->K < 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
float32_t beta1 = -(float32_t) crealf(q->z1 + q->z2);
float32_t beta0 = (float32_t) crealf(q->z1 * q->z2);
float32_t alpha1 = -(float32_t) crealf(q->p1 + q->p2);
float32_t alpha0 = (float32_t) crealf(q->p1 * q->p2);
float32_t T = p->css->T;
float32_t a0p = 4.0f + (alpha1 * 2.0f * T) + (alpha0 * T * T);
float32_t b0 = q->K * (4.0f + (beta1 * 2.0f * T) + (beta0 * T * T)) / a0p;
float32_t b1 = q->K * (-8.0f + (2.0f * beta0 * T * T)) / a0p;
float32_t b2 = q->K * (4.0f - (beta1 * 2.0f * T) + (beta0 * T * T)) / a0p;
float32_t a2 = (4.0f - (alpha1 * 2.0f * T) + (alpha0 * T * T)) / a0p;
float32_t c2 = -a2;
float32_t tau = (p->css->T / 2.0f) * (1.0f - c2) / (1.0f + c2);
p->sps->c1 = 2.0f / (p->css->T + 2.0f*tau);
p->sps->c2 = c2;
float32_t det = (c2 + 1.0f);
det *= det;
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= (F32_IS_ZERO(det)) ? ERR_PARAM_INVALID : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
p->sps->Kp = (c2*b0 - b1 - (2.0f + c2)*b2) / det;
p->sps->Ki = (c2 + 1.0f) * (b0 + b1 + b2) / det;
p->sps->Kd = (c2*c2*b0 - c2*b1 + b2) / (det * p->sps->c1);
#ifdef DCL_TESTPOINTS_ENABLED
p->css->tpt = det;
#endif
}
//! \brief Executes an ideal form PID controller on the FPU32
//! Implemented as an external assembly module
//! \param[in] p Pointer to the DCL_PID structure
//! \param[in] rk The controller set-point reference
//! \param[in] yk The measured feedback value
//! \param[in] lk External output clamp flag
//! \return The control effort
//!
extern float32_t DCL_runPID_C1(DCL_PID *p, float32_t rk, float32_t yk, float32_t lk);
//! \brief Executes an ideal form PID controller on the FPU32
//! \param[in] p Pointer to the DCL_PID structure
//! \param[in] rk The controller set-point reference
//! \param[in] yk The measured feedback value
//! \param[in] lk External output clamp flag
//! \return The control effort
//!
static inline float32_t DCL_runPID_C2(DCL_PID *p, float32_t rk, float32_t yk, float32_t lk)
{
float32_t v1, v4, v5, v8, v9, v10, v12;
v5 = (p->Kr * rk) - yk;
v8 = ((rk - yk) * p->Ki * p->Kp * p->i14) + p->i10;
p->i10 = v8;
v1 = yk * p->Kd * p->c1;
v4 = v1 - p->d2 - p->d3;
p->d2 = v1;
p->d3 = v4 * p->c2;
v9 = ((v5 - v4) * p->Kp) + v8;
v10 = (v9 > p->Umax) ? p->Umax : v9;
v10 = (v10 < p->Umin) ? p->Umin : v10;
v12 = (v10 == v9) ? 1.0f : 0.0f;
p->i14 = v12 * lk;
#ifdef DCL_TESTPOINTS_ENABLED
p->css->tpt = v4;
#endif
return(v10);
}
//! \brief Executes an parallel form PID controller on the FPU32
//! \param[in] p Pointer to the DCL_PID structure
//! \param[in] rk The controller set-point reference
//! \param[in] yk The measured feedback value
//! \param[in] lk External output clamp flag
//! \return The control effort
//!
static inline float32_t DCL_runPID_C3(DCL_PID *p, float32_t rk, float32_t yk, float32_t lk)
{
float32_t v1, v4, v5, v6, v8, v9, v10, v12;
v5 = rk - yk;
v6 = v5 * p->Kp;
v8 = v5 * p->Ki * p->i14 + p->i10;
p->i10 = v8;
v1 = v5 * p->Kd * p->c1;
v4 = v1 - p->d2 - p->d3;
p->d2 = v1;
p->d3 = v4 * p->c2;
v9 = v6 + v8 + v4;
v10 = (v9 > p->Umax) ? p->Umax : v9;
v10 = (v10 < p->Umin) ? p->Umin : v10;
v12 = (v10 == v9) ? 1.0f : 0.0f;
p->i14 = v12 * lk;
#ifdef DCL_TESTPOINTS_ENABLED
p->css->tpt = v8;
#endif
return(v10);
}
//! \brief Executes a parallel form PID controller on the FPU32
//! Implemented as an external assembly module
//! \param[in] p Pointer to the DCL_PID structure
//! \param[in] rk The controller set-point reference
//! \param[in] yk The measured feedback value
//! \param[in] lk External output clamp flag
//! \return The control effort
//!
extern float32_t DCL_runPID_C4(DCL_PID *p, float32_t rk, float32_t yk, float32_t lk);
//--- Linear PI controller ---------------------------------------------------
#pragma CODE_SECTION(DCL_updatePI,"dclfuncs")
#pragma CODE_SECTION(DCL_runPI_C2,"dclfuncs")
#pragma CODE_SECTION(DCL_runPI_C3,"dclfuncs")
#pragma CODE_SECTION(DCL_runPI_C5,"dclfuncs")
#pragma CODE_SECTION(DCL_runPI_C6,"dclfuncs")
//! \brief Defines the DCL_PI shadow parameter set
//!
typedef struct dcl_pi_sps {
float32_t Kp; //!< Proportional gain
float32_t Ki; //!< Integral gain
float32_t Umax; //!< Upper control saturation limit
float32_t Umin; //!< Lower control saturation limit
float32_t Imax; //!< Upper integrator saturation limit
float32_t Imin; //!< Lower integrator saturation limit
} DCL_PI_SPS;
//! \brief Defines default values to initialize the DCL_PI shadow structure
//!
#define PI_SPS_DEFAULTS { 1.0f, 0.0f, 1.0f, -1.0f, 1.0f, -1.0f }
//! \brief Defines the DCL_PI controller structure
//!
typedef struct dcl_pi {
float32_t Kp; //!< Proportional gain
float32_t Ki; //!< Integral gain
float32_t i10; //!< I storage
float32_t Umax; //!< Upper control saturation limit
float32_t Umin; //!< Lower control saturation limit
float32_t i6; //!< Saturation storage
float32_t i11; //!< I storage
float32_t Imax; //!< Upper integrator saturation limit
float32_t Imin; //!< Lower integrator saturation limit
DCL_PI_SPS *sps; //!< Pointer to the shadow parameter set
DCL_CSS *css; //!< Pointer to the common support structure
} DCL_PI;
//! \brief Defines default values to initialize the PI structure
//!
#define PI_DEFAULTS { 1.0f, 0.0f, 0.0f, 1.0f, -1.0f, 1.0f, 0.0f, 1.0f, -1.0f, \
NULL_ADDR, NULL_ADDR }
//! \brief Resets PID internal storage data
//! \param[in] p Pointer to the DCL_PI structure
//! \return None
//!
static inline void DCL_resetPI(DCL_PI *p)
{
uint16_t v;
v = DCL_DISABLE_INTS;
p->i10 = 0.0f;
p->i6 = 1.0f;
p->i11 = 0.0f;
DCL_RESTORE_INTS(v);
}
//! \brief Loads active PI controller parameters
//! \param[in] p Pointer to the active DCL_PI controller structure
//! \return None
//!
static inline void DCL_updatePI(DCL_PI *p)
{
uint16_t v;
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= (p->sps->Umax <= p->sps->Umin) ? ERR_PARAM_INVALID : ERR_NONE;
p->css->err |= (p->css->T <= 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
p->css->err |= (p->sps->Kp < 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
p->css->err |= (p->sps->Ki < 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
if (p->css->sts & STS_UPDATE_PENDING)
{
v = DCL_DISABLE_INTS;
p->Kp = p->sps->Kp;
p->Ki = p->sps->Ki;
p->Umax = p->sps->Umax;
p->Umin = p->sps->Umin;
p->Imax = p->sps->Imax;
p->Imin = p->sps->Imin;
DCL_RESTORE_INTS(v);
DCL_CLEAR_UPDATE_REQUEST(p);
}
}
//! \brief Loads PI tuning parameters
//! Implemented as assembly module
//! \param[in] p Pointer to the active DCL_PI controller structure
//! \return None
//!
extern void DCL_fupdatePI(DCL_PI *p);
//! \brief Configures a series PI controller in "zero-pole-gain" form
//! Note: 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] p Pointer to the DCL_PI controller structure
//! \param[in] q Pointer to the ZPK3 structure
//! \return None
//!
static inline void DCL_loadSeriesPIasZPK(DCL_PI *p, DCL_ZPK3 *q)
{
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= (q->K < 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
p->css->err |= (crealf(q->z1) > (1.0f / (2.0f * p->css->T))) ? ERR_PARAM_WARN : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
float32_t z1 = (float32_t) crealf(q->z1);
float32_t T = p->css->T;
p->sps->Kp = q->K * (1.0f + T * z1 / 2.0f);
p->sps->Ki = (-2.0f * T * z1) / (2.0f + T * z1);
}
//! \brief Configures a parallel PI controller in "zero-pole-gain" form
//! Note: new settings take effect after DCL_updatePI()
//! Zero frequency assumed to be in radians/sec.
//! \param[in] p Pointer to the active DCL_PI controller structure
//! \param[in] q Pointer to the ZPK3 structure
//! \return None
//!
static inline void DCL_loadParallelPIasZPK(DCL_PI *p, DCL_ZPK3 *q)
{
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= (q->K < 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
p->css->err |= (crealf(q->z1) > (1.0f / (2.0f * p->css->T))) ? ERR_PARAM_WARN : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
float32_t z1 = (float32_t) crealf(q->z1);
float32_t T = p->css->T;
p->sps->Kp = q->K * (1.0f + T * z1 / 2.0f);
p->sps->Ki = -q->K * T * z1;
}
//! \brief Executes a series form PI controller on the FPU32
//! Implemented as an external assembly module
//! \param[in] p 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
//!
extern float32_t DCL_runPI_C1(DCL_PI *p, float32_t rk, float32_t yk);
//! \brief Executes an inline series form PI controller on the FPU32
//! \param[in] p 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
//!
static inline float32_t DCL_runPI_C2(DCL_PI *p, float32_t rk, float32_t yk)
{
float32_t v2, v4, v5, v9;
v2 = p->Kp * (rk - yk);
v4 = p->i10 + (p->Ki * p->i6 * v2);
v5 = v2 + v4;
v9 = (v5 > p->Umax) ? p->Umax : v5;
v9 = (v9 < p->Umin) ? p->Umin : v9;
p->i10 = v4;
p->i6 = (v5 == v9) ? 1.0f : 0.0f;
#ifdef DCL_TESTPOINTS_ENABLED
p->css->tpt = v5;
#endif
return(v9);
}
//! \brief Executes a parallel form PI controller on the FPU32
//! Implemented as inline C function
//! \param[in] p 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
//!
static inline float32_t DCL_runPI_C3(DCL_PI *p, float32_t rk, float32_t yk)
{
float32_t v1, v2, v4, v5, v9;
v1 = rk - yk;
v2 = p->Kp * v1;
v4 = (v1 * p->Ki * p->i6) + p->i10;
p->i10 = v4;
v5 = v2 + v4;
v9 = (v5 > p->Umax) ? p->Umax : v5;
v9 = (v9 < p->Umin) ? p->Umin : v9;
p->i6 = (v5 == v9) ? 1.0f : 0.0f;
#ifdef DCL_TESTPOINTS_ENABLED
p->css->tpt = v5;
#endif
return(v9);
}
//! \brief Executes a parallel form PI controller on the FPU32
//! Implemented as an external assembly module
//! \param[in] p 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
//!
extern float32_t DCL_runPI_C4(DCL_PI *p, float32_t rk, float32_t yk);
//! \brief Executes an parallel form PI controller on the FPU32
//! Contains enhanced AWR logic
//! \param[in] p 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
//!
static inline float32_t DCL_runPI_C5(DCL_PI *p, float32_t rk, float32_t yk)
{
float32_t v1, v5, v7, v8;
uint16_t l11, l12, l14, l17, l18, l19;
v1 = rk - yk;
v5 = (v1 * p->Ki * p->i6) + p->i10;
p->i10 = v5;
v7 = (v1 * p->Kp) + v5;
v8 = (v7 > p->Umax) ? p->Umax : v7;
v8 = (v8 < p->Umin) ? p->Umin : v8;
l17 = ((v7 - v8) == 0) ? 1U : 0U;
l11 = (v5 >= p->Imax) ? 1U : 0U;
l12 = (v5 <= p->Imin) ? 1U : 0U;
l19 = (v5 > 0) ? 1U : 0U;
l14 = (v1 > 0) ? 1U : 0U;
l18 = l17 & (!(l11 | l12) | (l19 ^ l14));
p->i6 = (l18 == 0U) ? 0.0f : 1.0f;
#ifdef DCL_TESTPOINTS_ENABLED
p->css->tpt = v7;
#endif
return(v8);
}
//! \brief Executes a series form PI controller with Tustin integrator
//! on the FPU32.
//! \param[in] p 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
//!
static inline float32_t DCL_runPI_C6(DCL_PI *p, float32_t rk, float32_t yk)
{
float32_t v2, v4, v5, v8, v9;
v2 = (rk - yk) * p->Kp;
v8 = v2 * p->Ki * p->i6;
v4 = v8 + p->i11 + p->i10;
v5 = v2 + v4;
p->i10 = v4;
p->i11 = v8;
v9 = (v5 > p->Umax) ? p->Umax : v5;
v9 = (v9 < p->Umin) ? p->Umin : v9;
p->i6 = (v5 == v9) ? 1.0f : 0.0f;
#ifdef DCL_TESTPOINTS_ENABLED
p->css->tpt = v4;
#endif
return(v9);
}
//! \brief Executes a series form PI controller with Tustin integrator
//! on the FPU32. Implemented as assembly module.
//! \param[in] p 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
//!
extern float32_t DCL_runPI_C7(DCL_PI *p, float32_t rk, float32_t yk);
//--- Linear PI2 controller --------------------------------------------------
#pragma CODE_SECTION(DCL_updatePI2,"dclfuncs")
#pragma CODE_SECTION(DCL_runPI2_C1,"dclfuncs")
//! \brief Defines the DCL_PI2 shadow parameter set
//!
typedef struct dcl_pi2_sps {
float32_t Kp; //!< Proportional gain
float32_t Ki; //!< Integral gain
float32_t Umax; //!< Upper saturation limit
float32_t Umin; //!< Lower saturation limit
} DCL_PI2_SPS;
#define PI2_SPS_DEFAULTS { 1.0f, 0.0f, 1.0f, -1.0f }
//! \brief Defines the DCL_PI2 controller structure
//!
typedef struct dcl_pi2 {
float32_t Kp; //!< Proportional gain
float32_t Ki; //!< Integral gain
float32_t i6; //!< Integrator 1 storage
float32_t i9; //!< Integrator 2 storage
float32_t i12; //!< Saturation 1 storage
float32_t i13; //!< Saturation 2 storage
float32_t Umax; //!< Upper saturation limit
float32_t Umin; //!< Lower saturation limit
DCL_PI2_SPS *sps; //!< Pointer to the shadow parameter set
DCL_CSS *css; //!< Pointer to the common support structure
} DCL_PI2;
//! \brief Defines default values to initialize the DCL_PI2 structure
//!
#define PI2_DEFAULTS { 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, -1.0f, \
NULL_ADDR, NULL_ADDR }
//! \brief Resets the integrators of the PI2 controller
//! \param[in] p Pointer to the DCL_PI2 structure
//! \return None
//!
static inline void DCL_resetPI2(DCL_PI2 *p)
{
uint16_t v;
v = DCL_DISABLE_INTS;
p->i6 = p->i9 = 0.0f;
p->i12 = p->i13 = 1.0f;
DCL_RESTORE_INTS(v);
}
//! \brief Loads active controller parameters
//! \param[in] p Pointer to the DCL_PI2 controller structure
//! \return None
//!
static inline void DCL_updatePI2(DCL_PI2 *p)
{
uint16_t v;
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= (p->sps->Umax <= p->sps->Umin) ? ERR_PARAM_INVALID : ERR_NONE;
p->css->err |= (p->css->T <= 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
p->css->err |= (p->sps->Kp < 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
p->css->err |= (p->sps->Ki < 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
if (p->css->sts & STS_UPDATE_PENDING)
{
v = DCL_DISABLE_INTS;
p->Ki = p->sps->Ki;
p->Kp = p->sps->Kp;
p->Umax = p->sps->Umax;
p->Umin = p->sps->Umin;
DCL_RESTORE_INTS(v);
DCL_CLEAR_UPDATE_REQUEST(p);
}
}
//! \brief Loads PI2 tuning parameters
//! Implemented as assembly module
//! \param[in] p Pointer to the active DCL_PI2 controller structure
//! \return None
//!
extern void DCL_fupdatePI2(DCL_PI2 *p);
//! \brief Executes an inline series form PI2 controller on the FPU32
//! \param[in] p Pointer to the DCL_PI2 structure
//! \param[in] rk The controller set-point reference
//! \param[in] yk The measured feedback
//! \return The control effort
//!
static inline float32_t DCL_runPI2_C1(DCL_PI2 *p, float32_t rk, float32_t yk)
{
float32_t v1, v2, v5, v8, v10, v11, v14;
uint16_t l1, l2, l3, l4, l5, l6;
v1 = rk - yk;
v2 = p->Kp * v1;
v5 = (v1 * p->Ki * p->i12) + p->i6;
p->i6 = v5;
v8 = (v5 * p->i13) + p->i9;
p->i9 = v8;
v10 = v2 + v8;
v11 = (v10 > p->Umax) ? p->Umax : v10;
v11 = (v11 < p->Umin) ? p->Umin : v11;
v14 = v10 - v11;
l1 = (v1 > 0.0f) ? 1U : 0U;
l2 = (v14 > 0.0f) ? 1U : 0U;
l3 = (v14 == 0.0f) ? 1U : 0U;
l4 = (v5 > 0.0f) ? 1U : 0U;
l5 = l3 | (l1 ^ l2);
l6 = l3 | (l4 ^ l2);
p->i12 = (l5 == 1U) ? 1.0f : 0.0f;
p->i13 = (l6 == 1U) ? 1.0f : 0.0f;
#ifdef DCL_TESTPOINTS_ENABLED
p->css->tpt = v8;
#endif
return(v11);
}
//--- Direct Form 1 - 1st order ----------------------------------------------
#pragma CODE_SECTION(DCL_updateDF11,"dclfuncs")
#pragma CODE_SECTION(DCL_runDF11_C2,"dclfuncs")
//! \brief Defines the DCL_DF11 shadow parameter set
//!
typedef struct dcl_df11_sps {
float32_t b0; //!< b0
float32_t b1; //!< b1
float32_t a1; //!< a1
} DCL_DF11_SPS;
#define DF11_SPS_DEFAULTS { 0.5f, 0.5f, 1.0f }
//! \brief Defines the DCL_DF11 controller structure
//!
typedef struct dcl_df11 {
float32_t b0; //!< b0
float32_t b1; //!< b1
float32_t a1; //!< a1
float32_t d1; //!< e(k-1)
float32_t d2; //!< u(k-1)
DCL_DF11_SPS *sps; //!< Pointer to the shadow parameter set
DCL_CSS *css; //!< Pointer to the common support structure
} DCL_DF11;
//! \brief Defines default values to initialize the DCL_DF11 structure
//!
#define DF11_DEFAULTS { 0.5f, 0.5f, 1.0f, 0.0f, 0.0f, \
NULL_ADDR, NULL_ADDR }
//! \brief Resets the DCL_DF11 controller
//! \param[in] p Pointer to the DCL_DF11 structure
//! \return None
//!
static inline void DCL_resetDF11(DCL_DF11 *p)
{
uint16_t v;
v = DCL_DISABLE_INTS;
p->d1 = p->d2 = 0.0f;
DCL_RESTORE_INTS(v);
}
//! \brief Updates active coefficients in the DF11 structure
//! \param[in] p Pointer to the DCL_DF11 controller structure
//! \return None
//!
static inline void DCL_updateDF11(DCL_DF11 *p)
{
uint16_t v;
if (p->css->sts & STS_UPDATE_PENDING)
{
v = DCL_DISABLE_INTS;
p->b0 = p->sps->b0;
p->b1 = p->sps->b1;
p->a1 = p->sps->a1;
DCL_RESTORE_INTS(v);
DCL_CLEAR_UPDATE_REQUEST(p);
}
}
//! \brief Loads DF11 tuning parameters
//! Implemented as assembly module
//! \param[in] p Pointer to the active DCL_DF11 controller structure
//! \return None
//!
extern void DCL_fupdateDF11(DCL_DF11 *p);
//! \brief Determines stability of the shadow DF11 compensator
//! \param[in] p Pointer to the DCL_DF11 controller structure
//! \return 'true' if the pole has magnitude less than 1, 'false' otherwise
//!
static inline bool DCL_isStableDF11(DCL_DF11 *p)
{
return(DCL_isStablePn1(p->sps->a1));
}
//! \brief Loads the DF11 shadow coefficients from a ZPK3 description
//! Note: new settings take effect after DCL_updateDF11()
//! Only real z1 & p1 considered: all other roots ignored
//! \param[in] p Pointer to the DCL_DF11 controller structure
//! \param[in] q Pointer to the ZPK3 structure
//! \return None
//!
static inline void DCL_loadDF11asZPK(DCL_DF11 *p, DCL_ZPK3 *q)
{
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= !(F32_IS_VALUE(cimagf(q->z1), 0.0f)) ? ERR_PARAM_WARN : ERR_NONE;
p->css->err |= !(F32_IS_VALUE(cimagf(q->p1), 0.0f)) ? ERR_PARAM_WARN : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
float32_t a0p = 2.0f - (float32_t) crealf(q->p1) * p->css->T;
p->sps->b0 = q->K * (2.0f - (float32_t) crealf(q->z1) * p->css->T) / a0p;
p->sps->b1 = q->K * (-2.0f - (float32_t) crealf(q->z1) * p->css->T) / a0p;
p->sps->a1 = (-2.0f - (float32_t) crealf(q->p1) * p->css->T) / a0p;
}
//! \brief Loads compensator coefficients to emulate series form PI
//! Note: new settings take effect after DCL_updateDF11()
//! \param[in] p Pointer to the DCL_DF11 controller structure
//! \param[in] Kp Proportional gain
//! \param[in] Ki Integral gain
//! \param[in] T Sampling period in seconds
//! \return None
//!
static inline void DCL_loadDF11asPI(DCL_DF11 *p, float32_t Kp, float32_t Ki)
{
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= (Kp < 0.0f) ? ERR_PARAM_INVALID : ERR_NONE;
p->css->err |= (Ki < 0.0f) ? ERR_PARAM_INVALID : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
p->sps->b0 = Kp * (Ki * p->css->T + 2.0f) / 2.0f;
p->sps->b1 = Kp * (Ki * p->css->T - 2.0f) / 2.0f;
p->sps->a1 = -1.0f;
}
//! \brief Executes a 1st order Direct Form 1 controller on the FPU32
//! Implemented as an external assembly module
//! \param[in] p Pointer to the DCL_DF11 controller structure
//! \param[in] ek The servo error
//! \return The control effort
//!
extern float32_t DCL_runDF11_C1(DCL_DF11 *p, float32_t ek);
//! \brief Executes a 1st order Direct Form 1 controller on the FPU32
//! Implemented as inline C function
//! \param[in] p Pointer to the DCL_DF11 controller structure
//! \param[in] ek The servo error
//! \return The control effort
//!
static inline float32_t DCL_runDF11_C2(DCL_DF11 *p, float32_t ek)
{
p->d2 = (ek * p->b0) + (p->d1 * p->b1) - (p->d2 * p->a1);
p->d1 = ek;
return(p->d2);
}
//--- Direct Form 1 - 3rd order ----------------------------------------------
#pragma CODE_SECTION(DCL_updateDF13,"dclfuncs")
#pragma CODE_SECTION(DCL_runDF13_C4,"dclfuncs")
#pragma CODE_SECTION(DCL_runDF13_C5,"dclfuncs")
#pragma CODE_SECTION(DCL_runDF13_C6,"dclfuncs")
//! \brief Defines the DCL_DF13 shadow parameter set
//!
typedef struct dcl_df13_sps {
float32_t b0; //!< b0
float32_t b1; //!< b1
float32_t b2; //!< b2
float32_t b3; //!< b3
float32_t a0; //!< a0
float32_t a1; //!< a1
float32_t a2; //!< a2
float32_t a3; //!< a3
} DCL_DF13_SPS;
#define DF13_SPS_DEFAULTS { 0.25f, 0.25f, 0.25f, 0.25f, 1.0f, 0.0f, 0.0f, 0.0f }
//! \brief Defines the DCL_DF13 controller structure
//!
typedef struct dcl_df13 {
// coefficients
float32_t b0; //!< b0
float32_t b1; //!< b1
float32_t b2; //!< b2
float32_t b3; //!< b3
float32_t a0; //!< a0
float32_t a1; //!< a1
float32_t a2; //!< a2
float32_t a3; //!< a3
//data
float32_t d0; //!< e(k)
float32_t d1; //!< e(k-1)
float32_t d2; //!< e(k-2)
float32_t d3; //!< e(k-3)
float32_t d4; //!< u(k)
float32_t d5; //!< u(k-1)
float32_t d6; //!< u(k-2)
float32_t d7; //!< u(k-3)
DCL_DF13_SPS *sps; //!< Pointer to the shadow parameter set
DCL_CSS *css; //!< Pointer to the common support structure
} DCL_DF13;
//! \brief Defines default values to initialize the DCL_DF13 structure
//!
#define DF13_DEFAULTS { 0.25f, 0.25f, 0.25f, 0.25f, 1.0f, 0.0f, 0.0f, 0.0f, \
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, \
NULL_ADDR, NULL_ADDR }
//! \brief Resets internal controller data to zero
//! Implemented as inline C function
//! \param[in] p Pointer to the DCL_DF13 controller structure
//! \return None
//!
static inline void DCL_resetDF13(DCL_DF13 *p)
{
uint16_t v;
v = DCL_DISABLE_INTS;
p->d0 = p->d1 = p->d2 = p->d3 = p->d4 = p->d5 = p->d6 = p->d7 = 0.0f;
DCL_RESTORE_INTS(v);
}
//! \brief Copies coefficients from one DF13 structure to another
//! \param[in] p Pointer to the DCL_DF13 controller structure
//! \return None
//!
static inline void DCL_updateDF13(DCL_DF13 *p)
{
uint16_t v;
if (p->css->sts & STS_UPDATE_PENDING)
{
v = DCL_DISABLE_INTS;
p->b0 = p->sps->b0;
p->b1 = p->sps->b1;
p->b2 = p->sps->b2;
p->b3 = p->sps->b3;
p->a0 = p->sps->a0;
p->a1 = p->sps->a1;
p->a2 = p->sps->a2;
p->a3 = p->sps->a3;
DCL_RESTORE_INTS(v);
DCL_CLEAR_UPDATE_REQUEST(p);
}
}
//! \brief Loads DF13 tuning parameters
//! Implemented as assembly module
//! \param[in] p Pointer to the active DCL_DF13 controller structure
//! \return None
//!
extern void DCL_fupdateDF13(DCL_DF13 *p);
//! \brief Determines stability of the shadow compensator
//! \param[in] p Pointer to the DCL_DF13 controller structure
//! \return 'true' if all poles have magnitude less than 1, 'false' otherwise
//!
static inline bool DCL_isStableDF13(DCL_DF13 *p)
{
return(DCL_isStablePn3(1.0f, p->sps->a1, p->sps->a2, p->sps->a3));
}
//! \brief Loads the DF13 shadow coefficients from a ZPK3 description
//! Note: new settings take effect after DCL_updateDF13()
//! \param[in] p Pointer to the DCL_DF13 controller structure
//! \param[in] q Pointer to the DCL_ZPK3 structure
//! \return None
//!
static inline void DCL_loadDF13asZPK(DCL_DF13 *p, DCL_ZPK3 *q)
{
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= (!F32_IS_VALUE((cimagf(q->z1) + cimagf(q->z2) + cimagf(q->z3)), 0.0f)) ? ERR_PARAM_INVALID : ERR_NONE;
p->css->err |= (!F32_IS_VALUE((cimagf(q->p1) + cimagf(q->p2) + cimagf(q->p3)), 0.0f)) ? ERR_PARAM_INVALID : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
float32_t beta2 = -(float32_t) crealf(q->z1 + q->z2 + q->z3);
float32_t beta1 = (float32_t) crealf((q->z1 * q->z2) + (q->z2 * q->z3) + (q->z1 * q->z3));
float32_t beta0 = -(float32_t) crealf(q->z1 * q->z2 * q->z3);
float32_t alpha2 = -(float32_t) crealf(q->p1 + q->p2 + q->p3);
float32_t alpha1 = (float32_t) crealf((q->p1 * q->p2) + (q->p2 * q->p3) + (q->p1 * q->p3));
float32_t alpha0 = -(float32_t) crealf(q->p1 * q->p2 * q->p3);
float32_t T = p->css->T;
float32_t a0p = 8.0f + (alpha2 * 4.0f * T) + (alpha1 * 2.0f * T * T) + (alpha0 * T * T * T);
p->sps->b0 = q->K * (8.0f + (beta2 * 4.0f * T) + (beta1 * 2.0f * T * T) + (beta0 * T * T * T)) / a0p;
p->sps->b1 = q->K * (-24.0f - (beta2 * 4.0f * T) + (beta1 * 2.0f * T * T) + (3.0f * beta0 * T * T * T)) / a0p;
p->sps->b2 = q->K * (24.0f - (beta2 * 4.0f * T) - (beta1 * 2.0f * T * T) + (3.0f * beta0 * T * T * T)) / a0p;
p->sps->b3 = q->K * (-8.0f + (beta2 * 4.0f * T) - (beta1 * 2.0f * T * T) + (beta0 * T * T * T)) / a0p;
p->sps->a0 = 1.0f;
p->sps->a1 = (-24.0f - (alpha2 * 4.0f * T) + (alpha1 * 2.0f * T * T) + (3.0f * alpha0 * T * T * T)) / a0p;
p->sps->a2 = (24.0f - (alpha2 * 4.0f * T) - (alpha1 * 2.0f * T * T) + (3.0f * alpha0 * T * T * T)) / a0p;
p->sps->a3 = (-8.0f + (alpha2 * 4.0f * T) - (alpha1 * 2.0f * T * T) + (alpha0 * T * T * T)) / a0p;
}
//! \brief Executes a full 3rd order Direct Form 1 controller on the FPU32
//! Implemented as an external assembly module
//! \param[in] p Pointer to the DCL_DF13 controller structure
//! \param[in] ek The servo error
//! \return The control effort
//!
extern float32_t DCL_runDF13_C1(DCL_DF13 *p, float32_t ek);
//! \brief Executes an immediate 3rd order Direct Form 1 controller on the FPU32
//! Implemented as an external assembly module
//! \param[in] p Pointer to the DCL_DF13 controller structure
//! \param[in] ek The servo error
//! \param[in] vk The partial pre-computed control effort
//! \return The control effort
//!
extern float32_t DCL_runDF13_C2(DCL_DF13 *p, float32_t ek, float32_t vk);
//! \brief Executes a partial pre-computed 3rd order Direct Form 1 controller on the FPU32
//! Implemented as an external assembly module
//! \param[in] p Pointer to the DCL_DF13 controller structure
//! \param[in] ek The servo error
//! \param[in] uk The controller output in the previous sample interval
//! \return The control effort
//!
extern float32_t DCL_runDF13_C3(DCL_DF13 *p, float32_t ek, float32_t uk);
//! \brief Executes a full 3rd order Direct Form 1 controller on the FPU32
//! Implemented as inline C function
//! Note: d0 not used
//! \param[in] p Pointer to the DCL_DF13 controller structure
//! \param[in] ek The servo error
//! \return The control effort
//!
static inline float32_t DCL_runDF13_C4(DCL_DF13 *p, float32_t ek)
{
p->d4 = (ek * p->b0) + (p->d1 * p->b1) + (p->d2 * p->b2) + (p->d3 * p->b3) - (p->d5 * p->a1) - (p->d6 * p->a2) - (p->d7 * p->a3);
p->d3 = p->d2;
p->d2 = p->d1;
p->d1 = ek;
p->d7 = p->d6;
p->d6 = p->d5;
p->d5 = p->d4;
return(p->d4);
}
//! \brief Executes an immediate 3rd order Direct Form 1 controller on the FPU32
//! Implemented as inline C function
//! \param[in] p Pointer to the DCL_DF13 controller structure
//! \param[in] ek The servo error
//! \param[in] vk The partial pre-computed control effort
//! \return The control effort
//!
static inline float32_t DCL_runDF13_C5(DCL_DF13 *p, float32_t ek, float32_t vk)
{
p->d4 = (ek * p->b0) + vk;
return(p->d4);
}
//! \brief Executes a partial pre-computed 3rd order Direct Form 1 controller on the FPU32
//! Implemented as inline C function
//! Note: d0 not used
//! \param[in] p Pointer to the DCL_DF13 controller structure
//! \param[in] ek The servo error
//! \param[in] uk The controller output in the previous sample interval
//! \return The control effort
//!
static inline 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);
}
//--- Direct Form 2 - 2nd order ----------------------------------------------
#pragma CODE_SECTION(DCL_updateDF22,"dclfuncs")
#pragma CODE_SECTION(DCL_runDF22_C4,"dclfuncs")
#pragma CODE_SECTION(DCL_runDF22_C5,"dclfuncs")
#pragma CODE_SECTION(DCL_runDF22_C6,"dclfuncs")
//! \brief Defines the DCL_DF22 shadow parameter set
//!
typedef struct dcl_df22_sps {
float32_t b0; //!< b0
float32_t b1; //!< b1
float32_t b2; //!< b2
float32_t a1; //!< a1
float32_t a2; //!< a2
} DCL_DF22_SPS;
#define DF22_SPS_DEFAULTS { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f }
//! \brief Defines the DCL_DF22 controller structure
//!
typedef struct dcl_df22 {
float32_t b0; //!< b0
float32_t b1; //!< b1
float32_t b2; //!< b2
float32_t a1; //!< a1
float32_t a2; //!< a2
float32_t x1; //!< x1
float32_t x2; //!< x2
DCL_DF22_SPS *sps; //!< Pointer to the shadow parameter set
DCL_CSS *css; //!< Pointer to the common support structure
} DCL_DF22;
//! \brief Defines default values to initialize the DCL_DF22 structure
//!
#define DF22_DEFAULTS { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, \
NULL_ADDR, NULL_ADDR }
//! \brief Resets internal DF22 controller data to zero
//! \param[in] p Pointer to the DCL_DF22 controller structure
//! \return None
//!
static inline void DCL_resetDF22(DCL_DF22 *p)
{
uint16_t v;
v = DCL_DISABLE_INTS;
p->x1 = p->x2 = 0.0f;
DCL_RESTORE_INTS(v);
}
//! \brief Updates active coefficients from shadow set
//! \param[in] p Pointer to the DCL_DF22 controller structure
//! \return None
//!
static inline void DCL_updateDF22(DCL_DF22 *p)
{
uint16_t v;
if (p->css->sts & STS_UPDATE_PENDING)
{
v = DCL_DISABLE_INTS;
p->b0 = p->sps->b0;
p->b1 = p->sps->b1;
p->b2 = p->sps->b2;
p->a1 = p->sps->a1;
p->a2 = p->sps->a2;
DCL_RESTORE_INTS(v);
DCL_CLEAR_UPDATE_REQUEST(p);
}
}
//! \brief Loads DF22 tuning parameters
//! Implemented as assembly module
//! \param[in] p Pointer to the active DCL_DF22 controller structure
//! \return None
//!
extern void DCL_fupdateDF22(DCL_DF22 *p);
//! \brief Determines stability of the shadow compensator
//! \param[in] p Pointer to the DCL_DF22 controller structure
//! \return 'true' if both poles have magnitude less than 1, 'false' otherwise
//!
static inline bool DCL_isStableDF22(DCL_DF22 *p)
{
return(DCL_isStablePn2(1.0f, p->sps->a1, p->sps->a2));
}
//! \brief Loads the DF22 shadow coefficients from a ZPK3 description
//! Note: new settings take effect after DCL_updateDF22()
//! Only z1, z2, p1 & p2 considered: z3 & p3 ignored
//! \param[in] p Pointer to the DCL_DF22 controller structure
//! \param[in] q Pointer to the DCL_ZPK3 structure
//! \return None
//!
static inline void DCL_loadDF22asZPK(DCL_DF22 *p, DCL_ZPK3 *q)
{
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= (!F32_IS_VALUE((cimagf(q->z1) + cimagf(q->z2)), 0.0f)) ? ERR_PARAM_INVALID : ERR_NONE;
p->css->err |= (!F32_IS_VALUE((cimagf(q->p1) + cimagf(q->p2)), 0.0f)) ? ERR_PARAM_INVALID : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
float32_t beta1 = -(float32_t) crealf(q->z1 + q->z2);
float32_t beta0 = (float32_t) crealf(q->z1 * q->z2);
float32_t alpha1 = -(float32_t) crealf(q->p1 + q->p2);
float32_t alpha0 = (float32_t) crealf(q->p1 * q->p2);
float32_t T = p->css->T;
float32_t a0p = 4.0f + (alpha1 * 2.0f * T) + (alpha0 * T * T);
p->sps->b0 = q->K * (4.0f + (beta1 * 2.0f * T) + (beta0 * T * T)) / a0p;
p->sps->b1 = q->K * (-8.0f + (2.0f * beta0 * T * T)) / a0p;
p->sps->b2 = q->K * (4.0f - (beta1 * 2.0f * T) + (beta0 * T * T)) / a0p;
p->sps->a1 = (-8.0f + (2.0f * alpha0 * T * T)) / a0p;
p->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: new settings take effect after DCL_updateDF22()
//! \param[in] p Pointer to the DCL_DF22 controller structure
//! \param[in] z The damping ratio
//! \param[in] wn The un-damped natural frequency in rad/s
//! \return None
//!
static inline void DCL_loadDF22asZwn(DCL_DF22 *p, float32_t z, float32_t wn)
{
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= (z < 0.0f) ? ERR_PARAM_INVALID : ERR_NONE;
p->css->err |= (wn < 0.0f) ? ERR_PARAM_INVALID : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
float32_t T = p->css->T;
float32_t v1 = wn*wn*T*T;
float32_t a2p = 1.0f / (4.0f + 4.0f*z*wn*T + v1);
p->sps->b0 = v1 * a2p;
p->sps->b1 = 2.0f * p->sps->b0;
p->sps->b2 = p->sps->b0;
p->sps->a1 = (2.0f*v1 - 8.0f) * a2p;
p->sps->a2 = (4.0f - 4.0f*z*wn*T + v1) * a2p;
}
//! \brief Loads the shadow DF22 compensator coefficients to emulate a parallel form PID
//! Note: new settings take effect after DCL_updateDF22()
//! \param[in] p 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
//! \return None
//!
static inline void DCL_loadDF22asParallelPID(DCL_DF22 *p, float32_t Kp, float32_t Ki, float32_t Kd, float32_t fc)
{
float32_t Kdp, tau, c1, c2;
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= (Kp < 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
p->css->err |= (Ki < 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
p->css->err |= (Kd < 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
p->css->err |= (fc < 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
p->css->err |= (fc > (1.0f / (2.0f * p->css->T))) ? ERR_PARAM_WARN : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
tau = 1.0f / (2.0f * CONST_PI_32 * fc);
c1 = 2.0f / (p->css->T + 2.0f*tau);
c2 = c1 * (p->css->T - 2.0f*tau) / 2.0f;
Kdp = Kd * c1;
p->sps->b0 = Kp + Ki + Kdp;
p->sps->b1 = Kp*(c2 - 1) + Ki*c2 - 2.0f*Kdp;
p->sps->b2 = -c2*Kp + Kdp;
p->sps->a1 = c2 - 1;
p->sps->a2 = -c2;
}
//! \brief Loads the shadow DF22 compensator coefficients to emulate a series form PID
//! Note: new settings take effect after DCL_updateDF22()
//! \param[in] p 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
//! \return None
//!
static inline void DCL_loadDF22asSeriesPID(DCL_DF22 *p, float32_t Kp, float32_t Ki, float32_t Kd, float32_t fc)
{
float32_t Kdp, tau, c1, c2;
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= (Kp < 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
p->css->err |= (Ki < 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
p->css->err |= (Kd < 0.0f) ? ERR_PARAM_RANGE : ERR_NONE;
p->css->err |= ((fc < 0.0f) || (fc > (1.0f / (2.0f * p->css->T)))) ? ERR_PARAM_RANGE : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
tau = 1 / (2.0f * CONST_PI_32 * fc);
c1 = 2.0f / (p->css->T + 2.0f*tau);
c2 = c1 * (p->css->T - 2.0f*tau) / 2.0f;
Kdp = Kd * c1;
p->sps->b0 = Kp * (1 + Ki + Kdp);
p->sps->b1 = Kp * (c2 - 1 + Ki*c2 - 2*Kdp);
p->sps->b2 = Kp * (-c2 + Kdp);
p->sps->a1 = c2 - 1;
p->sps->a2 = -c2;
}
//! \brief Executes a full 2nd order Direct Form 2 controller on the FPU32
//! Implemented as an external assembly module
//! \param[in] p Pointer to the DCL_DF22 controller structure
//! \param[in] ek The servo error
//! \return The control effort
//!
extern float32_t DCL_runDF22_C1(DCL_DF22 *p, float32_t ek);
//! \brief Executes an immediate 2nd order Direct Form 2 controller on the FPU32
//! Implemented as an external assembly module
//! \param[in] p Pointer to the DCL_DF22 controller structure
//! \param[in] ek The servo error
//! \return The control effort
//!
extern float32_t DCL_runDF22_C2(DCL_DF22 *p, float32_t ek);
//! \brief Executes a partial pre-computed 2nd order Direct Form 2 controller on the FPU32
//! Implemented as an external assembly module
//! \param[in] p Pointer to the DCL_DF22 controller structure
//! \param[in] ek The servo error
//! \param[in] uk The controller output in the previous sample interval
//!
extern void DCL_runDF22_C3(DCL_DF22 *p, float32_t ek, float32_t uk);
//! \brief Executes a full 2nd order Direct Form 2 controller on the FPU32
//! Implemented as inline C function
//! \param[in] p Pointer to the DCL_DF22 controller structure
//! \param[in] ek The servo error
//! \return The control effort
//!
static inline float32_t DCL_runDF22_C4(DCL_DF22 *p, float32_t ek)
{
float32_t v7;
v7 = (ek * p->b0) + p->x1;
p->x1 = (ek * p->b1) + p->x2 - (v7 * p->a1);
p->x2 = (ek * p->b2) - (v7 * p->a2);
return(v7);
}
//! \brief Executes an immediate 2nd order Direct Form 2 controller on the FPU32
//! Implemented as inline C function
//! \param[in] p Pointer to the DCL_DF22 controller structure
//! \param[in] ek The servo error
//! \return The control effort
//!
static inline float32_t DCL_runDF22_C5(DCL_DF22 *p, float32_t ek)
{
return((ek * p->b0) + p->x1);
}
//! \brief Executes a partial pre-computed 2nd order Direct Form 2 controller on the FPU32
//! Implemented as inline C function
//! \param[in] p Pointer to the DCL_DF22 controller structure
//! \param[in] ek The servo error
//! \param[in] uk The controller output in the previous sample interval
//!
static inline void DCL_runDF22_C6(DCL_DF22 *p, float32_t ek, float32_t uk)
{
p->x1 = (ek * p->b1) + p->x2 - (uk * p->a1);
p->x2 = (ek * p->b2) - (uk * p->a2);
}
//--- Direct Form 2 - 3rd order ----------------------------------------------
#pragma CODE_SECTION(DCL_updateDF23,"dclfuncs")
#pragma CODE_SECTION(DCL_runDF23_C4,"dclfuncs")
#pragma CODE_SECTION(DCL_runDF23_C5,"dclfuncs")
#pragma CODE_SECTION(DCL_runDF23_C6,"dclfuncs")
//! \brief Defines the DCL_DF23 shadow parameter set
//!
typedef struct dcl_df23_sps {
float32_t b0; //!< b0
float32_t b1; //!< b1
float32_t b2; //!< b2
float32_t b3; //!< b3
float32_t a1; //!< a1
float32_t a2; //!< a2
float32_t a3; //!< a3
} DCL_DF23_SPS;
#define DF23_SPS_DEFAULTS { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }
//! \brief Defines the DCL_DF23 controller structure
//!
typedef struct dcl_df23 {
// coefficients
float32_t b0; //!< b0
float32_t b1; //!< b1
float32_t b2; //!< b2
float32_t b3; //!< b3
float32_t a1; //!< a1
float32_t a2; //!< a2
float32_t a3; //!< a3
// data
float32_t x1; //!< x1
float32_t x2; //!< x2
float32_t x3; //!< x3
DCL_DF23_SPS *sps; //!< Pointer to the shadow parameter set
DCL_CSS *css; //!< Pointer to the common support structure
} DCL_DF23;
//! \brief Defines default values to initialize the DCL_DF23 structure
//!
#define DF23_DEFAULTS { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, \
0.0f, 0.0f, 0.0f, NULL_ADDR, NULL_ADDR }
//! \brief Resets internal DF23 controller data to zero
//! \param[in] p Pointer to the DCL_DF23 controller structure
//! \return None
//!
static inline void DCL_resetDF23(DCL_DF23 *p)
{
uint16_t v;
v = DCL_DISABLE_INTS;
p->x1 = p->x2 = p->x3 = 0.0f;
DCL_RESTORE_INTS(v);
}
//! \brief Copies coefficients from one DF23 structure to another
//! \param[in] p Pointer to the DCL_DF23 controller structure
//! \return None
//!
static inline void DCL_updateDF23(DCL_DF23 *p)
{
uint16_t v;
if (p->css->sts & STS_UPDATE_PENDING)
{
v = DCL_DISABLE_INTS;
p->b0 = p->sps->b0;
p->b1 = p->sps->b1;
p->b2 = p->sps->b2;
p->b3 = p->sps->b3;
p->a1 = p->sps->a1;
p->a2 = p->sps->a2;
p->a3 = p->sps->a3;
DCL_RESTORE_INTS(v);
DCL_CLEAR_UPDATE_REQUEST(p);
}
}
//! \brief Loads DF23 tuning parameters
//! Implemented as assembly module
//! \param[in] p Pointer to the active DCL_DF23 controller structure
//! \return None
//!
extern void DCL_fupdateDF23(DCL_DF23 *p);
//! \brief Determines stability of the shadow compensator
//! \param[in] p Pointer to the DCL_DF23 controller structure
//! \return 'true' if all poles have magnitude less than 1, 'false' otherwise
//!
static inline bool DCL_isStableDF23(DCL_DF23 *p)
{
return(DCL_isStablePn3(1.0f, p->sps->a1, p->sps->a2, p->sps->a3));
}
//! \brief Loads the DF23 shadow coefficients from a ZPK3 description
//! Note: new settings take effect after DCL_updateDF23()
//! \param[in] p Pointer to the DCL_DF23 controller structure
//! \param[in] q Pointer to the DCL_ZPK3 structure
//! \return None
//!
static inline void DCL_loadDF23asZPK(DCL_DF23 *p, DCL_ZPK3 *q)
{
#ifdef DCL_ERROR_HANDLING_ENABLED
p->css->err |= (!F32_IS_VALUE((cimagf(q->z1) + cimagf(q->z2) + cimagf(q->z3)), 0.0f)) ? ERR_PARAM_INVALID : ERR_NONE;
p->css->err |= (!F32_IS_VALUE((cimagf(q->p1) + cimagf(q->p2) + cimagf(q->p3)), 0.0f)) ? ERR_PARAM_INVALID : ERR_NONE;
if (p->css->err)
{
DCL_GET_ERROR_LOC(p->css);
DCL_RUN_ERROR_HANDLER(p->css);
}
#endif
float32_t beta2 = -(float32_t) crealf(q->z1 + q->z2 + q->z3);
float32_t beta1 = (float32_t) crealf((q->z1 * q->z2) + (q->z2 * q->z3) + (q->z1 * q->z3));
float32_t beta0 = -(float32_t) crealf(q->z1 * q->z2 * q->z3);
float32_t alpha2 = -(float32_t) crealf(q->p1 + q->p2 + q->p3);
float32_t alpha1 = (float32_t) crealf((q->p1 * q->p2) + (q->p2 * q->p3) + (q->p1 * q->p3));
float32_t alpha0 = -(float32_t) crealf(q->p1 * q->p2 * q->p3);
float32_t T = p->css->T;
float32_t a0p = 8.0f + (alpha2 * 4.0f * T) + (alpha1 * 2.0f * T * T) + (alpha0 * T * T * T);
p->sps->b0 = q->K * (8.0f + (beta2 * 4.0f * T) + (beta1 * 2.0f * T * T) + (beta0 * T * T * T)) / a0p;
p->sps->b1 = q->K * (-24.0f - (beta2 * 4.0f * T) + (beta1 * 2.0f * T * T) + (3.0f * beta0 * T * T * T)) / a0p;
p->sps->b2 = q->K * (24.0f - (beta2 * 4.0f * T) - (beta1 * 2.0f * T * T) + (3.0f * beta0 * T * T * T)) / a0p;
p->sps->b3 = q->K * (-8.0f + (beta2 * 4.0f * T) - (beta1 * 2.0f * T * T) + (beta0 * T * T * T)) / a0p;
p->sps->a1 = (-24.0f - (alpha2 * 4.0f * T) + (alpha1 * 2.0f * T * T) + (3.0f * alpha0 * T * T * T)) / a0p;
p->sps->a2 = (24.0f - (alpha2 * 4.0f * T) - (alpha1 * 2.0f * T * T) + (3.0f * alpha0 * T * T * T)) / a0p;
p->sps->a3 = (-8.0f + (alpha2 * 4.0f * T) - (alpha1 * 2.0f * T * T) + (alpha0 * T * T * T)) / a0p;
}
//! \brief Executes a full 3rd order Direct Form 2 controller on the FPU32
//! Implemented as an external assembly module
//! \param[in] p Pointer to the DCL_DF23 controller structure
//! \param[in] ek The servo error
//! \return The control effort
//!
extern float32_t DCL_runDF23_C1(DCL_DF23 *p, float32_t ek);
//! \brief Executes an immediate 3rd order Direct Form 2 controller on the FPU32
//! Implemented as an external assembly module
//! \param[in] p Pointer to the DCL_DF23 controller structure
//! \param[in] ek The servo error
//! \return The control effort
//!
extern float32_t DCL_runDF23_C2(DCL_DF23 *p, float32_t ek);
//! \brief Executes a partial pre-computed 3rd order Direct Form 2 controller on the FPU32
//! Implemented as an external assembly module
//! \param[in] p Pointer to the DCL_DF23 controller structure
//! \param[in] ek The servo error
//! \param[in] uk The controller output in the previous sample interval
//!
extern void DCL_runDF23_C3(DCL_DF23 *p, float32_t ek, float32_t uk);
//! \brief Executes a full 3rd order Direct Form 2 controller on the FPU32
//! Implemented as inline C function
//! \param[in] p Pointer to the DCL_DF23 controller structure
//! \param[in] ek The servo error
//! \return The control effort
//!
static inline float32_t DCL_runDF23_C4(DCL_DF23 *p, float32_t ek)
{
float32_t v7;
v7 = (ek * p->b0) + p->x1;
p->x1 = (ek * p->b1) + p->x2 - (v7 * p->a1);
p->x2 = (ek * p->b2) + p->x3 - (v7 * p->a2);
p->x3 = (ek * p->b3) - (v7 * p->a3);
return(v7);
}
//! \brief Executes an immediate 3rd order Direct Form 2 controller on the FPU32
//! Implemented as inline C function
//! \param[in] p Pointer to the DCL_DF23 controller structure
//! \param[in] ek The servo error
//! \return The control effort
//!
static inline float32_t DCL_runDF23_C5(DCL_DF23 *p, float32_t ek)
{
return((ek * p->b0) + p->x1);
}
//! \brief Executes a partial pre-computed 3rd order Direct Form 2 controller on the FPU32
//! Implemented as inline C function
//! \param[in] p Pointer to the DCL_DF23 controller structure
//! \param[in] ek The servo error
//! \param[in] uk The controller output in the previous sample interval
//!
static inline void DCL_runDF23_C6(DCL_DF23 *p, float32_t ek, float32_t uk)
{
p->x1 = (ek * p->b1) + p->x2 - (uk * p->a1);
p->x2 = (ek * p->b2) + p->x3 - (uk * p->a2);
p->x3 = (ek * p->b3) - (uk * p->a3);
}
//--- Direct Form 2 - clamp --------------------------------------------------
//! \brief Saturates a control variable and returns 1 if either limit is exceeded.
//! Implemented as an external assembly module.
//!
//! \details Can be used to saturate a pre-computed Direct Form 2 controller.
//! If the immediate result is in range it can be used, otherwise
//! it can be clamped and the next partial pre-computation skipped.
//! An example of use with a pre-computed DF22 controller follows:
//!
//! \code
//! uk = DCL_runDF22_C2(&arma2, rk); // immediate result from pre-computed controller
//! i = DCL_runClamp_C1(&uk, 1.0f, -1.0f); // clamp immediate result to +/-1.0
//! // ...use uk here...
//! if (0 == i) // if immediate result is in range...
//! {
//! DCL_runDF22_C3(&arma2, rk, uk); // ...pre-compute the next partial result
//! }
//! \endcode
//!
//! \param[in] data The address of the data variable
//! \param[in] Umax The upper limit
//! \param[in] Umin The lower limit
//! \return Returns 0 if (Umin < data < Umax), else 1
//!
extern int16_t DCL_runClamp_C1(float32_t *data, float32_t Umax, float32_t Umin);
//! \brief Saturates a control variable and returns 1 if either limit is exceeded
//! \param[in] data The address of the data variable
//! \param[in] Umax The upper limit
//! \param[in] Umin The lower limit
//! \return Returns 0 if (Umin < data < Umax), else 1
//!
static inline int16_t DCL_runClamp_C2(float32_t *data, float32_t Umax, float32_t Umin)
{
float32_t iv = *(data);
*(data) = (*(data) > Umax) ? Umax : *(data);
*(data) = (*(data) < Umin) ? Umin : *(data);
return(((iv < Umax) && (iv > Umin)) ? 0 : 1);
}
//--- Gain Scheduler Module --------------------------------------------------
#pragma CODE_SECTION(DCL_updateGSM,"dclfuncs")
#pragma CODE_SECTION(DCL_runGSM_C1,"dclfuncs")
//! \brief Number of piecewise linear sections
//!
#define GSM_N 8
//! \brief Defines the shadow DCL_GSM structure
//!
typedef struct dcl_gsm_sps {
float32_t m[GSM_N]; //!< sector gain array
float32_t c[GSM_N+1]; //!< sector offset array
} DCL_GSM_SPS;
#define GSM_SPS_DEFAULTS { { 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f }, \
{ 0.0f, 0.125f, 0.25f, 0.375f, 0.5f, 0.625f, 0.75f, 0.875f, 1.0f } }
//! \brief Defines the DCL_GSM structure
//! Consists of gains and offsets for each linear section
//!
typedef struct dcl_gsm {
float32_t m[GSM_N]; //!< sector gain array
float32_t c[GSM_N+1]; //!< sector offset array
float32_t h; //!< interval size
DCL_GSM_SPS *sps; //!< Pointer to the shadow parameter set
DCL_CSS *css; //!< Pointer to the common support structure
} DCL_GSM;
//! \brief Defines default values for the GSM structure (GSM_N=8)
//!
#define GSM_DEFAULTS { { 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f }, \
{ 0.0f, 0.125f, 0.25f, 0.375f, 0.5f, 0.625f, 0.75f, 0.875f, 1.0f }, \
1.0f/GSM_N, NULL_ADDR, NULL_ADDR }
//! \brief Resets the active GSM parameters to unity gain
//! SPS is unaffected
//! \param[in] p Pointer to the active DCL_GSM controller structure
//! \return None
//!
static inline void DCL_resetGSM(DCL_GSM *p)
{
uint16_t j, v;
v = DCL_DISABLE_INTS;
for (j=0; j<GSM_N; j++)
{
p->m[j] = 1.0f;
p->c[j] = (float32_t) j * p->h;
}
p->c[GSM_N] = 1.0f;
DCL_RESTORE_INTS(v);
}
//! \brief Loads the active GSM parameters
//! \param[in] p Pointer to the active DCL_GSM controller structure
//! \return None
//!
static inline void DCL_updateGSM(DCL_GSM *p)
{
uint16_t j, v;
if (p->css->sts & STS_UPDATE_PENDING)
{
v = DCL_DISABLE_INTS;
for (j=0; j<GSM_N; j++)
{
p->m[j] = p->sps->m[j];
p->c[j] = p->sps->c[j];
}
p->c[GSM_N] = p->sps->c[GSM_N];
DCL_RESTORE_INTS(v);
DCL_CLEAR_UPDATE_REQUEST(p);
}
}
//! \brief Loads GSM tuning parameters
//! Implemented as assembly module
//! \param[in] p Pointer to the active DCL_GSM controller structure
//! \return None
//!
extern void DCL_fupdateGSM(DCL_GSM *p);
//! \brief Computes the shadow scheduler gains from the offset selection
//! Active gains are unaffected until DCL_updateGSM() is called
//! \param[in] p Pointer to DCL_GSM structure
//! \return None
//!
static inline void DCL_loadGSMgains(DCL_GSM *p)
{
int16_t j;
for (j=0; j<GSM_N; j++)
{
p->sps->m[j] = (p->sps->c[j+1] - p->sps->c[j]) * (float32_t) GSM_N;
}
}
//! \brief Computes the shadow scheduler offsets from the gain selection
//! Active offsets are unaffected until DCL_updateGSM() is called
//! \param[in] p Pointer to DCL_GSM structure
//! \return None
//!
static inline void DCL_loadGSMoffsets(DCL_GSM *p)
{
int16_t j;
for (j=0; j<=GSM_N; j++)
{
p->sps->c[j] = p->sps->c[j-1] + (p->sps->m[j-1] * p->h);
}
p->sps->c[0] = 0.0f;
}
//! \brief Runs the Gain Scheduler Module
//! \param[in] p Pointer to DCL_GSM structure
//! \param[in] x Input
//! \return Control effort
//!
static inline float32_t DCL_runGSM_C1(DCL_GSM *p, float32_t x)
{
int16_t sector;
float32_t sgnx, modx, rval;
sgnx = (x >= 0.0f) ? 1.0f : -1.0f;
modx = fabsf(x);
if (modx >= 1.0f)
{
rval = p->c[GSM_N] * sgnx;
}
else
{
sector = (int16_t) (modx * (float32_t) GSM_N);
rval = (p->m[sector] * (modx - (sector * p->h)) + p->c[sector]) * sgnx;
}
return(rval);
}
//----------------------------------------------------------------------------
#ifdef __cplusplus
}
#endif // extern "C"
#endif // _C_DCLF32_H
/* end of file */
Reference in New Issue View Git Blame Copy Permalink