/*
*========================================================================================
* (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;