80 lines
3.2 KiB
C
80 lines
3.2 KiB
C
/*
|
||
*========================================================================================
|
||
* (C) Copyright 2019 АО "Диаконт"
|
||
* г. Санкт-Петербург, Россия
|
||
*
|
||
* Файл: pi.h
|
||
* Описание: Модуль ПИД-регулятора
|
||
*----------------------------------------------------------------------------------------
|
||
* История изменений:
|
||
* номер дата | Фамилия И.О. | ЗИ | Описание изменений
|
||
*----------------------------------------------------------------------------------------
|
||
* 001 | 24.01.19 | Сычев В.А. | 136 | Начальная разработка
|
||
* 002 | 10.03.21 | Сычев В.А. | 231 | Удалено поле uCorr
|
||
*========================================================================================
|
||
*/
|
||
|
||
|
||
#ifndef __PID_H__
|
||
#define __PID_H__
|
||
/*
|
||
*----------------------------------------------------------------------------------------
|
||
*Включаемые файлы
|
||
*----------------------------------------------------------------------------------------
|
||
*/
|
||
#include "iqmath.h"
|
||
|
||
/*
|
||
*----------------------------------------------------------------------------------------
|
||
*Объявления макросов
|
||
*----------------------------------------------------------------------------------------
|
||
*/
|
||
#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__
|
||
/*
|
||
*----------------------------------------------------------------------------------------
|
||
*Объявления типов
|
||
*----------------------------------------------------------------------------------------
|
||
*/
|
||
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;
|
||
|
||
|
||
|