83 lines
2.6 KiB
C
83 lines
2.6 KiB
C
|
|
|||
|
|
/**
|
|||
|
|
****************************(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
|