#ifndef _PID_H_
#define _PID_H_


#include "stdbool.h"
#include "filter.h"

typedef struct
{
 float p;
 float i;
 float d;
}vector3f_pid;


typedef struct
{
    uint8 Err_Limit_Flag :1;//偏差限幅标志
    uint8 Integrate_Limit_Flag :1;//积分限幅标志
    uint8 Integrate_Separation_Flag :1;//积分分离标志
    float Expect;//期望
    float FeedBack;//反馈值
    float Err;//偏差
    float Last_Err;//上次偏差
    float Err_Max;//偏差限幅值
    float Integrate_Separation_Err;//积分分离偏差值
    float Integrate;//积分值
    float Integrate_Max;//积分限幅值
    float Kp;//控制参数Kp
    float Ki;//控制参数Ki
    float Kd;//控制参数Kd
    float Control_OutPut;//控制器总输出
    float Last_Control_OutPut;//上次控制器总输出
    float Control_OutPut_Limit;//输出限幅
    /***************************************/
    float Pre_Last_Err;//上上次偏差
    float Adaptable_Kd;//自适应微分参数
    float Last_FeedBack;//上次反馈值
    float Dis_Err;//微分量
    float Dis_Error_History[5];//历史微分量
    float Err_LPF;
    float Last_Err_LPF;
    float Dis_Err_LPF;
    float Last_Dis_Err_LPF;
    float Pre_Last_Dis_Err_LPF;
    lpf_buf _lpf_buf;//控制器低通输入输出缓冲
		lpf_param _lpf_param;//控制器低通滤波器参数
		float derivative,last_derivative;//上次微分量
		float d_lpf_alpha;
		systime Systime_t;
}PID_Controler;
float BPF_Butterworth(float curr_input,lpf_buf *Buffer,lpf_param *Parameter);

typedef struct
{
     PID_Controler Height_Position_Control;
     /*************光流控制器，新加****************/
     PID_Controler Optical_Position_Control;
     PID_Controler Optical_Speed_Control;
     PID_Controler SDK_Roll_Position_Control;
     PID_Controler SDK_Pitch_Position_Control;
}AllControler;

typedef enum
{
	Height_Position_Controler=0, 
	Optical_Position_Controler=1,
	Optical_Speed_Controler=2,
	SDK_Roll_Position_Controler=3,
	SDK_Pitch_Position_Controler=4,
}Controler_Label;

extern AllControler Total_Controller;


void  Total_PID_Init(void);
void  PID_Init(PID_Controler *Controler,Controler_Label Label);

float PID_Control(PID_Controler *Controler,float period_second);
float PID_Control_Yaw(PID_Controler *Controler,float period_second);
float PID_Control_Div_LPF(PID_Controler *Controler,float period_second);
float PID_Control_Err_LPF(PID_Controler *Controler,float period_second);
float PID_Control_SDK_Err_LPF(PID_Controler *Controler,uint8_t Differential_Enable_Flag,float period_second);
float PID_Control_Div_LPF_For_Gyro(PID_Controler *Controler,float period_second);
float pid_ctrl_rpy_gyro(PID_Controler *ctrl,float period_second,bool incomplete_diff,bool first_order_lpf);

void  PID_LPF_Reset(PID_Controler *Controler,Controler_Label Label);
void  PID_Integrate_Reset(PID_Controler *Controler);
void  Take_Off_Reset(void);
void East_North_Ctrl_Reset(void);

void Throttle_Control_Reset(void);


#endif