Dahlin控制算法实现

参考 ：
https://blog.csdn.net/yin_bu_feng/article/details/85871595

大林控制算法：

#ifndef _DALIN_CONTROLLER_H_
#define _DALIN_CONTROLLER_H_

#ifdef __cplusplus
 extern "C" {
#endif


#ifndef __API__
#define __API__
#endif

#ifndef __STATIC_INLINE__  
#define __STATIC_INLINE__ static inline
#endif

struct DahlinController {
    float last_error;
    float u;
    float last_u;
    float previous_u;
    float a0;
    float a1;
    float b0;
    float b1;
};

__API__ __STATIC_INLINE__ int DahlinController_Init(struct DahlinController *dc)
{
    if (!dc) return -1;

	dc->last_error   = 0;
    dc->u            = 0;
    dc->last_u       = 0;
    dc->previous_u   = 0;
    dc->a0           = 0;
    dc->a1           = 0;
    dc->b0           = 0;
    dc->b1           = 0;
    return 0;
}


__API__ __STATIC_INLINE__ int DahlinController_a0_Set(struct DahlinController *dc, float a0)
{
    if (!dc) return -1;

    dc->a0 = a0;
    return 0;
}


__API__ __STATIC_INLINE__ int DahlinController_a1_Set(struct DahlinController *dc, float a1)
{
    if (!dc) return -1;

    dc->a1 = a1;
    return 0;
}


__API__ __STATIC_INLINE__ int DahlinController_b0_Set(struct DahlinController *dc, float b0)
{
    if (!dc) return -1;

    dc->b0 = b0;
    return 0;
}


__API__ __STATIC_INLINE__ int DahlinController_b1_Set(struct DahlinController *dc, float b1)
{
    if (!dc) return -1;

    dc->b1 = b1;
    return 0;
}


__API__ __STATIC_INLINE__ float DahlinControl(struct DahlinController *dc, float target, float actual)
{
    int result;
    float error;

    error = target - actual;

	dc->u = dc->b0*dc->last_u + dc->b1*dc->previous_u + dc->a0*error + dc->a1*dc->last_error;  /* Dahlin equation */

    dc->last_error = error;
	dc->previous_u = dc->last_u;
	dc->last_u = dc->u;

	return dc->u;
}

#ifdef __cplusplus
}
#endif

#endif /* _DALIN_CONTROLLER_H_ */