404 lines
13 KiB
C
404 lines
13 KiB
C
|
/* DCLF64.h - Digital Control Library (64-bit floating point)
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
//#############################################################################
|
||
|
|
//!
|
||
|
|
//! 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_DCLF64_H
|
||
|
|
#define _C_DCLF64_H
|
||
|
|
|
||
|
|
#ifdef __cplusplus
|
||
|
|
extern "C" {
|
||
|
|
#endif
|
||
|
|
|
||
|
|
//! \file DCLF64.h
|
||
|
|
//! \brief Contains the public interface to the
|
||
|
|
//! 64-bit PID controller functions
|
||
|
|
|
||
|
|
#include "DCL.h"
|
||
|
|
|
||
|
|
//--- PIDF64 controller -----------------------------------------------
|
||
|
|
|
||
|
|
#pragma CODE_SECTION(DCL_updatePIDF64,"dclfuncs")
|
||
|
|
#pragma CODE_SECTION(DCL_runPIDF64_S1,"dclfuncs")
|
||
|
|
|
||
|
|
//! \brief Defines the shadow PIDF64 controller structure
|
||
|
|
//!
|
||
|
|
typedef struct dcl_pidf64_sps {
|
||
|
|
float64_t Kp; //!< Proportional gain
|
||
|
|
float64_t Ki; //!< Integral gain
|
||
|
|
float64_t Kd; //!< Derivative gain
|
||
|
|
float64_t Kr; //!< Set point weight
|
||
|
|
float64_t c1; //!< D-term filter coefficient 1
|
||
|
|
float64_t c2; //!< D-term filter coefficient 2
|
||
|
|
float64_t Umax; //!< Upper saturation limit
|
||
|
|
float64_t Umin; //!< Lower saturation limit
|
||
|
|
} DCL_PIDF64_SPS;
|
||
|
|
|
||
|
|
//! \brief Defines default values to initialize the DCL_PIDF64 shadow structure
|
||
|
|
//!
|
||
|
|
#define PIDF64_SPS_DEFAULTS { 1.0L, 0.0L, 0.0L, 1.0L, 1.0L, 0.0L, 1.0L, -1.0L }
|
||
|
|
|
||
|
|
//! \brief Defines the active PIDF64 controller structure
|
||
|
|
//!
|
||
|
|
typedef struct dcl_pidf64 {
|
||
|
|
// active parameters
|
||
|
|
float64_t Kp; //!< Proportional gain
|
||
|
|
float64_t Ki; //!< Integral gain
|
||
|
|
float64_t Kd; //!< Derivative gain
|
||
|
|
float64_t Kr; //!< Set point weight
|
||
|
|
float64_t c1; //!< D-term filter coefficient 1
|
||
|
|
float64_t c2; //!< D-term filter coefficient 2
|
||
|
|
float64_t d2; //!< D-term filter intermediate storage 1
|
||
|
|
float64_t d3; //!< D-term filter intermediate storage 2
|
||
|
|
float64_t i10; //!< I-term intermediate storage
|
||
|
|
float64_t i14; //!< Intermediate saturation storage
|
||
|
|
float64_t i16; //!< Spare
|
||
|
|
float64_t Umax; //!< Upper saturation limit
|
||
|
|
float64_t Umin; //!< Lower saturation limit
|
||
|
|
DCL_PIDF64_SPS *sps; //!< Pointer to shadow parameter set
|
||
|
|
DCL_CSS *css; //!< Pointer to common support structure
|
||
|
|
} DCL_PIDF64;
|
||
|
|
|
||
|
|
//! \brief Defines default values to initialize the DCL_PIDF64 active structure
|
||
|
|
//!
|
||
|
|
#define PIDF64_DEFAULTS { 1.0L, 0.0L, 0.0L, 1.0L, 1.0L, 0.0L, 0.0L, 0.0L, \
|
||
|
|
0.0L, 1.0L, 0.0L, 1.0L, -1.0L, NULL_ADDR, NULL_ADDR }
|
||
|
|
|
||
|
|
//! \brief Resets the PIDF64 internal storage data
|
||
|
|
//! \param[in] p Pointer to the DCL_PIDF64 structure
|
||
|
|
//! \return None
|
||
|
|
//!
|
||
|
|
static inline void DCL_resetPIDF64(DCL_PIDF64 *p)
|
||
|
|
{
|
||
|
|
uint16_t v;
|
||
|
|
|
||
|
|
v = DCL_DISABLE_INTS;
|
||
|
|
p->d2 = p->d3 = p->i10 = 0.0L;
|
||
|
|
p->i14 = 1.0L;
|
||
|
|
DCL_RESTORE_INTS(v);
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Copies shadow controller parameters to the active
|
||
|
|
//! DCL_PIDF64 structure while global interrupts disabled
|
||
|
|
//! \param[in] p Pointer to the DCL_PIDF64 controller structure
|
||
|
|
//! \return None
|
||
|
|
//!
|
||
|
|
static inline void DCL_updatePIDF64(DCL_PIDF64 *p)
|
||
|
|
{
|
||
|
|
uint16_t v;
|
||
|
|
|
||
|
|
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 the derivative path filter shadow coefficients
|
||
|
|
//! Note: changes will not become effective until update called
|
||
|
|
//! \param[in] p Pointer to the DCL_PIDF64 structure
|
||
|
|
//! \param[in] fc The desired filter bandwidth in Hz
|
||
|
|
//! \param[in] T The sample period in seconds
|
||
|
|
//! \return None
|
||
|
|
//!
|
||
|
|
static inline void DCL_setPIDF64filterBW(DCL_PIDF64 *p, float64_t fc)
|
||
|
|
{
|
||
|
|
float64_t tau;
|
||
|
|
|
||
|
|
#ifdef DCL_ERROR_HANDLING_ENABLED
|
||
|
|
p->css->err |= ((fc >= 1.0L / (2.0L * p->css->T)) || (fc <= 0.0L)) ? ERR_PARAM_RANGE : ERR_NONE;
|
||
|
|
if (p->css->err)
|
||
|
|
{
|
||
|
|
DCL_GET_ERROR_LOC(p->css);
|
||
|
|
DCL_RUN_ERROR_HANDLER(p->css);
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
tau = 1.0L / (2.0L * CONST_PI_64 * fc);
|
||
|
|
p->sps->c1 = 2.0L / (p->css->T + 2.0L * tau);
|
||
|
|
p->sps->c2 = (p->css->T - 2.0L * tau) / (p->css->T + 2.0L * tau);
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Loads the PIDF64 derivative path filter active coefficients
|
||
|
|
//! Note: new coefficients take effect immediately. SPS &
|
||
|
|
//! CSS contents are unaffected.
|
||
|
|
//! \param[in] p Pointer to the DCL_PIDF64 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_setActivePIDF64filterBW(DCL_PIDF64 *p, float64_t fc, float64_t T)
|
||
|
|
{
|
||
|
|
float64_t tau;
|
||
|
|
|
||
|
|
#ifdef DCL_ERROR_HANDLING_ENABLED
|
||
|
|
p->css->err |= ((fc >= 1.0L / (2.0L * T)) || (fc <= 0.0L)) ? ERR_PARAM_RANGE : ERR_NONE;
|
||
|
|
if (p->css->err)
|
||
|
|
{
|
||
|
|
DCL_GET_ERROR_LOC(p->css);
|
||
|
|
DCL_RUN_ERROR_HANDLER(p->css);
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
tau = 1.0L / (2.0L * CONST_PI_64 * fc);
|
||
|
|
p->c1 = 2.0L / (T + 2.0L * tau);
|
||
|
|
p->c2 = (T - 2.0L * tau) / (T + 2.0L * tau);
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Returns the active derivative path filter bandwidth in Hz
|
||
|
|
//! \param[in] p Pointer to the DCL_PIDF64 structure
|
||
|
|
//! \return The filter bandwidth in Hz
|
||
|
|
//!
|
||
|
|
static inline float64_t DCL_getPIDF64filterBW(DCL_PIDF64 *p)
|
||
|
|
{
|
||
|
|
float64_t tau;
|
||
|
|
|
||
|
|
tau = ((2.0L - p->c1 * p->css->T) / (2.0L * p->c1));
|
||
|
|
return(1.0L / (2.0L * CONST_PI_64 * tau));
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Executes an inline series form PIDF64 controller
|
||
|
|
//! \param[in] p Pointer to the DCL_PIDF64 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 float64_t DCL_runPIDF64_S1(DCL_PIDF64 *p, float64_t rk, float64_t yk, float64_t lk)
|
||
|
|
{
|
||
|
|
float64_t v1, v4, v5, v8, v9, v10, v12;
|
||
|
|
|
||
|
|
#ifdef DCL_ERROR_HANDLING_ENABLED
|
||
|
|
p->css->err |= (p->css->sts & STS_CONTROLLER_RUNNING) ? ERR_CONTROLLER : ERR_NONE;
|
||
|
|
if (p->css->err)
|
||
|
|
{
|
||
|
|
DCL_GET_ERROR_LOC(p->css);
|
||
|
|
DCL_RUN_ERROR_HANDLER(p->css);
|
||
|
|
}
|
||
|
|
DCL_CONTROLLER_BEGIN(p);
|
||
|
|
#endif
|
||
|
|
|
||
|
|
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.0 : 0.0;
|
||
|
|
p->i14 = v12 * lk;
|
||
|
|
|
||
|
|
#ifdef DCL_TESTPOINTS_ENABLED
|
||
|
|
p->css->tpt = (float32_t) v5;
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#ifdef DCL_ERROR_HANDLING_ENABLED
|
||
|
|
DCL_CONTROLLER_END(p);
|
||
|
|
#endif
|
||
|
|
|
||
|
|
return(v10);
|
||
|
|
}
|
||
|
|
|
||
|
|
//--- DF22F64 controller -----------------------------------------------------
|
||
|
|
|
||
|
|
#pragma CODE_SECTION(DCL_updateDF22F64,"dclfuncs")
|
||
|
|
#pragma CODE_SECTION(DCL_runDF22F64_S1,"dclfuncs")
|
||
|
|
#pragma CODE_SECTION(DCL_runDF22F64_S2,"dclfuncs")
|
||
|
|
#pragma CODE_SECTION(DCL_runDF22F64_S3,"dclfuncs")
|
||
|
|
|
||
|
|
//! \brief Defines the DCL_DF22F64 shadow parameter set
|
||
|
|
//!
|
||
|
|
typedef struct dcl_df22f64_sps {
|
||
|
|
float64_t b0; //!< b0
|
||
|
|
float64_t b1; //!< b1
|
||
|
|
float64_t b2; //!< b2
|
||
|
|
float64_t a1; //!< a1
|
||
|
|
float64_t a2; //!< a2
|
||
|
|
} DCL_DF22F64_SPS;
|
||
|
|
|
||
|
|
#define DF22F64_SPS_DEFAULTS { 1.0, 0.0, 0.0, 0.0, 0.0 }
|
||
|
|
|
||
|
|
//! \brief Defines the DCL_DF22F64 controller structure
|
||
|
|
//!
|
||
|
|
typedef struct dcl_df22f64 {
|
||
|
|
float64_t b0; //!< b0
|
||
|
|
float64_t b1; //!< b1
|
||
|
|
float64_t b2; //!< b2
|
||
|
|
float64_t a1; //!< a1
|
||
|
|
float64_t a2; //!< a2
|
||
|
|
float64_t x1; //!< x1
|
||
|
|
float64_t x2; //!< x2
|
||
|
|
DCL_DF22F64_SPS *sps; //!< Pointer to the shadow parameter set
|
||
|
|
DCL_CSS *css; //!< Pointer to the common support structure
|
||
|
|
} DCL_DF22F64;
|
||
|
|
|
||
|
|
//! \brief Defines default values to initialize the DCL_DF22F64
|
||
|
|
//! structure
|
||
|
|
//!
|
||
|
|
#define DF22F64_DEFAULTS { 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, \
|
||
|
|
NULL_ADDR, NULL_ADDR }
|
||
|
|
|
||
|
|
//! \brief Resets internal DF22F64 controller data to zero
|
||
|
|
//! \param[in] p Pointer to the DCL_DF22F64 controller structure
|
||
|
|
//! \return None
|
||
|
|
//!
|
||
|
|
static inline void DCL_resetDF22F64(DCL_DF22F64 *p)
|
||
|
|
{
|
||
|
|
uint16_t v;
|
||
|
|
|
||
|
|
v = DCL_DISABLE_INTS;
|
||
|
|
p->x1 = p->x2 = 0.0;
|
||
|
|
DCL_RESTORE_INTS(v);
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Updates active coefficients from shadow set
|
||
|
|
//! \param[in] p Pointer to the DCL_DF22F64 controller structure
|
||
|
|
//! \return None
|
||
|
|
//!
|
||
|
|
static inline void DCL_updateDF22F64(DCL_DF22F64 *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 Executes a full 2nd order Direct Form 2 controller on the
|
||
|
|
//! FPU64. Implemented as inline C function.
|
||
|
|
//! \param[in] p Pointer to the DCL_DF22F64 controller structure
|
||
|
|
//! \param[in] ek The servo error
|
||
|
|
//! \return The control effort
|
||
|
|
//!
|
||
|
|
static inline float64_t DCL_runDF22F64_S1(DCL_DF22F64 *p, float64_t ek)
|
||
|
|
{
|
||
|
|
float64_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);
|
||
|
|
|
||
|
|
#ifdef DCL_TESTPOINTS_ENABLED
|
||
|
|
p->css->tpt = (float32_t) p->x2;
|
||
|
|
#endif
|
||
|
|
|
||
|
|
return(v7);
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Executes an immediate 2nd order Direct Form 2 controller
|
||
|
|
//! on the FPU64. Implemented as inline C function.
|
||
|
|
//! \param[in] p Pointer to the DCL_DF22F64 controller structure
|
||
|
|
//! \param[in] ek The servo error
|
||
|
|
//! \return The control effort
|
||
|
|
//!
|
||
|
|
static inline float64_t DCL_runDF22F64_S2(DCL_DF22F64 *p, float64_t ek)
|
||
|
|
{
|
||
|
|
return((ek * p->b0) + p->x1);
|
||
|
|
}
|
||
|
|
|
||
|
|
//! \brief Executes a partial pre-computed 2nd order Direct Form 2
|
||
|
|
//! controller on the FPU64. Implemented as inline C function.
|
||
|
|
//! \param[in] p Pointer to the DCL_DF22F64 controller structure
|
||
|
|
//! \param[in] ek The servo error
|
||
|
|
//! \param[in] uk The controller output in the previous sample interval
|
||
|
|
//!
|
||
|
|
static inline void DCL_runDF22F64_S3(DCL_DF22F64 *p, float64_t ek, float64_t uk)
|
||
|
|
{
|
||
|
|
p->x1 = (ek * p->b1) + p->x2 - (uk * p->a1);
|
||
|
|
p->x2 = (ek * p->b2) - (uk * p->a2);
|
||
|
|
}
|
||
|
|
|
||
|
|
//----------------------------------------------------------------------------
|
||
|
|
|
||
|
|
//! \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_S1(float64_t *data, float64_t Umax, float64_t Umin)
|
||
|
|
{
|
||
|
|
float64_t iv = *(data);
|
||
|
|
|
||
|
|
*(data) = (*(data) > Umax) ? Umax : *(data);
|
||
|
|
*(data) = (*(data) < Umin) ? Umin : *(data);
|
||
|
|
|
||
|
|
return(((iv < Umax) && (iv > Umin)) ? 0 : 1);
|
||
|
|
}
|
||
|
|
|
||
|
|
//----------------------------------------------------------------------------
|
||
|
|
|
||
|
|
#ifdef __cplusplus
|
||
|
|
}
|
||
|
|
#endif // extern "C"
|
||
|
|
|
||
|
|
#endif // _C_DCLF64_H
|
||
|
|
|
||
|
|
/* end of file */
|