/**
  ****************************(C) COPYRIGHT 2016 DJI****************************
  * @file       pid.c/h
  * @brief      PID实现函数，包括初始化和PID计算函数
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. 初始版本
  *
  @verbatim
  ==============================================================================

  ==============================================================================  
  @endverbatim
  ****************************(C) COPYRIGHT 2016 DJI****************************
  */
#ifndef PID_H
#define PID_H
#include "struct_typedef.h"

enum PID_MODE
{
    PID_POSITION = 0,  // 位置式PID
    PID_DELTA,         // 增量式PID
    PID_POSITION_D     // 带低通滤波的D
};

/* PID参数结构体 */
typedef struct {
  float p;             /* 比例参数，默认为1，单位标准化 */
  float i;             /* 积分参数 */
  float d;             /* 微分参数 */
  float i_limit;       /* 积分限幅 */
  float out_limit;     /* 输出最大限制 */
  float d_cutoff_freq; /* D项低通截止频率 */
} pid_param_t;

typedef struct
{
    uint8_t mode;                  // PID模式
    const pid_param_t *param;      // PID参数结构体指针

    fp32 set;                      // 设定值
    fp32 fdb;                      // 反馈值

    fp32 out;                      // PID输出
    fp32 Pout;                     // 比例输出
    fp32 Iout;                     // 积分输出
    fp32 Dout;                     // 微分输出
    fp32 Dbuf[3];                  // 微分值缓冲区 [0: 当前值, 1: 上一次值, 2: 上上次值]
    fp32 error[3];                 // 误差缓冲区 [0: 当前误差, 1: 上一次误差, 2: 上上次误差]

} pid_type_def;

/**
  * @brief          PID结构体数据初始化
  * @param[out]     pid: PID结构体指针
  * @param[in]      mode: PID_POSITION: 普通PID
  *                 PID_DELTA: 增量PID
  * @param[in]      param: PID参数 [0: kp, 1: ki, 2: kd, 3: i_limit, 4: out_limit, 5: d_cutoff_freq]
  * @retval         none
  */
extern int8_t PID_init(pid_type_def *pid, uint8_t mode, const pid_param_t *param);

/**
  * @brief          PID计算
  * @param[out]     pid: PID结构体指针
  * @param[in]      ref: 反馈数据
  * @param[in]      set: 设定值
  * @retval         PID输出值
  */
extern fp32 PID_calc(pid_type_def *pid, fp32 ref, fp32 set);

/**
  * @brief          PID输出清零
  * @param[out]     pid: PID结构体指针
  * @retval         none
  */
extern void PID_clear(pid_type_def *pid);

#endif