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/APL/motion_control_task.c | 510 ++++++++++++++++++++++++++++++++++++++++++++++++++++----
1 files changed, 472 insertions(+), 38 deletions(-)
diff --git a/STM32H743/APL/motion_control_task.c b/STM32H743/APL/motion_control_task.c
index 8543cae..c822738 100644
--- a/STM32H743/APL/motion_control_task.c
+++ b/STM32H743/APL/motion_control_task.c
@@ -11,6 +11,9 @@
#include "stm32h7xx_hal.h"
#include <string.h>
+#include <math.h>
+
+#define ENABLE_MC_CTRL_LOG 1
#include "DBG.h"
#include "GPS.h"
@@ -20,6 +23,9 @@
#include "motion_control.h"
#include "motion_path_data.h"
#include "pwm_ctrol.h"
+#include "motion_mode.h"
+#include "SBUS.h"
+#include "AppConfig.h"
#define RAD2DEG (57.29577951308232f)
@@ -79,6 +85,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,
@@ -122,22 +133,38 @@
HIDO_BOOL imu_valid = (GPS_GetGPIMU(&gpimu) == HIDO_OK);
HIDO_UINT32 now = HAL_GetTick();
- if (gps_valid == HIDO_TRUE && gprmi.m_u32TimeOfWeek != g_last_gprmi_tow)
+ if (gps_valid == HIDO_TRUE)
{
- /* 鏂扮殑GPS鏁版嵁鍒版潵锛堟椂闂存埑鍙樺寲锛夛紝鎵嶆洿鏂拌埅鍚� */
- Geo_GprmiToENU(&gprmi, &g_motion_origin, enu);
- MC_UpdateGps(&g_motion_state, enu, &gprmi);
- memcpy(g_last_enu, enu, sizeof(enu));
- g_last_heading_deg = gprmi.m_fHeadingAngle;
- g_last_pitch_deg = gprmi.m_fPitchAngle;
- g_last_roll_deg = gprmi.m_fRollAngle;
- g_last_pose_valid = HIDO_TRUE;
- g_last_gps_ms = now;
- g_last_gprmi_tow = gprmi.m_u32TimeOfWeek;
- g_last_sensor_timestamp_ms = gprmi.m_u32TimeOfWeek;
+ HIDO_UINT32 gps_timestamp = (gprmi.m_u32UTCTime != 0U) ? gprmi.m_u32UTCTime : (HIDO_UINT32)now;
+ if ((gprmi.m_u32UTCTime == 0U) || (gps_timestamp != g_last_gprmi_tow))
+ {
+ /* 鏂扮殑GPS鏁版嵁鍒版潵锛堟椂闂存埑鍙樺寲锛夛紝鎵嶆洿鏂拌埅鍚� */
+ Geo_GprmiToENU(&gprmi, &g_motion_origin, enu);
+ MC_UpdateGps(&g_motion_state, enu, &gprmi);
+ memcpy(g_last_enu, enu, sizeof(enu));
+ 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;
+ g_last_sensor_timestamp_ms = gps_timestamp;
+ }
}
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;
}
@@ -158,7 +185,293 @@
/* --- 2) 璋冪敤杩愬姩鎺у埗鍣紙鍥哄畾 75 Hz dt锛� --- */
MC_Output output;
- MC_Compute(&g_motion_state, 1.0f / MC_CFG_CONTROL_HZ, &output);
+ HIDO_BOOL sbus_valid = (SBUS_IsSignalValid(MOTION_SBUS_TIMEOUT_MS) == HIDO_TRUE);
+ 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;
+ if (gps_valid == HIDO_TRUE)
+ {
+ HIDO_UINT32 status = gprmi.m_u32StatusFlags;
+ HIDO_BOOL init_ok = ((status & IM23A_STATUS_READY) != 0U);
+ 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 > 闃堝�硷紙鑷姩妯″紡寮�鍏筹紝甯﹁繜婊烇級
+ * 3. GPS 鍒濆鍖栧畬鎴愶紙FINIT_OK锛�
+ * 4. GPS 杩炴帴鍒� GNSS锛圙NSS_CONNECT锛�*/
+ 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
+ {
+ /* 鎵嬪姩妯″紡鎴栨潯浠朵笉婊¤冻锛氶噸缃緭鍑哄苟璁╁嚭鎺у埗鏉� */
+ 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)
+ {
+#if ENABLE_MC_CTRL_LOG
+ if (sbus_valid == HIDO_FALSE)
+ {
+ HIDO_Debug2("[MC_CTRL] Waiting: SBUS signal lost\r\n");
+ }
+ else if (ch8 <= MOTION_SBUS_AUTO_THRESHOLD_US)
+ {
+ HIDO_Debug2("[MC_CTRL] Waiting: CH8=%u (manual mode)\r\n", ch8);
+ }
+ else if (gps_ready == HIDO_FALSE)
+ {
+ HIDO_UINT32 status = gps_valid ? gprmi.m_u32StatusFlags : 0U;
+ HIDO_Debug2("[MC_CTRL] Waiting: GPS not ready (status=0x%08X)\r\n", status);
+ }
+#endif
+ }
+ }
+
+ static HIDO_UINT32 s_ctrl_log_idx = 0U;
+ if ((s_ctrl_log_idx++ % 10U) == 0U)
+ {
+#if ENABLE_MC_CTRL_LOG
+ /* 浣跨敤鏁存暟琛ㄧず娉曟墦鍗版诞鐐规暟锛岄伩鍏嶆爤鎹熷潖 */
+ int pos_x_int = (int)g_motion_state.pos[0];
+ int pos_x_frac = (int)(fabsf(g_motion_state.pos[0] - pos_x_int) * 100);
+ int pos_y_int = (int)g_motion_state.pos[1];
+ int pos_y_frac = (int)(fabsf(g_motion_state.pos[1] - pos_y_int) * 100);
+ int pos_z_int = (int)g_motion_state.pos[2];
+ int pos_z_frac = (int)(fabsf(g_motion_state.pos[2] - pos_z_int) * 100);
+ int hdg_int = (int)g_motion_state.heading_deg;
+ int hdg_frac = (int)(fabsf(g_motion_state.heading_deg - hdg_int) * 100);
+ int tgt_x_int = (int)g_motion_state.current_target_xy[0];
+ int tgt_x_frac = (int)(fabsf(g_motion_state.current_target_xy[0] - tgt_x_int) * 100);
+ 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 - (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=%c%d.%02d path_idx=%u\n",
+ g_motion_state.stage,
+ pos_x_int, pos_x_frac,
+ pos_y_int, pos_y_frac,
+ pos_z_int, pos_z_frac,
+ hdg_int, hdg_frac,
+ tgt_x_int, tgt_x_frac,
+ tgt_y_int, tgt_y_frac,
+ fwd_int, fwd_frac,
+ turn_sign, turn_int, turn_frac,
+ g_motion_state.nearest_index);
+#endif
+ }
/* --- 3) 鏍规嵁鎺у埗閲忔洿鏂� PWM锛屽苟鎶� forward/turn 鍥炰紶缁� Python --- */
HIDO_UINT16 applied_steering = MC_CFG_PWM_CENTER_US;
@@ -169,9 +482,55 @@
}
else
{
- MotionControl_StopOutputs();
- applied_steering = MC_CFG_PWM_CENTER_US;
- applied_throttle = MC_CFG_PWM_CENTER_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;
+ }
+ 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;
+ }
+ else if (output.stage == MC_STAGE_FINISHED)
+ {
+ /* 宸插埌杈剧粓鐐癸細鍋滄杈撳嚭 */
+ 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;
@@ -181,11 +540,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++;
@@ -256,39 +654,74 @@
}
}
-/* 灏嗘帶鍒惰緭鍑烘槧灏勪负 PWM锛屽苟鍙戝洖 Python 绔� */
+/* 灏嗘帶鍒惰緭鍑烘槧灏勪负 PWM锛堜娇鐢ㄦ牎鍑嗘ā鍨嬪弽瑙ｏ級*/
static void MotionControl_ApplyOutput(const MC_Output *output,
HIDO_UINT16 *_pu16Steering,
HIDO_UINT16 *_pu16Throttle)
{
- float forward_ratio = output->forward_mps / g_motion_config.max_forward_mps;
- float turn_ratio = output->turn_rate / g_motion_config.max_turn_rate;
- forward_ratio = MC_CLAMP(forward_ratio, -1.0f, 1.0f);
- turn_ratio = MC_CLAMP(turn_ratio, -1.0f, 1.0f);
-
- int32_t throttle = (int32_t)MC_CFG_PWM_CENTER_US + (int32_t)(forward_ratio * (float)MC_CFG_PWM_SPAN_US);
- int32_t steering = (int32_t)MC_CFG_PWM_CENTER_US + (int32_t)(turn_ratio * (float)MC_CFG_PWM_SPAN_US);
-
- if (throttle < 1000)
+ /* ========== 鍓嶅悜閫熷害 鈫� 娌归棬PWM锛堜娇鐢ㄦ牎鍑嗘ā鍨嬪弽瑙ｏ級========== */
+ /* 鏍″噯妯″瀷锛歷 = MC_CFG_FORWARD_K 脳 (1500 - pwm) + MC_CFG_FORWARD_BIAS
+ * 鍙嶈В锛歱wm = 1500 - (v - MC_CFG_FORWARD_BIAS) / MC_CFG_FORWARD_K
+ * 璇存槑锛�1000=鏈�澶у墠杩涳紝1500=鍋滄锛�2000=鏈�澶у悗閫�
+ */
+ float target_velocity = output->forward_mps;
+ float throttle_f = 1500.0f - (target_velocity - MC_CFG_FORWARD_BIAS) / MC_CFG_FORWARD_K;
+
+ /* 闄愬埗PWM鑼冨洿 */
+ if (throttle_f < 1000.0f)
{
- throttle = 1000;
+ throttle_f = 1000.0f;
}
- else if (throttle > 2000)
+ else if (throttle_f > 2000.0f)
{
- throttle = 2000;
+ throttle_f = 2000.0f;
}
-
- if (steering < 1000)
+
+ /* 姝诲尯澶勭悊锛氭帴杩�1500鏃剁洿鎺ヨ涓�1500 */
+ if (fabsf(throttle_f - 1500.0f) < (float)MC_CFG_FORWARD_DEADZONE_PWM)
{
- steering = 1000;
+ throttle_f = 1500.0f;
}
- else if (steering > 2000)
+
+ /* ========== 杞悜瑙掗�熷害 鈫� 杞悜PWM锛堜娇鐢ㄦ牎鍑嗘ā鍨嬪弽瑙ｏ級========== */
+ /* 鏍″噯妯″瀷锛毾塤left = MC_CFG_STEERING_K_LEFT 脳 (1500 - pwm) 褰� pwm < 1500 (宸﹁浆)
+ * 蠅_right = -MC_CFG_STEERING_K_RIGHT 脳 (pwm - 1500) 褰� pwm > 1500 (鍙宠浆)
+ * 鍙嶈В锛歱wm_left = 1500 - 蠅 / MC_CFG_STEERING_K_LEFT
+ * pwm_right = 1500 - 蠅 / MC_CFG_STEERING_K_RIGHT
+ * 璇存槑锛�1000=鏈�澶у乏杞紝1500=鐩磋锛�2000=鏈�澶у彸杞�
+ */
+ float target_yaw_rate = output->turn_rate; // rad/s锛屾鍊�=宸﹁浆锛岃礋鍊�=鍙宠浆
+ float steering_f = 1500.0f;
+
+ /* 璋冭瘯LOG锛氭墦鍗拌浆鍚戞槧灏勮繃绋嬶紙浣庨锛� */
+
+ if (target_yaw_rate > 0.001f) // 宸﹁浆锛堟瑙掗�熷害锛�
{
- steering = 2000;
+ steering_f = 1500.0f - target_yaw_rate / MC_CFG_STEERING_K_LEFT;
+ if (steering_f < 1000.0f)
+ {
+ steering_f = 1000.0f;
+ }
}
-
- Set_Motor_Pulse((uint32_t)throttle);
- Set_Steering_Pulse((uint32_t)steering);
+ else if (target_yaw_rate < -0.001f) // 鍙宠浆锛堣礋瑙掗�熷害锛�
+ {
+ steering_f = 1500.0f - target_yaw_rate / MC_CFG_STEERING_K_RIGHT;
+ if (steering_f > 2000.0f)
+ {
+ steering_f = 2000.0f;
+ }
+ }
+ else // 鐩磋锛氬簲鐢ㄨ浆鍚戣ˉ鍋夸慨姝ｆ満姊板亸宸紙姝诲尯 卤0.001 rad/s锛�
+ {
+ steering_f = 1500.0f + (float)MC_CFG_STEERING_TRIM;
+ }
+
+ /* 杞崲涓烘暣鏁板苟杈撳嚭 */
+ uint32_t throttle = (uint32_t)(throttle_f + 0.5f);
+ uint32_t steering = (uint32_t)(steering_f + 0.5f);
+
+ Set_Motor_Pulse(throttle);
+ Set_Steering_Pulse(steering);
if (_pu16Steering != NULL)
{
@@ -300,6 +733,7 @@
}
}
+
/* 澶辨晥淇濇姢锛氬洖涓� PWM 骞朵笂鎶ラ浂鎺у埗閲� */
static void MotionControl_StopOutputs(void)
{
--
Gitblit v1.10.0