/*******************************************************************************
 * 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.831706)
#define MC_CFG_ORIGIN_LON_DEG            (116.279282)
#define MC_CFG_ORIGIN_ALT_M              (4)

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

/*******************************************************************************
 * 校准参数（基于实测数据，日期：2025-12-05）
 * 前向速度模型：v = MC_CFG_FORWARD_K × (1500 - pwm) + MC_CFG_FORWARD_BIAS
 * 说明：1000 PWM = 最大前进，1500 PWM = 停止，2000 PWM = 最大后退
 *******************************************************************************/
#define MC_CFG_FORWARD_K                 (0.00196f)   // 速度系数 (m/s per PWM)
#define MC_CFG_FORWARD_BIAS              (-0.077f)    // 速度偏置 (m/s)
#define MC_CFG_FORWARD_DEADZONE_PWM      (40)          // 前进死区阈值

/* 转向角速度模型：ω = MC_CFG_STEERING_K × (1500 - pwm) for left/right */
#define MC_CFG_STEERING_K_LEFT           (0.00206f)    // 左转系数 (rad/s per PWM, 1000 PWM -> ~1.03 rad/s -> ~59 deg/s)
#define MC_CFG_STEERING_K_RIGHT          (0.00201f)    // 右转系数 (rad/s per PWM, 2000 PWM -> ~1.00 rad/s -> ~57 deg/s)
#define MC_CFG_STEERING_TRIM             (10)          // 转向补偿值（修正左偏）

/* 基于校准数据更新速度限制 */
#undef MC_CFG_MAX_FORWARD_MPS
#define MC_CFG_MAX_FORWARD_MPS           (0.56f)       // 实测最大前进速度 (1000 PWM)

#endif /* FML_MOTION_CONFIG_H */