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/APL/motion_control_task.c | 245 +++++++++++++++++++++++++++++++++++++++++++++---
1 files changed, 228 insertions(+), 17 deletions(-)
diff --git a/STM32H743/APL/motion_control_task.c b/STM32H743/APL/motion_control_task.c
index a694081..a16239c 100644
--- a/STM32H743/APL/motion_control_task.c
+++ b/STM32H743/APL/motion_control_task.c
@@ -83,6 +83,11 @@
&g_motion_config,
g_motion_path_xy,
g_motion_path_point_count);
+
+ DBG_Printf("[MC_INIT] Path loaded: count=%u, first_point=(%.2f,%.2f)\r\n",
+ g_motion_path_point_count,
+ g_motion_path_xy[0],
+ g_motion_path_xy[1]);
BaseType_t ret = xTaskCreate(
MotionControl_TaskEntry,
@@ -138,6 +143,12 @@
g_last_heading_deg = gprmi.m_fHeadingAngle;
g_last_pitch_deg = gprmi.m_fPitchAngle;
g_last_roll_deg = gprmi.m_fRollAngle;
+
+ /* 妫�娴媝ose_valid鐘舵�佸彉鍖� */
+ if (g_last_pose_valid == HIDO_FALSE)
+ {
+ HIDO_Debug2("[MC_POSE]pose_valid: FALSE->TRUE (GPS recovered)\r\n");
+ }
g_last_pose_valid = HIDO_TRUE;
g_last_gps_ms = now;
g_last_gprmi_tow = gps_timestamp;
@@ -146,6 +157,12 @@
}
else if (gps_valid == HIDO_FALSE && (now - g_last_gps_ms) > 200U)
{
+ /* 妫�娴媝ose_valid鐘舵�佸彉鍖� */
+ if (g_last_pose_valid == HIDO_TRUE)
+ {
+ HIDO_Debug2("[MC_POSE]pose_valid: TRUE->FALSE (GPS timeout, age=%ums)\r\n",
+ now - g_last_gps_ms);
+ }
g_motion_state.pose_valid = HIDO_FALSE;
g_last_pose_valid = HIDO_FALSE;
}
@@ -167,7 +184,40 @@
/* --- 2) 璋冪敤杩愬姩鎺у埗鍣紙鍥哄畾 75 Hz dt锛� --- */
MC_Output output;
HIDO_BOOL sbus_valid = (SBUS_IsSignalValid(MOTION_SBUS_TIMEOUT_MS) == HIDO_TRUE);
- HIDO_UINT16 ch8 = SBUS_GetChannel(MOTION_SBUS_AUTO_CHANNEL);
+ HIDO_UINT16 ch8_raw = SBUS_GetChannel(MOTION_SBUS_AUTO_CHANNEL);
+
+ /* 杩囨护SBUS寮傚父鍊硷細
+ * 1024 = SBUS_CENTER_VALUE锛堜俊鍙蜂涪澶辨椂鐨勯粯璁よ繑鍥炲�硷級
+ * <172 鎴� >1811 = SBUS鏈夋晥鑼冨洿涔嬪锛堝搴擯WM 1000-2000锛�
+ * 褰撴娴嬪埌寮傚父鍊兼椂锛屼繚鎸佷箣鍓嶇殑鏈夋晥鍊间笉鍙� */
+ static HIDO_UINT16 ch8 = 1000; /* 榛樿鎵嬪姩妯″紡 */
+ static HIDO_UINT32 s_ch8_failsafe_count = 0;
+ static HIDO_UINT16 s_ch8_last_valid = 1000;
+
+ /* 鍒ゆ柇鏄惁涓烘湁鏁圫BUS鍊硷細172-1811鑼冨洿鍐咃紝涓斾笉鏄簿纭殑1024 */
+ HIDO_BOOL is_valid = (ch8_raw >= 172 && ch8_raw <= 1811 && ch8_raw != 1024);
+
+ if (is_valid)
+ {
+ ch8 = ch8_raw; /* 鍙洿鏂版湁鏁堝�� */
+ s_ch8_last_valid = ch8_raw;
+ if (s_ch8_failsafe_count > 0)
+ {
+ DBG_Printf("[MC_CTRL] CH8 recovered from failsafe, count=%u, new value=%u\r\n",
+ s_ch8_failsafe_count, ch8);
+ s_ch8_failsafe_count = 0;
+ }
+ }
+ else
+ {
+ /* 寮傚父鍊硷紝淇濇寔涔嬪墠鐨勫�硷紝璁板綍failsafe浜嬩欢 */
+ s_ch8_failsafe_count++;
+ if (s_ch8_failsafe_count == 1)
+ {
+ DBG_Printf("[MC_CTRL] CH8 failsafe detected (%u), keeping previous value=%u\r\n",
+ ch8_raw, ch8);
+ }
+ }
/* 妫�鏌� GPS 鐘舵�侊細蹇呴』鍒濆鍖栧畬鎴愪笖杩炴帴鍒� GNSS */
HIDO_BOOL gps_ready = HIDO_FALSE;
@@ -208,29 +258,148 @@
#endif
/* 鍙湁褰撴弧瓒虫墍鏈夋潯浠舵椂鎵嶆墽琛岃嚜鍔ㄦ帶鍒讹細
-
- /* 鍙湁褰撴弧瓒虫墍鏈夋潯浠舵椂鎵嶆墽琛岃嚜鍔ㄦ帶鍒讹細
* 1. SBUS 淇″彿鏈夋晥
- * 2. CH8 > 闃堝�硷紙鑷姩妯″紡寮�鍏筹級
+ * 2. CH8 > 闃堝�硷紙鑷姩妯″紡寮�鍏筹紝甯﹁繜婊烇級
* 3. GPS 鍒濆鍖栧畬鎴愶紙FINIT_OK锛�
* 4. GPS 杩炴帴鍒� GNSS锛圙NSS_CONNECT锛�*/
static HIDO_BOOL s_last_auto_condition = HIDO_FALSE;
- HIDO_BOOL current_auto_condition = (sbus_valid == HIDO_TRUE && ch8 > MOTION_SBUS_AUTO_THRESHOLD_US && gps_ready == HIDO_TRUE);
+ static HIDO_BOOL s_last_sbus_valid = HIDO_FALSE;
+ static HIDO_BOOL s_last_gps_ready = HIDO_FALSE;
+ static HIDO_UINT16 s_last_ch8 = 0;
+ static HIDO_BOOL s_ch8_auto_state = HIDO_FALSE; /* CH8杩熸粸鐘舵�� */
+ static HIDO_BOOL s_last_ch8_auto_state = HIDO_FALSE; /* 涓婁竴娆＄殑CH8杩熸粸鐘舵�� */
+
+ /* CH8杩熸粸锛氶槻姝㈠湪闃堝�奸檮杩戞姈鍔ㄦ椂鍙嶅瑙﹀彂
+ * 杩涘叆鑷姩妯″紡锛欳H8 > 1600 (楂橀槇鍊�)
+ * 閫�鍑鸿嚜鍔ㄦā寮忥細CH8 < 1400 (浣庨槇鍊�)
+ * 姝诲尯锛�1400-1600锛屽湪姝ゅ尯闂翠繚鎸佷箣鍓嶇殑鐘舵��
+ *
+ * 杩炵画纭鏈哄埗锛氶渶瑕佽繛缁�3娆℃娴嬪埌鐩稿悓鐘舵�佹墠鍒囨崲
+ * 杩欐槸鏈�绋冲畾鐨勯槻鎶栨柟妗堬紝闃叉鐬�佸共鎵� */
+ #define CH8_THRESHOLD_ENTER 1600
+ #define CH8_THRESHOLD_EXIT 1400
+ #define CH8_CONFIRM_COUNT 3
+
+ static HIDO_UINT8 s_ch8_enter_count = 0; /* 杩炵画妫�娴嬪埌杩涘叆鑷姩妯″紡鐨勬鏁� */
+ static HIDO_UINT8 s_ch8_exit_count = 0; /* 杩炵画妫�娴嬪埌閫�鍑鸿嚜鍔ㄦā寮忕殑娆℃暟 */
+
+ if (s_ch8_auto_state == HIDO_FALSE)
+ {
+ /* 褰撳墠鍦ㄦ墜鍔ㄦā寮忥紝妫�鏌ユ槸鍚﹀簲璇ヨ繘鍏ヨ嚜鍔ㄦā寮� */
+ if (ch8 > CH8_THRESHOLD_ENTER)
+ {
+ s_ch8_enter_count++;
+ s_ch8_exit_count = 0; /* 閲嶇疆閫�鍑鸿鏁� */
+
+ if (s_ch8_enter_count >= CH8_CONFIRM_COUNT)
+ {
+ /* 杩炵画3娆＄‘璁わ紝鍒囨崲鍒拌嚜鍔ㄦā寮� */
+ s_ch8_auto_state = HIDO_TRUE;
+ s_ch8_enter_count = 0;
+ DBG_Printf("[MC_CTRL] CH8 state confirmed: MANUAL -> AUTO (ch8=%u)\r\n", ch8);
+ }
+ }
+ else
+ {
+ s_ch8_enter_count = 0; /* 鏈弧瓒虫潯浠讹紝閲嶇疆璁℃暟 */
+ }
+ }
+ else
+ {
+ /* 褰撳墠鍦ㄨ嚜鍔ㄦā寮忥紝妫�鏌ユ槸鍚﹀簲璇ラ��鍑鸿嚜鍔ㄦā寮� */
+ if (ch8 < CH8_THRESHOLD_EXIT)
+ {
+ s_ch8_exit_count++;
+ s_ch8_enter_count = 0; /* 閲嶇疆杩涘叆璁℃暟 */
+
+ if (s_ch8_exit_count >= CH8_CONFIRM_COUNT)
+ {
+ /* 杩炵画3娆＄‘璁わ紝鍒囨崲鍒版墜鍔ㄦā寮� */
+ s_ch8_auto_state = HIDO_FALSE;
+ s_ch8_exit_count = 0;
+ DBG_Printf("[MC_CTRL] CH8 state confirmed: AUTO -> MANUAL (ch8=%u)\r\n", ch8);
+ }
+ }
+ else
+ {
+ s_ch8_exit_count = 0; /* 鏈弧瓒虫潯浠讹紝閲嶇疆璁℃暟 */
+ }
+ }
+
+ HIDO_BOOL current_auto_condition = (sbus_valid == HIDO_TRUE && s_ch8_auto_state == HIDO_TRUE && gps_ready == HIDO_TRUE);
- /* 妫�娴嬪埌浠庢墜鍔ㄥ垏鎹㈠埌鑷姩妯″紡鐨勪笂鍗囨部锛氶噸缃姸鎬佹満浠庡ご寮�濮� */
+ /* 妫�娴嬪埌浠庢墜鍔ㄥ垏鎹㈠埌鑷姩妯″紡鐨勪笂鍗囨部锛氶噸缃姸鎬佹満浠庡ご寮�濮�
+ * 澧炲姞绋冲畾鎬ц姹傦細鍙湁鍦↖DLE鎴朏INISHED鐘舵�佹椂鎵嶅厑璁搁噸鏂板垵濮嬪寲
+ * 濡傛灉姝ｅ湪鎵ц浠诲姟锛圙OTO_START鎴朏OLLOW_PATH锛夛紝蹇界暐妯″紡鍒囨崲鎶栧姩 */
if (s_last_auto_condition == HIDO_FALSE && current_auto_condition == HIDO_TRUE)
{
- DBG_Printf("[MC_CTRL] Auto mode triggered! Resetting to GOTO_START.\r\n");
- MC_Init(&g_motion_state,
- &g_motion_config,
- g_motion_path_xy,
- g_motion_path_point_count);
+ E_MCStage current_stage = g_motion_state.stage;
+
+ if (current_stage == MC_STAGE_IDLE || current_stage == MC_STAGE_FINISHED)
+ {
+ DBG_Printf("[MC_CTRL] *** Auto mode triggered! Resetting to GOTO_START ***\r\n");
+ DBG_Printf("[MC_CTRL] SBUS: %d->%d, CH8: %u->%u (state=%d->%d), GPS_READY: %d->%d\r\n",
+ s_last_sbus_valid, sbus_valid,
+ s_last_ch8, ch8,
+ s_last_ch8_auto_state, s_ch8_auto_state,
+ s_last_gps_ready, gps_ready);
+ MC_Init(&g_motion_state,
+ &g_motion_config,
+ g_motion_path_xy,
+ g_motion_path_point_count);
+ }
+ else
+ {
+ /* 姝ｅ湪鎵ц浠诲姟锛屽拷鐣ユ娆℃ā寮忓垏鎹紙鍙兘鏄俊鍙锋姈鍔級 */
+ DBG_Printf("[MC_CTRL] WARNING: Auto mode re-triggered during %d, ignoring (SBUS interference)\r\n",
+ current_stage);
+ }
}
+
+ /* 妫�娴嬫潯浠跺彉鍖栧苟璁板綍 */
+ if (s_last_sbus_valid != sbus_valid)
+ {
+ DBG_Printf("[MC_CTRL] SBUS valid changed: %d -> %d\r\n", s_last_sbus_valid, sbus_valid);
+ }
+ if (s_last_gps_ready != gps_ready)
+ {
+ DBG_Printf("[MC_CTRL] GPS ready changed: %d -> %d\r\n", s_last_gps_ready, gps_ready);
+ }
+
+ /* 璁板綍鐘舵�佸彉鍖栵紙宸插湪涓婇潰杩炵画纭鏃惰褰曪紝杩欓噷鍙洿鏂發ast鐘舵�侊級 */
+ s_last_ch8_auto_state = s_ch8_auto_state;
+
s_last_auto_condition = current_auto_condition;
+ s_last_sbus_valid = sbus_valid;
+ s_last_gps_ready = gps_ready;
+ s_last_ch8 = ch8;
if (current_auto_condition == HIDO_TRUE)
{
+ /* 璁板綍鐘舵�佸垏鎹� */
+ static E_MCStage s_last_stage = MC_STAGE_IDLE;
+ E_MCStage prev_stage = g_motion_state.stage;
+
MC_Compute(&g_motion_state, 1.0f / MC_CFG_CONTROL_HZ, &output);
+
+ if (prev_stage != output.stage)
+ {
+ const HIDO_CHAR *prev_label = MotionControl_StageLabel(prev_stage);
+ const HIDO_CHAR *curr_label = MotionControl_StageLabel(output.stage);
+ DBG_Printf("[MC_CTRL] *** Stage transition: %s -> %s ***\r\n", prev_label, curr_label);
+
+ if (output.stage == MC_STAGE_GOTO_START && prev_stage == MC_STAGE_FOLLOW_PATH)
+ {
+ /* 寮傚父锛氫粠follow_path鍥炲埌goto_start锛佽褰曡缁嗕俊鎭� */
+ DBG_Printf("[MC_CTRL] WARNING: Unexpected transition from FOLLOW_PATH to GOTO_START!\r\n");
+ DBG_Printf("[MC_CTRL] nearest_idx=%u, path_count=%u, pos=(%.2f,%.2f)\r\n",
+ g_motion_state.nearest_index,
+ g_motion_state.path_count,
+ g_motion_state.pos[0],
+ g_motion_state.pos[1]);
+ }
+ }
+ s_last_stage = output.stage;
}
else
{
@@ -278,11 +447,14 @@
int tgt_y_int = (int)g_motion_state.current_target_xy[1];
int tgt_y_frac = (int)(fabsf(g_motion_state.current_target_xy[1] - tgt_y_int) * 100);
int fwd_int = (int)output.forward_mps;
- int fwd_frac = (int)(fabsf(output.forward_mps - fwd_int) * 100);
- int turn_int = (int)output.turn_rate;
- int turn_frac = (int)(fabsf(output.turn_rate - turn_int) * 100);
+ int fwd_frac = (int)(fabsf(output.forward_mps - (float)fwd_int) * 100.0f);
+ /* 淇锛氫繚鐣檛urn鐨勭鍙凤紝浣跨敤甯︾鍙锋牸寮忚緭鍑� */
+ float turn_abs = fabsf(output.turn_rate);
+ int turn_int = (int)turn_abs;
+ int turn_frac = (int)((turn_abs - (float)turn_int) * 100.0f);
+ char turn_sign = (output.turn_rate < 0.0f) ? '-' : '+';
- HIDO_Debug2("[MC_CTRL] stage=%d pos=(%d.%02d,%d.%02d,%d.%02d) hdg=%d.%02d tgt=(%d.%02d,%d.%02d) fwd=%d.%02d turn=%d.%02d path_idx=%u\n",
+ HIDO_Debug2("[MC_CTRL] stage=%d pos=(%d.%02d,%d.%02d,%d.%02d) hdg=%d.%02d tgt=(%d.%02d,%d.%02d) fwd=%d.%02d turn=%c%d.%02d path_idx=%u\n",
g_motion_state.stage,
pos_x_int, pos_x_frac,
pos_y_int, pos_y_frac,
@@ -291,7 +463,7 @@
tgt_x_int, tgt_x_frac,
tgt_y_int, tgt_y_frac,
fwd_int, fwd_frac,
- turn_int, turn_frac,
+ turn_sign, turn_int, turn_frac,
g_motion_state.nearest_index);
}
@@ -355,11 +527,50 @@
g_last_pitch_deg = output.pitch_deg;
g_last_roll_deg = output.roll_deg;
g_last_pose_valid = g_motion_state.pose_valid;
- g_last_target_valid = output.target_valid;
+
+ /* 妫�娴嬬洰鏍囩偣璺冲彉鍒�(0,0)鐨勬儏鍐� */
+ static HIDO_BOOL s_warn_zero_target = HIDO_FALSE;
if (output.target_valid == HIDO_TRUE)
{
+ /* 妫�娴嬬洰鏍囩偣绐佺劧璺冲埌鎺ヨ繎(0,0)鐨勬儏鍐� */
+ HIDO_BOOL is_near_zero = (fabsf(output.target_xy[0]) < 0.1f && fabsf(output.target_xy[1]) < 0.1f);
+ HIDO_BOOL was_far_from_zero = (fabsf(g_last_target_xy[0]) > 1.0f || fabsf(g_last_target_xy[1]) > 1.0f);
+
+ if (is_near_zero && was_far_from_zero && g_last_target_valid)
+ {
+ int old_x_int = (int)g_last_target_xy[0];
+ int old_x_frac = (int)(fabsf(g_last_target_xy[0] - old_x_int) * 100);
+ int old_y_int = (int)g_last_target_xy[1];
+ int old_y_frac = (int)(fabsf(g_last_target_xy[1] - old_y_int) * 100);
+
+ HIDO_Debug2("[MC_TGT]WARNING: Target jumped to (0,0)! Previous target=(%d.%02d,%d.%02d) stage=%d\r\n",
+ old_x_int, old_x_frac, old_y_int, old_y_frac, output.stage);
+ s_warn_zero_target = HIDO_TRUE;
+ }
+ else if (!is_near_zero && s_warn_zero_target)
+ {
+ int new_x_int = (int)output.target_xy[0];
+ int new_x_frac = (int)(fabsf(output.target_xy[0] - new_x_int) * 100);
+ int new_y_int = (int)output.target_xy[1];
+ int new_y_frac = (int)(fabsf(output.target_xy[1] - new_y_int) * 100);
+
+ HIDO_Debug2("[MC_TGT]Target recovered from (0,0) to (%d.%02d,%d.%02d)\r\n",
+ new_x_int, new_x_frac, new_y_int, new_y_frac);
+ s_warn_zero_target = HIDO_FALSE;
+ }
+
g_last_target_xy[0] = output.target_xy[0];
g_last_target_xy[1] = output.target_xy[1];
+ g_last_target_valid = HIDO_TRUE;
+ }
+ else
+ {
+ /* target_valid鍙樹负FALSE */
+ if (g_last_target_valid)
+ {
+ HIDO_Debug2("[MC_TGT]target_valid: TRUE->FALSE in task (will report 0,0)\r\n");
+ }
+ g_last_target_valid = HIDO_FALSE;
}
g_freq_sample_count++;
--
Gitblit v1.10.0