From 8d0d8dec18cc9170f3fb82a4dba18160dd8e5233 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期三, 10 十二月 2025 23:24:59 +0800
Subject: [PATCH] 还原到路径第一次调通的版本，
---
STM32H743/FML/motion_control.c | 100 +++++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 99 insertions(+), 1 deletions(-)
diff --git a/STM32H743/FML/motion_control.c b/STM32H743/FML/motion_control.c
index 3ceff35..3658ba2 100644
--- a/STM32H743/FML/motion_control.c
+++ b/STM32H743/FML/motion_control.c
@@ -67,6 +67,14 @@
/* 璇诲彇璺緞鐐癸紝绱㈠紩瓒婄晫鏃舵埅鍒版湯灏� */
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;
@@ -231,6 +239,26 @@
{
float start_point[2];
mc_get_point(state, 0U, start_point);
+
+ /* 妫�娴嬬洰鏍囩偣鍙樺寲骞惰褰� */
+ static HIDO_UINT32 s_goto_log_idx = 0U;
+ HIDO_BOOL target_changed = (fabsf(state->current_target_xy[0] - start_point[0]) > 0.01f) ||
+ (fabsf(state->current_target_xy[1] - start_point[1]) > 0.01f);
+
+ if (target_changed || (s_goto_log_idx++ % 50U) == 0U)
+ {
+ int tgt_x_int = (int)start_point[0];
+ int tgt_x_frac = (int)(fabsf(start_point[0] - tgt_x_int) * 100);
+ int tgt_y_int = (int)start_point[1];
+ int tgt_y_frac = (int)(fabsf(start_point[1] - tgt_y_int) * 100);
+
+ if (target_changed)
+ {
+ HIDO_Debug2("[MC_TGT]GOTO_START target changed: (%d.%02d,%d.%02d)\r\n",
+ tgt_x_int, tgt_x_frac, tgt_y_int, tgt_y_frac);
+ }
+ }
+
state->current_target_xy[0] = start_point[0];
state->current_target_xy[1] = start_point[1];
out->target_valid = HIDO_TRUE;
@@ -260,7 +288,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;
@@ -353,6 +383,28 @@
float target[2];
mc_get_point(state, lookahead_idx, target);
+
+ /* 妫�娴嬬洰鏍囩偣鍙樺寲骞惰褰� */
+ HIDO_BOOL target_changed = (fabsf(state->current_target_xy[0] - target[0]) > 0.01f) ||
+ (fabsf(state->current_target_xy[1] - target[1]) > 0.01f);
+
+ if (target_changed)
+ {
+ int old_x_int = (int)state->current_target_xy[0];
+ int old_x_frac = (int)(fabsf(state->current_target_xy[0] - old_x_int) * 100);
+ int old_y_int = (int)state->current_target_xy[1];
+ int old_y_frac = (int)(fabsf(state->current_target_xy[1] - old_y_int) * 100);
+ int new_x_int = (int)target[0];
+ int new_x_frac = (int)(fabsf(target[0] - new_x_int) * 100);
+ int new_y_int = (int)target[1];
+ int new_y_frac = (int)(fabsf(target[1] - new_y_int) * 100);
+
+ HIDO_Debug2("[MC_TGT]FOLLOW target changed: (%d.%02d,%d.%02d)->(%d.%02d,%d.%02d) near=%u look=%u\r\n",
+ old_x_int, old_x_frac, old_y_int, old_y_frac,
+ new_x_int, new_x_frac, new_y_int, new_y_frac,
+ nearest_idx, lookahead_idx);
+ }
+
state->current_target_xy[0] = target[0];
state->current_target_xy[1] = target[1];
out->target_valid = HIDO_TRUE;
@@ -427,6 +479,8 @@
{
if (_state == HIDO_NULL || _cfg == HIDO_NULL || _path_xy == HIDO_NULL || _point_count < 2U)
{
+ DBG_Printf("[MC_Init] ERROR: Invalid parameters (state=%p, cfg=%p, path=%p, count=%u)\r\n",
+ (void*)_state, (void*)_cfg, (void*)_path_xy, _point_count);
return;
}
@@ -437,6 +491,9 @@
_state->stage = MC_STAGE_GOTO_START;
_state->nearest_index = 0U;
_state->lookahead_index = 0U;
+
+ DBG_Printf("[MC_Init] OK: path_count=%u, first_point=(%.2f,%.2f)\r\n",
+ _point_count, _path_xy[0], _path_xy[1]);
}
/* 娉ㄥ叆鏈�鏂� ENU 浣嶅Э鍙婅埅鍚�/閫熷害 */
@@ -500,15 +557,32 @@
_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)
+ {
+ HIDO_Debug2("[MC_TGT]WARNING: target_valid=FALSE, path invalid (path=%p, count=%u)\r\n",
+ (void*)_state->path_xy, _state->path_count);
+ s_last_path_valid = HIDO_FALSE;
+ }
return;
}
+ s_last_path_valid = HIDO_TRUE;
if (_state->pose_valid == HIDO_FALSE)
{
+ if (s_last_pose_valid)
+ {
+ HIDO_Debug2("[MC_TGT]WARNING: target_valid=FALSE, pose_valid=FALSE\r\n");
+ s_last_pose_valid = HIDO_FALSE;
+ }
return;
}
+ s_last_pose_valid = HIDO_TRUE;
if (_state->imu_valid == HIDO_TRUE && _dt_s > 0.0f)
{
@@ -545,7 +619,9 @@
break;
}
- _out->turn_rate = -_out->turn_rate;
+ /* 涓嶅啀鍙栧弽锛歽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];
@@ -553,17 +629,39 @@
_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)
+ {
+ int x_int = (int)_out->target_xy[0];
+ int x_frac = (int)(fabsf(_out->target_xy[0] - x_int) * 100);
+ int y_int = (int)_out->target_xy[1];
+ int y_frac = (int)(fabsf(_out->target_xy[1] - y_int) * 100);
+ HIDO_Debug2("[MC_TGT]target_valid: FALSE->TRUE, stage=%d, target=(%d.%02d,%d.%02d)\r\n",
+ _state->stage, x_int, x_frac, y_int, y_frac);
+ }
}
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)
+ {
+ HIDO_Debug2("[MC_TGT]target_valid: TRUE->FALSE, stage=%d->%d (will output 0,0)\r\n",
+ s_last_stage_for_target, _state->stage);
+ }
}
+
+ s_last_target_valid = _out->target_valid;
+ s_last_stage_for_target = _state->stage;
}
--
Gitblit v1.10.0