#ifndef _ADRC_H__
#define _ADRC_H__

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

typedef struct {
	//扩张状态观测器
	float b0;
	float z1,z2;
	float p1,p2;
	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;
}


static void __INLINE ladrc_change_b0(ladrc_t *adrc, float b0) {
	adrc->b0 = b0;
}

static void __INLINE ladrc_change_K(ladrc_t *adrc, float k) {
	adrc->wcv = k;
}

static void ladrc_change_params(ladrc_t*  adrc, float wo, float wc, float b0) {
	adrc->b0 = b0;
	adrc->wcv = wc;
	adrc->p1 = 2 * wo;
	adrc->p2 = wo * wo;
}


#endif /* _ADRC_H__ */