#ifndef _QUAD_POS_CONTROL_H_
#define _QUAD_POS_CONTROL_H_

#include "quad_pos_control_math.h"
#include "component/pid.h"

// 速度控制器参数
typedef struct {
    KPID_Params_t xy;  // XY轴PID参数
    KPID_Params_t z;   // Z轴PID参数
    float hover_thrust; // 悬停推力
    float thr_min;      // 最小推力
    float thr_max;      // 最大推力
    float thr_xy_margin; // 水平推力裕度
    float g;            // 重力加速度
} VelControlParams_t;

typedef struct
{
    struct {
        float x;
        float y;
        float z;
    } pos_desire, 
      pos_now,
      vel_forward,
      vel_desire,
      vel_now,     // 当前速度
      vel_dot,     // 速度微分
      thr_sp;      // 推力设定值

} QuadPosition_t;

typedef struct
{
    float a;
} QuadPosture_t;

// 函数声明
void QuadPosControl_Init(void);
void PositionControl(QuadPosition_t *quad_pos, float dt);
void VelocityControl(QuadPosition_t *quad_pos, float dt);

#endif // _QUAD_POS_CONTROL_H_