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
|