From 275b03224aa6170d4dc8c661c1cd949dd88c1fcb Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期一, 24 十一月 2025 15:10:21 +0800
Subject: [PATCH] 转向问题解决，方向定义问题，开始走了

---
 STM32H743/FML/motion_control.c |  177 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++--
 1 files changed, 169 insertions(+), 8 deletions(-)

diff --git a/STM32H743/FML/motion_control.c b/STM32H743/FML/motion_control.c
index b5ed3ee..bf26309 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,6 +37,37 @@
     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;
+        
+        /* DEBUG: 杈撳嚭婊炲悗閫昏緫瑙﹀彂 */
+        extern void HIDO_Debug2(const char *format, ...);
+        HIDO_Debug2("[MC_DBG] HYSTERESIS: pi_delta=%.4f old_err=%.4f new_err=%.4f sign=%.1f\r\n",
+                    pi_delta, old_err, err, 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])
 {
@@ -66,6 +99,28 @@
     return dist;
 }
 
+/* 鏁板鍧愭爣锛堜笢涓� 0掳锛岄�嗘椂閽堜负姝ｏ級鈫� 缃楃洏鍧愭爣锛堝寳涓� 0掳锛岄『鏃堕拡涓烘锛� */
+static float mc_math_rad_to_compass_deg(float heading_rad)
+{
+    float compass_deg = 90.0f - (heading_rad * RAD2DEG);
+    while (compass_deg >= 360.0f)
+    {
+        compass_deg -= 360.0f;
+    }
+    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);
+}
+
 /* 鎼滅储褰撳墠浣嶇疆鏈�杩戠殑璺緞鐐� */
 static HIDO_UINT32 mc_find_nearest(const MC_State *state)
 {
@@ -175,10 +230,53 @@
     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);
-    float heading_err = mc_wrap_angle(desired_heading - state->heading_rad);
+    /* DEBUG: 鍦ㄨ皟鐢╝tan2鍓嶈緭鍑哄弬鏁� */
+    extern void HIDO_Debug2(const char *format, ...);
+    HIDO_Debug2("[MC_DBG] ATAN2_IN: vec_y=%.6f vec_x=%.6f\r\n", vec_y, vec_x);
+    
+    /* 鐩存帴璋冪敤atan2f娴嬭瘯 */
+    extern float atan2f(float y, float x);
+    float test_direct = atan2f(vec_y, vec_x);
+    HIDO_Debug2("[MC_DBG] Direct atan2f(vec_y, vec_x) = %.6f\r\n", test_direct);
+    
+    /* 浣跨敤鏂扮殑DSP搴揂PI: arm_atan2_f32(y, x, result) */
+    float desired_heading;
+    arm_status status = arm_atan2_f32(vec_y, vec_x, &desired_heading);
+    if (status != ARM_MATH_SUCCESS) {
+        HIDO_Debug2("[MC_DBG] WARNING: arm_atan2_f32 failed with status %d\r\n", status);
+    }
+    
+    /* DEBUG: 娴嬭瘯涓嶅悓鐨勮緭鍑烘柟寮� */
+    float test_val = desired_heading;
+    int int_part = (int)test_val;
+    float frac_part = (test_val - (float)int_part) * 1000.0f;
+    HIDO_Debug2("[MC_DBG] ATAN2_OUT: arm_atan2_f32=%d.%03d rad\r\n", 
+                int_part, (int)frac_part);
+    HIDO_Debug2("[MC_DBG] ATAN2_OUT: in degrees = %.2f\r\n", 
+                desired_heading * RAD2DEG);
+    
+    /* 娴嬭瘯宸茬煡鍊� */
+    float test_atan2;
+    arm_atan2_f32(1.0f, 1.0f, &test_atan2);
+    float test_direct2 = atan2f(1.0f, 1.0f);
+    HIDO_Debug2("[MC_DBG] TEST: arm_atan2(1,1)=%.6f atan2f(1,1)=%.6f (should be ~0.785)\r\n", 
+                test_atan2, test_direct2);
+    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);
+    
+    /* DEBUG: 杈撳嚭goto_start鎺у埗璁＄畻 */
+    extern void HIDO_Debug2(const char *format, ...);
+    HIDO_Debug2("[MC_DBG] GOTO: pos=(%.2f,%.2f) tgt=(%.2f,%.2f) vec=(%.2f,%.2f) des_hdg_rad=%.4f\r\n",
+                state->pos[0], state->pos[1], start_point[0], start_point[1],
+                vec_x, vec_y, desired_heading);
+    HIDO_Debug2("[MC_DBG] GOTO: cur_hdg=%.2f tgt_hdg=%.2f err=%.4f turn_sign=%.1f yaw_cmd=%.4f\r\n",
+                mc_math_rad_to_compass_deg(state->heading_rad),
+                out->target_heading_deg,
+                heading_err,
+                state->last_turn_sign,
+                yaw_rate_cmd);
 
     float forward = 0.0f;
     if (dist > state->config.start_pos_tolerance_m)
@@ -226,8 +324,10 @@
 
     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);
-    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;
 
@@ -236,6 +336,16 @@
                        + state->config.heading_kd * heading_err_rate
                        + state->config.xtrack_kp * cross_track;
     yaw_rate_cmd = MC_CLAMP(yaw_rate_cmd, -state->config.max_turn_rate, state->config.max_turn_rate);
+    
+    /* DEBUG: 杈撳嚭follow_path鎺у埗璁＄畻 */
+    extern void HIDO_Debug2(const char *format, ...);
+    HIDO_Debug2("[MC_DBG] FOLLOW: cur_hdg=%.2f tgt_hdg=%.2f err=%.4f turn_sign=%.1f yaw_cmd=%.4f xtrack=%.3f\r\n",
+                mc_math_rad_to_compass_deg(state->heading_rad),
+                out->target_heading_deg,
+                heading_err,
+                state->last_turn_sign,
+                yaw_rate_cmd,
+                cross_track);
 
     float forward = state->config.base_speed_mps
                   - state->config.heading_speed_scale * fabsf(heading_err)
@@ -313,8 +423,18 @@
     _state->pos[0] = _enu[0];
     _state->pos[1] = _enu[1];
     _state->pos[2] = _enu[2];
-    _state->heading_rad = mc_wrap_angle((_gprmi->m_fHeadingAngle) * DEG2RAD);
-    _state->heading_deg = _gprmi->m_fHeadingAngle;
+    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);
+    
+    /* DEBUG: 杈撳嚭GPS鏇存柊淇℃伅锛堝寘鍚姬搴﹀�间互渚胯皟璇曪級 */
+    extern void HIDO_Debug2(const char *format, ...);
+    float old_heading_rad = mc_compass_deg_to_math_rad(old_heading_deg);
+    HIDO_Debug2("[MC_DBG] GPS: old_hdg=%.2f(old_rad=%.6f) gps_hdg=%.2f(gps_rad=%.6f) new_hdg=%.2f(new_rad=%.6f)\r\n",
+                old_heading_deg, old_heading_rad,
+                _gprmi->m_fHeadingAngle, gps_heading_rad,
+                _state->heading_deg, _state->heading_rad);
     _state->pitch_deg = _gprmi->m_fPitchAngle;
     _state->roll_deg = _gprmi->m_fRollAngle;
     _state->vel[0] = _gprmi->m_fEastVelocity;
@@ -337,11 +457,20 @@
         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;
+    
+    /* DEBUG: 杈撳嚭IMU鏇存柊淇℃伅 */
+    extern void HIDO_Debug2(const char *format, ...);
+    HIDO_Debug2("[MC_DBG] IMU: GyroZ=%.2f deg/s, yaw_rate_rad=%.4f\r\n", 
+                _gpimu->m_fGyroZ, _state->yaw_rate_rad);
 }
 
-/* 鍏ュ彛锛氭牴鎹樁娈佃绠楁帶鍒堕噺 */
+/* 鍏ュ彛锛氭牴鎹樁娈佃绠楁帶鍒堕噺 */ 
 HIDO_VOID MC_Compute(MC_State *_state, float _dt_s, MC_Output *_out)
 {
     if (_state == HIDO_NULL || _out == HIDO_NULL)
@@ -364,11 +493,41 @@
         return;
     }
 
+    /* DEBUG: 杈撳嚭璁＄畻寮�濮嬬姸鎬侊紙姣�50娆¤緭鍑轰竴娆★紝閬垮厤鍒峰睆锛� */
+    static HIDO_UINT32 dbg_counter = 0;
+    dbg_counter++;
+    if (dbg_counter % 50 == 0)
+    {
+        extern void HIDO_Debug2(const char *format, ...);
+        HIDO_Debug2("[MC_DBG] COMPUTE: pose_valid=%d imu_valid=%d dt=%.4f hdg=%.2f yaw_rate=%.4f\r\n",
+                    _state->pose_valid, _state->imu_valid, _dt_s, 
+                    _state->heading_deg, _state->yaw_rate_rad);
+    }
+
+    if (_state->imu_valid == HIDO_TRUE && _dt_s > 0.0f)
+    {
+        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);
+        
+        /* DEBUG: 杈撳嚭绉垎杩囩▼锛堝寘鍚姬搴﹀�间互渚胯皟璇曪級 */
+        extern void HIDO_Debug2(const char *format, ...);
+        HIDO_Debug2("[MC_DBG] INTEG: dt=%.4f yaw_rate=%.4f delta_rad=%.6f old_rad=%.6f new_rad=%.6f old_hdg=%.2f new_hdg=%.2f delta_deg=%.2f\r\n",
+                    _dt_s, _state->yaw_rate_rad, delta_rad, old_heading_rad, new_heading_rad,
+                    old_heading_deg, _state->heading_deg, 
+                    _state->heading_deg - old_heading_deg);
+    }
+
     if (_state->stage == MC_STAGE_IDLE)
     {
         _state->stage = MC_STAGE_GOTO_START;
     }
 
+    _out->target_heading_deg = _state->heading_deg;
+
     switch (_state->stage)
     {
     case MC_STAGE_GOTO_START:
@@ -379,11 +538,13 @@
         break;
     case MC_STAGE_FINISHED:
         _out->active = HIDO_FALSE;
+        _out->target_heading_deg = _state->heading_deg;
         break;
     default:
         break;
     }
 
+    _out->turn_rate = -_out->turn_rate;
     _out->stage = _state->stage;
     _out->pos_enu[0] = _state->pos[0];
     _out->pos_enu[1] = _state->pos[1];

--
Gitblit v1.10.0