From b53fff11e6f0d560594834de32886239cbba90a3 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期二, 16 十二月 2025 15:48:58 +0800
Subject: [PATCH] 外部调完，可以解析下发的MQTT数据了，但是路径文件太大准备换成http模式

---
 STM32H743/FML/motion_control.c |  296 ++++++++++++++++++++++++++++++++++++++++++++++------------
 1 files changed, 234 insertions(+), 62 deletions(-)

diff --git a/STM32H743/FML/motion_control.c b/STM32H743/FML/motion_control.c
index 7b19d09..fdb4902 100644
--- a/STM32H743/FML/motion_control.c
+++ b/STM32H743/FML/motion_control.c
@@ -20,6 +20,8 @@
 #define DEG2RAD                (0.017453292519943295769236907684886f)
 #define RAD2DEG                (57.295779513082320876798154814105f)
 #define MC_CLAMP(v, lo, hi)    ((v) < (lo) ? (lo) : (((v) > (hi)) ? (hi) : (v)))
+/* 褰撹埅鍚戣宸�艰繎 卤蟺 鏃剁殑鍒ゅ喅绐楀彛锛堢害 0.5掳锛� */
+#define MC_HEADING_PI_EPS      (0.00872664626f)
 
 /* 灏嗚搴︾害鏉熷埌 [-pi, pi]锛岄伩鍏嶈埅鍚戣烦鍙� */
 static float mc_wrap_angle(float angle)
@@ -35,9 +37,44 @@
     return angle;
 }
 
+/* 鍦ㄨ宸帴杩� 卤蟺 鏃朵繚鎸佷笂涓�鍛ㄦ湡鐨勮浆鍚戞柟鍚戯紝閬垮厤鏉ュ洖鍙嶈浆 */
+static float mc_heading_error_with_hysteresis(MC_State *state, float desired_heading)
+{
+    float err = mc_wrap_angle(desired_heading - state->heading_rad);
+    float abs_err = fabsf(err);
+    float pi_delta = (float)M_PI - abs_err;
+
+    if (pi_delta <= MC_HEADING_PI_EPS)
+    {
+        float preferred_sign = state->last_turn_sign;
+        if (preferred_sign == 0.0f)
+        {
+            preferred_sign = (err >= 0.0f) ? 1.0f : -1.0f;
+        }
+        float old_err = err;
+        err = preferred_sign * abs_err;
+        state->last_turn_sign = preferred_sign;
+        
+    }
+    else if (abs_err > 1.0e-6f)
+    {
+        state->last_turn_sign = (err > 0.0f) ? 1.0f : -1.0f;
+    }
+
+    return err;
+}
+
 /* 璇诲彇璺緞鐐癸紝绱㈠紩瓒婄晫鏃舵埅鍒版湯灏� */
 static void mc_get_point(const MC_State *state, HIDO_UINT32 idx, float outPt[2])
 {
+    /* 闃插尽鎬ф鏌ワ細濡傛灉璺緞鏁版嵁鏈垵濮嬪寲锛岃繑鍥炲師鐐� */
+    if (state->path_xy == HIDO_NULL || state->path_count == 0U)
+    {
+        outPt[0] = 0.0f;
+        outPt[1] = 0.0f;
+        return;
+    }
+    
     if (idx >= state->path_count)
     {
         idx = state->path_count - 1U;
@@ -66,48 +103,84 @@
     return dist;
 }
 
-/* 鎼滅储褰撳墠浣嶇疆鏈�杩戠殑璺緞鐐� */
-static HIDO_UINT32 mc_find_nearest(const MC_State *state)
+/* 鏁板鍧愭爣锛堜笢涓� 0掳锛岄�嗘椂閽堜负姝ｏ級鈫� 缃楃洏鍧愭爣锛堝寳涓� 0掳锛岄『鏃堕拡涓烘锛� */
+static float mc_math_rad_to_compass_deg(float heading_rad)
 {
-    float best_dist = 1.0e9f;
-    HIDO_UINT32 best_idx = state->nearest_index;
-    float pos[2] = {state->pos[0], state->pos[1]};
-
-    for (HIDO_UINT32 i = 0; i < state->path_count; i++)
+    float compass_deg = 90.0f - (heading_rad * RAD2DEG);
+    while (compass_deg >= 360.0f)
     {
-        float point[2];
-        mc_get_point(state, i, point);
-        float dist_sq = mc_distance_sq(point, pos);
-        if (dist_sq < best_dist)
-        {
-            best_dist = dist_sq;
-            best_idx = i;
-        }
+        compass_deg -= 360.0f;
     }
-    return best_idx;
+    while (compass_deg < 0.0f)
+    {
+        compass_deg += 360.0f;
+    }
+    return compass_deg;
 }
 
-/* 娌胯矾寰勭疮绉窛绂伙紝鐩村埌杈惧埌鍓嶈鐩爣 */
+/* 缃楃洏鍧愭爣锛堝寳涓� 0掳锛岄『鏃堕拡涓烘锛夆啋 鏁板鍧愭爣锛堜笢涓� 0掳锛岄�嗘椂閽堜负姝ｏ級 */
+static float mc_compass_deg_to_math_rad(float heading_deg)
+{
+    float math_deg = 90.0f - heading_deg;
+    return mc_wrap_angle(math_deg * DEG2RAD);
+}
+
+/* 鍒ゆ柇杞﹁締鏄惁宸茬粡"鍒拌揪"鏌愪釜璺緞鐐�
+ * 鏍囧噯锛氳窛绂昏鐐瑰皬浜庨槇鍊硷紙0.5绫筹級鍗充负鍒拌揪 */
+static HIDO_BOOL mc_is_point_reached(const MC_State *state, HIDO_UINT32 point_idx)
+{
+    if (point_idx >= state->path_count)
+    {
+        return HIDO_FALSE;
+    }
+    
+    float pos[2] = {state->pos[0], state->pos[1]};
+    float point[2];
+    mc_get_point(state, point_idx, point);
+    
+    float dist = mc_distance(pos, point);
+    
+    /* 瀹氫箟"鍒拌揪"闃堝�间负0.5绫� */
+    const float REACH_THRESHOLD_M = 0.5f;
+    
+    return (dist < REACH_THRESHOLD_M) ? HIDO_TRUE : HIDO_FALSE;
+}
+
+/* 鎼滅储褰撳墠浣嶇疆鏈�杩戠殑璺緞鐐癸紙鍙厑璁稿悜鍓嶆帹杩涳紝蹇呴』鍒拌揪鎵嶆帹杩涳級
+ * 绫讳技鑸偣瀵艰埅锛氬繀椤诲厛鍒拌揪鑸偣0锛屾墠鑳藉鑸埌鑸偣1 */
+static HIDO_UINT32 mc_find_nearest(const MC_State *state)
+{
+    HIDO_UINT32 current_idx = state->nearest_index;
+    
+    /* 妫�鏌ユ槸鍚﹀埌杈惧綋鍓嶇偣 */
+    if (mc_is_point_reached(state, current_idx) == HIDO_TRUE)
+    {
+        /* 鍒拌揪浜嗭紝鎺ㄨ繘鍒颁笅涓�涓偣锛堝鏋滀笉鏄渶鍚庝竴涓級 */
+        if (current_idx < state->path_count - 1U)
+        {
+            return current_idx + 1U;
+        }
+    }
+    
+    /* 鍚﹀垯淇濇寔褰撳墠绱㈠紩涓嶅彉 */
+    return current_idx;
+}
+
+/* 娌胯矾寰勭疮绉窛绂伙紝鐩村埌杈惧埌鍓嶈鐩爣
+ * 娉ㄦ剰锛歭ookahead鐐瑰缁堣嚦灏戞槸start_idx鐨勪笅涓�涓偣锛岀‘淇濋『搴忓鑸� */
 static HIDO_UINT32 mc_find_lookahead(const MC_State *state, HIDO_UINT32 start_idx, float lookahead_m)
 {
-    float accum = 0.0f;
-    float current[2];
-    mc_get_point(state, start_idx, current);
-
-    for (HIDO_UINT32 i = start_idx; i < state->path_count - 1U; i++)
+    /* 濡傛灉宸茬粡鏄渶鍚庝竴涓偣锛岀洿鎺ヨ繑鍥� */
+    if (start_idx >= state->path_count - 1U)
     {
-        float next[2];
-        mc_get_point(state, i + 1U, next);
-        float seg = mc_distance(current, next);
-        accum += seg;
-        if (accum >= lookahead_m)
-        {
-            return i + 1U;
-        }
-        current[0] = next[0];
-        current[1] = next[1];
+        return state->path_count - 1U;
     }
-    return state->path_count - 1U;
+    
+    /* lookahead鐐硅嚦灏戞槸start_idx鐨勪笅涓�涓偣锛堥『搴忓鑸細蹇呴』鍏堢粡杩囦笅涓�涓偣锛� */
+    HIDO_UINT32 next_idx = start_idx + 1U;
+    
+    /* 绠�鍖栭�昏緫锛氱洿鎺ヨ繑鍥炰笅涓�涓偣锛岀‘淇濇寜椤哄簭璁块棶姣忎釜鑸偣 */
+    return start_idx;
 }
 
 /* 璁＄畻甯︾鍙锋í鍚戣宸紙宸︽鍙宠礋锛� */
@@ -118,8 +191,17 @@
         return 0.0f;
     }
 
-    HIDO_UINT32 idx0 = (nearest_idx == 0U) ? 0U : nearest_idx - 1U;
-    HIDO_UINT32 idx1 = (nearest_idx + 1U < state->path_count) ? nearest_idx + 1U : nearest_idx;
+    HIDO_UINT32 idx0, idx1;
+    if (nearest_idx == 0U)
+    {
+        idx0 = 0U;
+        idx1 = (state->path_count > 1U) ? 1U : 0U;
+    }
+    else
+    {
+        idx0 = nearest_idx - 1U;
+        idx1 = nearest_idx;
+    }
 
     float p0[2];
     float p1[2];
@@ -166,6 +248,10 @@
 {
     float start_point[2];
     mc_get_point(state, 0U, start_point);
+    
+    /* 妫�娴嬬洰鏍囩偣鍙樺寲骞惰褰� - 宸茬Щ闄OG */
+    static HIDO_UINT32 s_goto_log_idx = 0U;
+    
     state->current_target_xy[0] = start_point[0];
     state->current_target_xy[1] = start_point[1];
     out->target_valid = HIDO_TRUE;
@@ -175,12 +261,13 @@
     float vec_y = start_point[1] - state->pos[1];
     float dist = mc_distance(start_point, state->pos);
 
-    float desired_heading = arm_atan2_f32(vec_y, vec_x);
-    out->target_heading_deg = desired_heading * RAD2DEG;
-    float heading_err = mc_wrap_angle(desired_heading - state->heading_rad);
+    float desired_heading;
+    arm_status status = arm_atan2_f32(vec_y, vec_x, &desired_heading);
+    out->target_heading_deg = mc_math_rad_to_compass_deg(desired_heading);
+    float heading_err = mc_heading_error_with_hysteresis(state, desired_heading);
     float yaw_rate_cmd = state->config.heading_kp * heading_err;
     yaw_rate_cmd = MC_CLAMP(yaw_rate_cmd, -state->config.max_turn_rate, state->config.max_turn_rate);
-
+    
     float forward = 0.0f;
     if (dist > state->config.start_pos_tolerance_m)
     {
@@ -194,7 +281,9 @@
     }
     else if (fabsf(heading_err) < state->config.start_heading_tolerance_rad)
     {
+        /* 鍒拌揪璧风偣涓旇埅鍚戝鍑嗭紝鍒囨崲鍒拌矾寰勮窡韪ā寮� */
         state->stage = MC_STAGE_FOLLOW_PATH;
+        state->nearest_index = 0U;  /* 閲嶇疆涓鸿矾寰勭涓�涓偣 */
     }
 
     out->forward_mps = forward;
@@ -208,17 +297,58 @@
 /* 闃舵锛氱函杩借釜 + 鑸悜/妯悜璇樊琛ュ伩 */
 static void mc_compute_follow_path(MC_State *state, float dt_s, MC_Output *out)
 {
+    /* 瀹夊叏妫�鏌ワ細璺緞蹇呴』鑷冲皯鏈�1涓偣 */
+    if (state->path_count == 0U)
+    {
+        out->forward_mps = 0.0f;
+        out->turn_rate = 0.0f;
+        out->active = HIDO_FALSE;
+        return;
+    }
+    
     HIDO_UINT32 nearest_idx = mc_find_nearest(state);
+    
+    /* 瀹夊叏妫�鏌ワ細nearest_idx 蹇呴』鍦ㄦ湁鏁堣寖鍥村唴 */
+    if (nearest_idx >= state->path_count)
+    {
+        nearest_idx = state->path_count - 1U;
+    }
     state->nearest_index = nearest_idx;
 
     float lookahead = state->config.lookahead_min_m;
     float speed_ratio = MC_CLAMP(state->speed_mps / state->config.max_forward_mps, 0.0f, 1.0f);
     lookahead += (state->config.lookahead_max_m - state->config.lookahead_min_m) * speed_ratio;
     HIDO_UINT32 lookahead_idx = mc_find_lookahead(state, nearest_idx, lookahead);
+    
+    /* 瀹夊叏妫�鏌ワ細lookahead_idx 蹇呴』鍦ㄦ湁鏁堣寖鍥村唴 */
+    if (lookahead_idx >= state->path_count)
+    {
+        lookahead_idx = state->path_count - 1U;
+    }
     state->lookahead_index = lookahead_idx;
+    
+    /* 璋冭瘯锛氭瘡20甯ц緭鍑轰竴娆¤矾寰勮窡韪姸鎬� */
+    static HIDO_UINT32 s_path_log_idx = 0U;
+    static HIDO_UINT32 s_last_nearest = 0xFFFFFFFF;
+    static HIDO_UINT32 s_last_lookahead = 0xFFFFFFFF;
+    
+    /* 濡傛灉绱㈠紩鍙戠敓鍙樺寲锛岀珛鍗宠緭鍑篖OG */
+    /* 璺緞鐐硅皟璇昄OG - 宸茬Щ闄� */
+//    static HIDO_UINT32 s_last_nearest = 0xFFFFFFFF;
+//    static HIDO_UINT32 s_last_lookahead = 0xFFFFFFFF;
+    HIDO_BOOL index_changed = (nearest_idx != s_last_nearest) || (lookahead_idx != s_last_lookahead);
+    
+    if (index_changed)
+    {
+        s_last_nearest = nearest_idx;
+        s_last_lookahead = lookahead_idx;
+    }
 
     float target[2];
     mc_get_point(state, lookahead_idx, target);
+    
+    /* 妫�娴嬬洰鏍囩偣鍙樺寲骞惰褰� - 宸茬Щ闄OG */
+    
     state->current_target_xy[0] = target[0];
     state->current_target_xy[1] = target[1];
     out->target_valid = HIDO_TRUE;
@@ -227,18 +357,23 @@
 
     float vec_x = target[0] - state->pos[0];
     float vec_y = target[1] - state->pos[1];
-    float desired_heading = arm_atan2_f32(vec_y, vec_x);
-    out->target_heading_deg = desired_heading * RAD2DEG;
-    float heading_err = mc_wrap_angle(desired_heading - state->heading_rad);
+    float desired_heading;
+    arm_atan2_f32(vec_y, vec_x, &desired_heading);
+    out->target_heading_deg = mc_math_rad_to_compass_deg(desired_heading);
+    float heading_err = mc_heading_error_with_hysteresis(state, desired_heading);
     float heading_err_rate = (heading_err - state->last_heading_err) / (dt_s > 0.0f ? dt_s : 0.013f);
     state->last_heading_err = heading_err;
 
     float cross_track = mc_cross_track_error(state, nearest_idx);
-    float yaw_rate_cmd = state->config.heading_kp * heading_err
-                       + state->config.heading_kd * heading_err_rate
-                       + state->config.xtrack_kp * cross_track;
+    float p_term = state->config.heading_kp * heading_err;
+    float d_term = state->config.heading_kd * heading_err_rate;
+    float x_term = state->config.xtrack_kp * cross_track;
+    
+    float yaw_rate_cmd = p_term + d_term + x_term;
     yaw_rate_cmd = MC_CLAMP(yaw_rate_cmd, -state->config.max_turn_rate, state->config.max_turn_rate);
-
+    
+    /* 璋冭瘯LOG锛氭瘡10甯ф墦鍗颁竴娆ID鎺у埗璇︽儏锛屽府鍔╁垎鏋愭鍖�/涓嶈浆鍚戦棶棰� - 宸茬Щ闄� */
+    
     float forward = state->config.base_speed_mps
                   - state->config.heading_speed_scale * fabsf(heading_err)
                   - state->config.xtrack_speed_scale * fabsf(cross_track);
@@ -287,7 +422,7 @@
     _cfg->xtrack_speed_scale = MC_CFG_XTRACK_SPEED_SCALE;
 }
 
-/* 鍒濆鍖栨帶鍒跺櫒锛岀粦瀹氳矾寰勪笌閰嶇疆 */
+/* MC_Init */
 HIDO_VOID MC_Init(MC_State *_state, const MC_Config *_cfg, const float *_path_xy, HIDO_UINT32 _point_count)
 {
     if (_state == HIDO_NULL || _cfg == HIDO_NULL || _path_xy == HIDO_NULL || _point_count < 2U)
@@ -315,17 +450,12 @@
     _state->pos[0] = _enu[0];
     _state->pos[1] = _enu[1];
     _state->pos[2] = _enu[2];
-    float gps_heading_rad = mc_wrap_angle((_gprmi->m_fHeadingAngle) * DEG2RAD);
-    if (_state->imu_valid == HIDO_TRUE)
-    {
-        float heading_err = mc_wrap_angle(gps_heading_rad - _state->heading_rad);
-        _state->heading_rad = mc_wrap_angle(_state->heading_rad + heading_err * MC_CFG_GPS_HEADING_BLEND);
-    }
-    else
-    {
-        _state->heading_rad = gps_heading_rad;
-    }
-    _state->heading_deg = _state->heading_rad * RAD2DEG;
+    float old_heading_deg = _state->heading_deg;
+    float gps_heading_rad = mc_compass_deg_to_math_rad(_gprmi->m_fHeadingAngle);
+    _state->heading_rad = gps_heading_rad;
+    _state->heading_deg = mc_math_rad_to_compass_deg(_state->heading_rad);
+    
+    float old_heading_rad = mc_compass_deg_to_math_rad(old_heading_deg);
     _state->pitch_deg = _gprmi->m_fPitchAngle;
     _state->roll_deg = _gprmi->m_fRollAngle;
     _state->vel[0] = _gprmi->m_fEastVelocity;
@@ -348,11 +478,16 @@
         return;
     }
 
-    _state->yaw_rate_rad = _gpimu->m_fGyroZ * DEG2RAD;
+    /* GyroZ绗﹀彿瀹氫箟锛氶『鏃堕拡姝ｏ紝閫嗘椂閽堣礋锛堜粠涓婂線涓嬬湅Z杞达紝鍙虫墜铻烘棆瀹氬垯锛�
+     * 鏁板鍧愭爣绯伙細涓滀负0掳锛岄�嗘椂閽堜负姝ｏ紙CCW>0锛�
+     * 鎵�浠ワ細GyroZ姝ｅ�硷紙椤烘椂閽堬級鈫� yaw_rate_rad璐熷�硷紝GyroZ璐熷�硷紙閫嗘椂閽堬級鈫� yaw_rate_rad姝ｅ��
+     * 闇�瑕佸彇鍙嶏細yaw_rate_rad = -GyroZ * DEG2RAD */
+    _state->yaw_rate_rad = -(_gpimu->m_fGyroZ * DEG2RAD);
     _state->imu_valid = HIDO_TRUE;
+    
 }
 
-/* 鍏ュ彛锛氭牴鎹樁娈佃绠楁帶鍒堕噺 */
+/* 鍏ュ彛锛氭牴鎹樁娈佃绠楁帶鍒堕噺 */ 
 HIDO_VOID MC_Compute(MC_State *_state, float _dt_s, MC_Output *_out)
 {
     if (_state == HIDO_NULL || _out == HIDO_NULL)
@@ -365,20 +500,40 @@
     _out->target_xy[0] = 0.0f;
     _out->target_xy[1] = 0.0f;
 
+    /* 璁板綍target_valid鍙樹负FALSE鐨勫師鍥� */
+    static HIDO_BOOL s_last_path_valid = HIDO_TRUE;
+    static HIDO_BOOL s_last_pose_valid = HIDO_TRUE;
+    
     if (_state->path_xy == HIDO_NULL || _state->path_count < 2U)
     {
+        if (s_last_path_valid)
+        {
+            s_last_path_valid = HIDO_FALSE;
+        }
         return;
     }
+    s_last_path_valid = HIDO_TRUE;
 
     if (_state->pose_valid == HIDO_FALSE)
     {
+        if (s_last_pose_valid)
+        {
+            s_last_pose_valid = HIDO_FALSE;
+        }
         return;
     }
+    s_last_pose_valid = HIDO_TRUE;
 
     if (_state->imu_valid == HIDO_TRUE && _dt_s > 0.0f)
     {
-        _state->heading_rad = mc_wrap_angle(_state->heading_rad + _state->yaw_rate_rad * _dt_s);
-        _state->heading_deg = _state->heading_rad * RAD2DEG;
+        float old_heading_rad = _state->heading_rad;
+        float old_heading_deg = _state->heading_deg;
+        float delta_rad = _state->yaw_rate_rad * _dt_s;
+        _state->heading_rad = mc_wrap_angle(_state->heading_rad + delta_rad);
+        float new_heading_rad = _state->heading_rad;
+        _state->heading_deg = mc_math_rad_to_compass_deg(_state->heading_rad);
+        
+        (void)new_heading_rad;
     }
 
     if (_state->stage == MC_STAGE_IDLE)
@@ -404,6 +559,9 @@
         break;
     }
 
+    /* 涓嶅啀鍙栧弽锛歽aw_rate_cmd锛堟暟瀛﹀潗鏍囩郴锛屾=CCW锛夌洿鎺ヤ綔涓簍urn_rate杈撳嚭
+     * motion_control_task.c涓紝姝ｅ��=宸﹁浆(閫嗘椂閽�)锛岃礋鍊�=鍙宠浆(椤烘椂閽�)锛屼笌鏁板鍧愭爣绯讳竴鑷� */
+    // _out->turn_rate = -_out->turn_rate;  // 宸叉敞閲婏細姝ゅ彇鍙嶅鑷存柟鍚戦敊璇�
     _out->stage = _state->stage;
     _out->pos_enu[0] = _state->pos[0];
     _out->pos_enu[1] = _state->pos[1];
@@ -411,17 +569,31 @@
     _out->heading_deg = _state->heading_deg;
     _out->pitch_deg = _state->pitch_deg;
     _out->roll_deg = _state->roll_deg;
+    static HIDO_BOOL s_last_target_valid = HIDO_FALSE;
+    static E_MCStage s_last_stage_for_target = MC_STAGE_IDLE;
+    
     if (_state->stage == MC_STAGE_FOLLOW_PATH || _state->stage == MC_STAGE_GOTO_START)
     {
         _out->target_valid = HIDO_TRUE;
         _out->target_xy[0] = _state->current_target_xy[0];
         _out->target_xy[1] = _state->current_target_xy[1];
+        
+        if (!s_last_target_valid)
+        {
+        }
     }
     else
     {
         _out->target_valid = HIDO_FALSE;
         _out->target_xy[0] = 0.0f;
         _out->target_xy[1] = 0.0f;
+        
+        if (s_last_target_valid || _state->stage != s_last_stage_for_target)
+        {
+        }
     }
+    
+    s_last_target_valid = _out->target_valid;
+    s_last_stage_for_target = _state->stage;
 }
 

--
Gitblit v1.10.0