/*******************************************************************************
 * File Name         : motion_config.h
 * Description       : Tunable parameters for mower motion controller
 * Created on        : 2025-11-22
 *******************************************************************************/

#ifndef FML_MOTION_CONFIG_H_
#define FML_MOTION_CONFIG_H_

#include "HIDO_TypeDef.h"

/* 控制回路期望频率 (Hz) */
#define MC_CFG_CONTROL_HZ                (75.0f)
/* 最大前进 / 后退速度 (m/s) */
#define MC_CFG_MAX_FORWARD_MPS           (0.60f)
#define MC_CFG_MAX_REVERSE_MPS           (0.20f)
/* 转向角速度上限 (rad/s) */
#define MC_CFG_MAX_TURN_RATE_RAD         (0.80f)
/* 巡航基础速度与最低跟随速度 (m/s) */
#define MC_CFG_BASE_SPEED_MPS            (0.45f)
#define MC_CFG_MIN_FOLLOW_SPEED_MPS      (0.08f)
/* Pure pursuit lookahead 范围 (m) */
#define MC_CFG_LOOKAHEAD_MIN_M           (0.35f)
#define MC_CFG_LOOKAHEAD_MAX_M           (1.20f)
/* 起点/终点距离与航向容差 (m / rad) */
#define MC_CFG_START_POS_TOL_M           (0.25f)
#define MC_CFG_START_HEADING_TOL_RAD     (0.20f)
#define MC_CFG_GOAL_TOL_M                (0.30f)
/* 控制增益：航向 P/D、横向 P，以及误差→降速的比例 */
#define MC_CFG_HEADING_KP                (1.6f)
#define MC_CFG_HEADING_KD                (0.25f)
#define MC_CFG_XTRACK_KP                 (1.0f)
#define MC_CFG_HEADING_SPEED_SCALE       (0.60f)
#define MC_CFG_XTRACK_SPEED_SCALE        (0.40f)

/* GGA 原点经纬高（deg/m），供 ENU 转换使用 */
#define MC_CFG_ORIGIN_LAT_DEG            (39.8310000)
#define MC_CFG_ORIGIN_LON_DEG            (116.2785000)
#define MC_CFG_ORIGIN_ALT_M              (47.500)

/* PWM 中值与 span，用于 forward/turn → 脉宽映射 */
#define MC_CFG_PWM_CENTER_US             (1500U)
#define MC_CFG_PWM_SPAN_US               (500U)

#endif /* FML_MOTION_CONFIG_H_ */