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
|