96 lines
2.2 KiB
C
96 lines
2.2 KiB
C
/*
|
||
Modified from
|
||
https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.h
|
||
*/
|
||
|
||
#pragma once
|
||
|
||
#ifdef __cplusplus
|
||
extern "C" {
|
||
#endif
|
||
|
||
#include <stdint.h>
|
||
|
||
#include "filter.h"
|
||
#include "user_math.h"
|
||
|
||
/* PID模式 */
|
||
typedef enum {
|
||
KPID_MODE_NO_D = 0, /* 不使用微分项，PI控制器 */
|
||
KPID_MODE_CALC_D, /* 根据反馈的值计算离散微分，忽略PID_Calc中的fb_dot */
|
||
KPID_MODE_SET_D /* 直接提供微分值，PID_Calc中的fb_dot将被使用，(Gyros) */
|
||
} KPID_Mode_t;
|
||
|
||
/* PID参数 */
|
||
typedef struct {
|
||
float k; /* 控制器增益，设置为1用于并行模式 */
|
||
float p; /* 比例项增益，设置为1用于标准形式 */
|
||
float i; /* 积分项增益 */
|
||
float d; /* 微分项增益 */
|
||
float i_limit; /* 积分项上限 */
|
||
float out_limit; /* 输出绝对值限制 */
|
||
float d_cutoff_freq; /* D项低通截止频率 */
|
||
float range; /* 计算循环误差时使用，大于0时启用 */
|
||
} KPID_Params_t;
|
||
|
||
/* PID主结构体 */
|
||
typedef struct {
|
||
KPID_Mode_t mode;
|
||
const KPID_Params_t *param;
|
||
|
||
float dt_min; /* 最小PID_Calc调用间隔 */
|
||
float i; /* 积分 */
|
||
|
||
struct {
|
||
float err; /* 上次误差 */
|
||
float k_fb; /* 上次反馈值 */
|
||
float out; /* 上次输出 */
|
||
} last;
|
||
|
||
LowPassFilter2p_t dfilter; /* D项低通滤波器 */
|
||
} KPID_t;
|
||
|
||
/**
|
||
* @brief 初始化PID
|
||
*
|
||
* @param pid PID结构体
|
||
* @param mode PID模式
|
||
* @param sample_freq 采样频率
|
||
* @param param PID参数
|
||
* @return int8_t 0对应没有错误
|
||
*/
|
||
int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
|
||
const KPID_Params_t *param);
|
||
|
||
/**
|
||
* @brief PID计算
|
||
*
|
||
* @param pid PID结构体
|
||
* @param sp 设定值
|
||
* @param fb 反馈值
|
||
* @param fb_dot 反馈值微分
|
||
* @param dt 间隔时间
|
||
* @return float 计算的输出
|
||
*/
|
||
float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt);
|
||
|
||
/**
|
||
* @brief 重置微分项
|
||
*
|
||
* @param pid PID结构体
|
||
* @return int8_t 0对应没有错误
|
||
*/
|
||
int8_t PID_ResetIntegral(KPID_t *pid);
|
||
|
||
/**
|
||
* @brief 重置PID
|
||
*
|
||
* @param pid PID结构体
|
||
* @return int8_t 0对应没有错误
|
||
*/
|
||
int8_t PID_Reset(KPID_t *pid);
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|