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 | 118 +++++++++++++++++++++++++++++++++++++----------------------
1 files changed, 74 insertions(+), 44 deletions(-)
diff --git a/STM32H743/FML/motion_control.c b/STM32H743/FML/motion_control.c
index 3ceff35..fdb4902 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;
@@ -172,7 +180,7 @@
HIDO_UINT32 next_idx = start_idx + 1U;
/* 绠�鍖栭�昏緫锛氱洿鎺ヨ繑鍥炰笅涓�涓偣锛岀‘淇濇寜椤哄簭璁块棶姣忎釜鑸偣 */
- return next_idx;
+ return start_idx;
}
/* 璁＄畻甯︾鍙锋í鍚戣宸紙宸︽鍙宠礋锛� */
@@ -183,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];
@@ -231,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;
@@ -260,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;
@@ -306,53 +329,26 @@
/* 璋冭瘯锛氭瘡20甯ц緭鍑轰竴娆¤矾寰勮窡韪姸鎬� */
static HIDO_UINT32 s_path_log_idx = 0U;
- static HIDO_UINT32 s_last_nearest = 0U;
- static HIDO_UINT32 s_last_lookahead = 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 ((s_path_log_idx++ % 20U) == 0U || index_changed == HIDO_TRUE)
+ if (index_changed)
{
- 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);
+
+ /* 妫�娴嬬洰鏍囩偣鍙樺寲骞惰褰� - 宸茬Щ闄OG */
+
state->current_target_xy[0] = target[0];
state->current_target_xy[1] = target[1];
out->target_valid = HIDO_TRUE;
@@ -369,11 +365,15 @@
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);
@@ -422,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)
@@ -500,15 +500,29 @@
_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)
{
@@ -545,7 +559,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 +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