#ifndef __MODE_ATTITUDE_H
#define __MODE_ATTITUDE_H

#include "flt_butter.h"
#include "lowpass_filter.h"
#include "stdbool.h"

struct controller_xy
{
    float pc[2];            /* 当前位置  m */
    float vc[2];            /* 当前速度  m/s */
    float ac[2];            /* 当前加速度 m/s^2 */

    float pt[2];             /* 目标位置  m */
    float at[2];             /* 目标加速度 m/s^2 */
    float vt[2];             /* 目标速度  m/s */
    float ve_integ[2];       /* 速度误差积分 */
    LpfButter2 vt_ff_flt[2]; /* 目标速度微分前馈滤波器 */

    float pos_integ[2];     /* 位置误差积分 */

    bool reset; /* 控制器重置 */
};
extern struct controller_xy _cxy;
extern float dist_comp[2];

bool attitude_init(void);
void attitude_run(void);
void attitude_exit(void);

#endif