From 3c2021441490ae9b93f7a2ef0f379909b589edd9 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期六, 13 十二月 2025 18:53:16 +0800
Subject: [PATCH] 应该是拍视频的版本，外包在此基础上加了MQTT部分。
---
STM32H743/APL/motion_control_task.c | 322 +++++++++++++++++++++++++++++++++++++++++++++++++++--
1 files changed, 308 insertions(+), 14 deletions(-)
diff --git a/STM32H743/APL/motion_control_task.c b/STM32H743/APL/motion_control_task.c
index e21109c..a16239c 100644
--- a/STM32H743/APL/motion_control_task.c
+++ b/STM32H743/APL/motion_control_task.c
@@ -23,6 +23,7 @@
#include "pwm_ctrol.h"
#include "motion_mode.h"
#include "SBUS.h"
+#include "AppConfig.h"
#define RAD2DEG (57.29577951308232f)
@@ -82,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,
@@ -137,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;
@@ -145,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;
}
@@ -166,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;
@@ -177,15 +228,178 @@
HIDO_BOOL gnss_connected = ((status & IM23A_STATUS_GNSS_CONNECT) != 0U);
gps_ready = (init_ok == HIDO_TRUE) && (gnss_connected == HIDO_TRUE);
}
+
+#if HITL_SIMULATION
+ /* 纭欢鍦ㄧ幆浠跨湡妯″紡锛�
+ * 1. 寮哄埗 SBUS 淇″彿鏈夋晥
+ * 2. 妯℃嫙 CH8 閫氶亾锛氫笂鐢靛墠 10 绉掍负 1000 (鎵嬪姩)锛�10 绉掑悗鍙樹负 1800 (鑷姩)
+ * 杩欐牱鍙互娴嬭瘯浠庢墜鍔ㄥ垏鎹㈠埌鑷姩鐨勯�昏緫 (鐘舵�佹満澶嶄綅)
+ */
+ sbus_valid = HIDO_TRUE;
+
+ /* 浠跨湡妯″紡涓嬶紝鍙鏀跺埌杩囦竴娆℃湁鏁堟暟鎹紝灏辫涓� GPS 灏辩华锛岄伩鍏嶅洜涓烘爣蹇椾綅鏈疆浣嶅鑷翠笉鍚姩 */
+ if (gps_valid == HIDO_TRUE)
+ {
+ gps_ready = HIDO_TRUE;
+ }
+
+ static HIDO_UINT16 g_hitl_ch8 = 1000;
+ static HIDO_UINT32 s_hitl_start_ms = 0;
+ if (s_hitl_start_ms == 0)
+ {
+ s_hitl_start_ms = HAL_GetTick();
+ }
+
+ if (HAL_GetTick() - s_hitl_start_ms > 1000U)
+ {
+ g_hitl_ch8 = 1800; // 10绉掑悗鑷姩鍒囧叆鑷姩妯″紡
+ }
+ ch8 = g_hitl_ch8;
+#endif
/* 鍙湁褰撴弧瓒虫墍鏈夋潯浠舵椂鎵嶆墽琛岃嚜鍔ㄦ帶鍒讹細
* 1. SBUS 淇″彿鏈夋晥
- * 2. CH8 > 闃堝�硷紙鑷姩妯″紡寮�鍏筹級
+ * 2. CH8 > 闃堝�硷紙鑷姩妯″紡寮�鍏筹紝甯﹁繜婊烇級
* 3. GPS 鍒濆鍖栧畬鎴愶紙FINIT_OK锛�
* 4. GPS 杩炴帴鍒� GNSS锛圙NSS_CONNECT锛�*/
- if (sbus_valid == HIDO_TRUE && ch8 > MOTION_SBUS_AUTO_THRESHOLD_US && gps_ready == HIDO_TRUE)
+ static HIDO_BOOL s_last_auto_condition = HIDO_FALSE;
+ 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)
+ {
+ 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
{
@@ -193,6 +407,9 @@
memset(&output, 0, sizeof(MC_Output));
output.active = HIDO_FALSE;
+ /* 绉婚櫎杩欓噷鐨勫己鍒跺洖涓紝鏀逛负鍦ㄤ笅鏂规牴鎹叿浣撳師鍥犲鐞� */
+ // MotionControl_StopOutputs();
+
/* 瀹氭湡鎵撳嵃鐘舵�佹彁绀� */
static HIDO_UINT32 s_status_log = 0U;
if ((s_status_log++ % 100U) == 0U)
@@ -230,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,
@@ -243,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);
}
@@ -256,13 +476,48 @@
}
else
{
- if(ch8 > MOTION_SBUS_AUTO_THRESHOLD_US)
+ /* 闈炶嚜鍔ㄦā寮忎笅锛孧otionControl_StopOutputs() 宸插湪涓婇潰璋冪敤锛岃繖閲屼笉鍐嶉噸澶嶈缃�
+ * 濡傛灉闇�瑕佹墜鍔ㄦā寮忕洿閫氾紝搴旇鍦� SBUS 妯″潡澶勭悊锛屾垨鑰呭湪杩欓噷璇诲彇 SBUS 鎵嬪姩閫氶亾鍊煎苟杈撳嚭
+ * 鐩墠閫昏緫鏄細闈炶嚜鍔ㄦā寮忎笅锛岃繍鍔ㄦ帶鍒朵换鍔¤緭鍑哄洖涓紙瀹夊叏鍋滄锛夛紝鎵嬪姩鎺у埗鐢� SBUS 鍥炶皟鎴栧叾浠栨満鍒舵帴绠�
+ * 濡傛灉绯荤粺璁捐鏄� MotionControl 浠诲姟鍦ㄦ墜鍔ㄦā寮忎笅涔熻礋璐ｉ�忎紶閬ユ帶鍣ㄤ俊鍙凤紝鍒欓渶瑕佷慨鏀规澶勩��
+ * 鍋囪锛氭墜鍔ㄦā寮忎笅锛孲BUS 妯″潡鐨勪腑鏂洖璋冧細鐩存帴鎺у埗鐢垫満锛屾垨鑰呮湁鍏朵粬浠诲姟澶勭悊銆�
+ * 涓洪槻姝㈠啿绐侊紝杩欓噷浠呬繚鎸佽褰曘�備絾涓婇潰宸茬粡璋冪敤 StopOutputs 璁剧疆浜� PWM 涓轰腑鍊笺��
+ * 濡傛灉 SBUS 鐩存帴鎺у埗鐢垫満锛岃繖閲岀殑 StopOutputs 浼氫笌 SBUS 鍐茬獊鍚楋紵
+ * 閫氬父鍋氭硶锛氳嚜鍔ㄦā寮忎笅鏈换鍔℃帶鍒� PWM锛涙墜鍔ㄦā寮忎笅鏈换鍔′笉鎺у埗 PWM锛堟垨鑰呰緭鍑烘棤鏁堝�硷級銆�
+ * 浣� StopOutputs 鏄惧紡璁剧疆浜� 1500銆傚鏋� SBUS 涔熸槸璁剧疆 PWM锛屼細鏈夌珵浜夈��
+ *
+ * 淇锛氬鏋� ch8 < 1500 (鎵嬪姩妯″紡)锛屾湰浠诲姟涓嶅簲璇� Set_Motor_Pulse(1500)锛�
+ * 鍚﹀垯浼氳鐩� SBUS 鐨勬墜鍔ㄦ帶鍒朵俊鍙枫��
+ *
+ * 淇敼閫昏緫锛�
+ * 1. 鑷姩妯″紡 (current_auto_condition == TRUE): ApplyOutput -> Set_Pulse
+ * 2. 寮傚父鍋滄 (sbus_valid=FALSE 鎴� gps_ready=FALSE): StopOutputs -> Set_Pulse(1500)
+ * 3. 鎵嬪姩妯″紡 (sbus_valid=TRUE && ch8 < 1500): 涓嶆搷浣� PWM锛岃鏉冪粰鎵嬪姩鎺у埗閫昏緫
+ */
+
+ if (sbus_valid == HIDO_FALSE)
{
- MotionControl_StopOutputs();
- applied_steering = MC_CFG_PWM_CENTER_US;
- applied_throttle = MC_CFG_PWM_CENTER_US;
+ /* 閬ユ帶鍣ㄤ俊鍙蜂涪澶憋細寮哄埗鍋滆溅 */
+ MotionControl_StopOutputs();
+ applied_steering = MC_CFG_PWM_CENTER_US;
+ applied_throttle = MC_CFG_PWM_CENTER_US;
}
- /* 鏄惧紡鍥為��鍒版墜鍔ㄦā寮忥紙閬ユ帶鍣ㄧ洿閫氬湪 SBUS 妯″潡涓鐞嗭紝鎴栬�呭湪姝ゅ瀹屽叏涓嶈緭鍑猴級 */
+ else if (ch8 <= MOTION_SBUS_AUTO_THRESHOLD_US)
+ {
+ /* 鎵嬪姩妯″紡锛氫笉杈撳嚭 PWM锛屼粎璁板綍锛堝亣璁� SBUS 妯″潡鎴栧叾浠栭�昏緫鍦ㄦ帶鍒剁數鏈猴級
+ * 涓轰簡鍥炴樉姝ｇ‘锛岃繖閲岃祴鍊间负 1500 鎴栬鍙栧綋鍓� PWM锛堝鏋滆兘璇诲埌锛�
+ */
+ applied_steering = MC_CFG_PWM_CENTER_US;
+ applied_throttle = MC_CFG_PWM_CENTER_US;
+ /* 娉ㄦ剰锛氫笉瑕佽皟鐢� MotionControl_StopOutputs()锛屽惁鍒欎細鍦ㄦ澶勬妸鎵嬪姩娌归棬寮鸿褰掗浂 */
+ }
+ else if (gps_ready == HIDO_FALSE)
+ {
+ /* 鑷姩妯″紡寮�鍏虫墦寮�锛屼絾 GPS 鏈氨缁細瀹夊叏鍋滆溅 */
+ MotionControl_StopOutputs();
+ applied_steering = MC_CFG_PWM_CENTER_US;
+ applied_throttle = MC_CFG_PWM_CENTER_US;
+ }
}
g_last_steering_pwm = applied_steering;
g_last_throttle_pwm = applied_throttle;
@@ -272,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