From 427d0b6ee0966fcf19a75914a47360969e00e456 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期二, 25 十一月 2025 11:06:02 +0800
Subject: [PATCH] 路径跟随初步调通
---
STM32H743/FML/motion_control.c | 231 +++++++++++++++++++++++++++++----------------------------
1 files changed, 116 insertions(+), 115 deletions(-)
diff --git a/STM32H743/FML/motion_control.c b/STM32H743/FML/motion_control.c
index bf26309..3ceff35 100644
--- a/STM32H743/FML/motion_control.c
+++ b/STM32H743/FML/motion_control.c
@@ -55,10 +55,6 @@
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)
{
@@ -121,48 +117,62 @@
return mc_wrap_angle(math_deg * DEG2RAD);
}
-/* 鎼滅储褰撳墠浣嶇疆鏈�杩戠殑璺緞鐐� */
-static HIDO_UINT32 mc_find_nearest(const MC_State *state)
+/* 鍒ゆ柇杞﹁締鏄惁宸茬粡"鍒拌揪"鏌愪釜璺緞鐐�
+ * 鏍囧噯锛氳窛绂昏鐐瑰皬浜庨槇鍊硷紙0.5绫筹級鍗充负鍒拌揪 */
+static HIDO_BOOL mc_is_point_reached(const MC_State *state, HIDO_UINT32 point_idx)
{
- 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++)
+ if (point_idx >= state->path_count)
{
- 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;
- }
+ return HIDO_FALSE;
}
- return best_idx;
+
+ 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 next_idx;
}
/* 璁＄畻甯︾鍙锋í鍚戣宸紙宸︽鍙宠礋锛� */
@@ -230,54 +240,13 @@
float vec_y = start_point[1] - state->pos[1];
float dist = mc_distance(start_point, state->pos);
- /* 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)
{
@@ -305,14 +274,82 @@
/* 闃舵锛氱函杩借釜 + 鑸悜/妯悜璇樊琛ュ伩 */
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 = 0U;
+ static HIDO_UINT32 s_last_lookahead = 0U;
+
+ /* 濡傛灉绱㈠紩鍙戠敓鍙樺寲锛岀珛鍗宠緭鍑篖OG */
+ HIDO_BOOL index_changed = (nearest_idx != s_last_nearest) || (lookahead_idx != s_last_lookahead);
+
+ if ((s_path_log_idx++ % 20U) == 0U || index_changed == HIDO_TRUE)
+ {
+ float nearest_pt[2];
+ mc_get_point(state, nearest_idx, nearest_pt);
+ float dist_to_nearest = mc_distance(nearest_pt, state->pos);
+ HIDO_BOOL reached = mc_is_point_reached(state, nearest_idx);
+
+ /* 浣跨敤鏁存暟琛ㄧず娉曟墦鍗版诞鐐规暟 */
+ int pos_x_int = (int)state->pos[0];
+ int pos_x_frac = (int)(fabsf(state->pos[0] - pos_x_int) * 100);
+ int pos_y_int = (int)state->pos[1];
+ int pos_y_frac = (int)(fabsf(state->pos[1] - pos_y_int) * 100);
+ int near_x_int = (int)nearest_pt[0];
+ int near_x_frac = (int)(fabsf(nearest_pt[0] - near_x_int) * 100);
+ int near_y_int = (int)nearest_pt[1];
+ int near_y_frac = (int)(fabsf(nearest_pt[1] - near_y_int) * 100);
+ int dn_int = (int)dist_to_nearest;
+ int dn_frac = (int)((dist_to_nearest - dn_int) * 100);
+
+ if (index_changed == HIDO_TRUE)
+ {
+ HIDO_Debug2("[MC_PATH]CHG pos=(%d.%02d,%d.%02d) near=%u->%u look=%u->%u dn=%d.%02d reach=%d\r\n",
+ pos_x_int, pos_x_frac, pos_y_int, pos_y_frac,
+ s_last_nearest, nearest_idx,
+ s_last_lookahead, lookahead_idx,
+ dn_int, dn_frac, reached);
+ }
+ else
+ {
+ HIDO_Debug2("[MC_PATH] near=%u(%d.%02d,%d.%02d) look=%u dn=%d.%02d reach=%d\r\n",
+ nearest_idx, near_x_int, near_x_frac, near_y_int, near_y_frac,
+ lookahead_idx,
+ dn_int, dn_frac, reached);
+ }
+
+ s_last_nearest = nearest_idx;
+ s_last_lookahead = lookahead_idx;
+ }
float target[2];
mc_get_point(state, lookahead_idx, target);
@@ -337,16 +374,6 @@
+ 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)
- state->config.xtrack_speed_scale * fabsf(cross_track);
@@ -428,13 +455,7 @@
_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;
@@ -464,10 +485,6 @@
_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);
}
/* 鍏ュ彛锛氭牴鎹樁娈佃绠楁帶鍒堕噺 */
@@ -493,17 +510,6 @@
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;
@@ -513,12 +519,7 @@
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);
+ (void)new_heading_rad;
}
if (_state->stage == MC_STAGE_IDLE)
--
Gitblit v1.10.0