80 lines
3.2 KiB
C
80 lines
3.2 KiB
C
|
/*
|
|||
|
|
*========================================================================================
|
|||
|
|
* (C) Copyright 2019 <EFBFBD><EFBFBD> "<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>"
|
|||
|
|
* <EFBFBD>. <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>-<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*
|
|||
|
|
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>: pi.h
|
|||
|
|
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD>-<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*----------------------------------------------------------------------------------------
|
|||
|
|
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
|
|||
|
|
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD> | <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD>.<EFBFBD>. | <EFBFBD><EFBFBD> | <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*----------------------------------------------------------------------------------------
|
|||
|
|
* 001 | 24.01.19 | <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD>.<EFBFBD>. | 136 | <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
* 002 | 10.03.21 | <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD>.<EFBFBD>. | 231 | <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD> uCorr
|
|||
|
|
*========================================================================================
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
|
|||
|
|
#ifndef __PID_H__
|
|||
|
|
#define __PID_H__
|
|||
|
|
/*
|
|||
|
|
*----------------------------------------------------------------------------------------
|
|||
|
|
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*----------------------------------------------------------------------------------------
|
|||
|
|
*/
|
|||
|
|
#include "iqmath.h"
|
|||
|
|
|
|||
|
|
/*
|
|||
|
|
*----------------------------------------------------------------------------------------
|
|||
|
|
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*----------------------------------------------------------------------------------------
|
|||
|
|
*/
|
|||
|
|
#define PID_RESET(PI) (PI.Fbk = PI.i1 = PI.v1 = PI.d1 = PI.Out = 0.0f)
|
|||
|
|
|
|||
|
|
/*------------------------------------------------------------------------------
|
|||
|
|
PI_GRANDO Macro Definition
|
|||
|
|
------------------------------------------------------------------------------*/
|
|||
|
|
|
|||
|
|
#define PID_MACRO(v) \
|
|||
|
|
\
|
|||
|
|
/* proportional term */ \
|
|||
|
|
v.up = _IQmpy(v.Kp, (v.Ref - v.Fbk)); \
|
|||
|
|
\
|
|||
|
|
/* integral term */ \
|
|||
|
|
v.ui = (v.Out == v.v1)?(_IQmpy(v.Ki, v.up)+ v.i1) : v.i1; \
|
|||
|
|
v.i1 = v.ui; \
|
|||
|
|
/* diff term*/ \
|
|||
|
|
v.ud = _IQmpy(v.Kd, (v.up - v.d1)); \
|
|||
|
|
v.d1 = v.up; \
|
|||
|
|
/* control output */ \
|
|||
|
|
v.v1 = v.up + v.ui + v.ud; \
|
|||
|
|
\
|
|||
|
|
v.Out= _IQsat(v.v1, v.Umax, v.Umin); \
|
|||
|
|
|
|||
|
|
#endif // __PID_H__
|
|||
|
|
/*
|
|||
|
|
*----------------------------------------------------------------------------------------
|
|||
|
|
*<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*----------------------------------------------------------------------------------------
|
|||
|
|
*/
|
|||
|
|
typedef struct
|
|||
|
|
{
|
|||
|
|
_iq Ref; // Input: reference set-point
|
|||
|
|
_iq Fbk; // Input: feedback
|
|||
|
|
_iq Out; // Output: controller output
|
|||
|
|
_iq Kp; // Parameter: proportional loop gain
|
|||
|
|
_iq Ki; // Parameter: integral gain
|
|||
|
|
_iq Kd; // Diff gain
|
|||
|
|
_iq Umax; // Parameter: upper saturation limit
|
|||
|
|
_iq Umin; // Parameter: lower saturation limit
|
|||
|
|
_iq up; // Data: proportional term
|
|||
|
|
_iq ui; // Data: integral term
|
|||
|
|
_iq ud; // Diff term
|
|||
|
|
_iq v1; // Data: pre-saturated controller output
|
|||
|
|
_iq i1; // Data: integrator storage: ui(k-1)
|
|||
|
|
_iq d1; // Diff storage
|
|||
|
|
} TPid;
|
|||
|
|
|
|||
|
|
|
|||
|
|
|