From 4adc78553c8d48ff122506195fa33641134bd7b1 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期六, 13 十二月 2025 18:55:14 +0800
Subject: [PATCH] 路径点不重复的控制算法测试通了，但是好像不如之前路径点重复的版本好。增加了蓝牙接口部分。准备移植外包的MQTT。
---
STM32H743/FML/motion_control.c | 275 ++++++++++++++++++++++++++++++------------------------
1 files changed, 153 insertions(+), 122 deletions(-)
diff --git a/STM32H743/FML/motion_control.c b/STM32H743/FML/motion_control.c
index bf26309..fdb4902 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)
{
@@ -71,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;
@@ -121,48 +125,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 start_idx;
}
/* 璁＄畻甯︾鍙锋í鍚戣宸紙宸︽鍙宠礋锛� */
@@ -173,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];
@@ -221,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;
@@ -230,54 +261,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)
{
@@ -291,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;
@@ -305,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;
@@ -332,21 +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);
- /* 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);
-
+ /* 璋冭瘯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);
@@ -395,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)
@@ -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);
}
/* 鍏ュ彛锛氭牴鎹樁娈佃绠楁帶鍒堕噺 */
@@ -483,26 +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;
}
-
- /* 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);
- }
+ s_last_pose_valid = HIDO_TRUE;
if (_state->imu_valid == HIDO_TRUE && _dt_s > 0.0f)
{
@@ -513,12 +533,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)
@@ -544,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];
@@ -552,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