#ifndef _ADRC_H__
#define _ADRC_H__

#include "math/fast_math.h"
#include "bsp/bsp.h"

typedef struct {
	//扩张状态观测器
	float b0;
	float z1,z2;
	float p1,p2;
	float last_z1, last_z2;
	float ts;
	//线性状态误差反馈控制器
	float wcv;
	float out_min;
	float out_max;
	float out;
}ladrc_t;

void ladrc_init(ladrc_t *adrc, float ts, float wo, float wc, float b0);
float ladrc_run(ladrc_t *adrc, float in, float feedback);
void ladrc_reset(ladrc_t* adrc, float feedback, float out_init);
void ladrc_copy(ladrc_t *dst, ladrc_t *src);

static void __INLINE ladrc_set_range(ladrc_t *adrc, float min, float max) {
	adrc->out_min = min;
	adrc->out_max = max;
}
#endif /* _ADRC_H__ */