/*******************************************************************************
|
* 文件名称 : motion_control.h
|
* 文件说明 : 割草机纯追踪运动控制器（C 接口声明）
|
* 创建日期 : 2025-11-22
|
*******************************************************************************/
|
|
#ifndef FML_MOTION_CONTROL_H_
|
#define FML_MOTION_CONTROL_H_
|
|
#include "HIDO_TypeDef.h"
|
#include "GPS.h"
|
|
/* 控制器状态机阶段 */
|
typedef enum
|
{
|
MC_STAGE_IDLE = 0, /* 未运行，等待首次 GPS */
|
MC_STAGE_GOTO_START, /* 移动到起点并校准航向 */
|
MC_STAGE_FOLLOW_PATH, /* 正常纯追踪阶段 */
|
MC_STAGE_FINISHED /* 路径已完成（保持静止） */
|
} E_MCStage;
|
|
/* 可调参数（默认值见 motion_config.h） */
|
typedef struct
|
{
|
float max_forward_mps; /* m/s：前进限速 */
|
float max_reverse_mps; /* m/s：倒车限速（未使用，可扩展） */
|
float max_turn_rate; /* rad/s：yaw 速率上限 */
|
float base_speed_mps; /* m/s：巡航基准速度 */
|
float min_follow_speed_mps; /* m/s：防止速度降至 0 的下限 */
|
float lookahead_min_m; /* m：低速时的最小前视距离 */
|
float lookahead_max_m; /* m：高速时的最大前视距离 */
|
float start_pos_tolerance_m; /* m：判断到达起点的距离容差 */
|
float start_heading_tolerance_rad; /* rad：起点航向容差 */
|
float goal_tolerance_m; /* m：终点完成阈值 */
|
float heading_kp; /* 航向误差 P 增益 */
|
float heading_kd; /* 航向误差 D 增益 */
|
float xtrack_kp; /* 横向误差 P 增益 */
|
float heading_speed_scale; /* 航向误差→减速比例 */
|
float xtrack_speed_scale; /* 横向误差→减速比例 */
|
} MC_Config;
|
|
/* 控制器输出（前向速度 + 角速度 + 调试信息） */
|
typedef struct
|
{
|
float forward_mps; /* 期望前向速度 (m/s) */
|
float turn_rate; /* 期望 yaw 角速度 (rad/s) */
|
float heading_error; /* 航向误差 (rad) */
|
float cross_track_error; /* 横向误差 (m) */
|
HIDO_BOOL active; /* 是否生成了有效控制量 */
|
E_MCStage stage; /* 当前控制阶段 */
|
float pos_enu[3]; /* 当前 ENU 位置 (E/N/U) */
|
float heading_deg; /* 当前航向 (deg) */
|
float target_heading_deg; /* 目标航向 (deg) */
|
float pitch_deg; /* 当前俯仰 (deg) */
|
float roll_deg; /* 当前横滚 (deg) */
|
HIDO_BOOL target_valid; /* 目标点是否有效 */
|
float target_xy[2]; /* 目标点 (ENU) */
|
} MC_Output;
|
|
/* 控制器内部状态结构 */
|
typedef struct
|
{
|
MC_Config config; /* 运行时使用的配置 */
|
const float *path_xy; /* ENU 路径数组(x0,y0,...) */
|
HIDO_UINT32 path_count; /* 路径点数量 */
|
float pos[3]; /* 当前 ENU 位置 (E,N,U) (m) */
|
float vel[2]; /* ENU 速度向量 (m/s) */
|
float heading_rad; /* 当前航向 (rad) */
|
float heading_deg; /* 当前航向 (deg) */
|
float pitch_deg; /* 当前俯仰 (deg) */
|
float roll_deg; /* 当前横滚 (deg) */
|
float yaw_rate_rad; /* 当前角速度 (rad/s) */
|
float speed_mps; /* 标量速度 (m/s) */
|
HIDO_BOOL pose_valid; /* GPS 数据是否有效 */
|
HIDO_BOOL imu_valid; /* IMU 数据是否有效 */
|
E_MCStage stage; /* 控制阶段 */
|
float current_target_xy[2]; /* 最近一次纯追踪目标点 */
|
HIDO_UINT32 nearest_index; /* 最近路径点索引（缓存） */
|
HIDO_UINT32 lookahead_index; /* 前视点索引（缓存） */
|
float last_heading_err; /* 上一周期航向误差 (rad) */
|
float last_turn_sign; /* 上一周期选择的转向方向（-1/0/+1） */
|
float heading_err_d; /* 保留字段：航向误差微分 */
|
float progress_m; /* 累计里程 (m，可扩展) */
|
} MC_State;
|
|
/* 使用库默认值初始化配置 */
|
HIDO_VOID MC_DefaultConfig(MC_Config *_cfg);
|
/* 绑定路径与配置，初始化控制器状态 */
|
HIDO_VOID MC_Init(MC_State *_state, const MC_Config *_cfg, const float *_path_xy, HIDO_UINT32 _point_count);
|
/* 注入最新 GPRMI 数据（ENU 位姿 + 航向/速度） */
|
HIDO_VOID MC_UpdateGps(MC_State *_state, const float _enu[3], const ST_GPRMI *_gprmi);
|
/* 注入 IMU 陀螺数据（yaw 角速度） */
|
HIDO_VOID MC_UpdateImu(MC_State *_state, const ST_GPIMU *_gpimu);
|
/* 执行一次控制循环，输出目标控制量 */
|
HIDO_VOID MC_Compute(MC_State *_state, float _dt_s, MC_Output *_out);
|
|
#endif /* FML_MOTION_CONTROL_H_ */
|
