From 18f1d1afd16ae159b9f20cef640a594c848ad249 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期五, 05 十二月 2025 14:39:02 +0800
Subject: [PATCH] 校准完成，准备修改一下运动控制算法，转向的时候有直行分量，保存一版
---
STM32H743/APL/motion_control_task.c | 95 ++++++++++++++++++++++++++++++++++++++++++++---
1 files changed, 89 insertions(+), 6 deletions(-)
diff --git a/STM32H743/APL/motion_control_task.c b/STM32H743/APL/motion_control_task.c
index e21109c..a694081 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)
@@ -177,13 +178,57 @@
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锛�*/
- if (sbus_valid == HIDO_TRUE && ch8 > MOTION_SBUS_AUTO_THRESHOLD_US && gps_ready == HIDO_TRUE)
+ 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);
+
+ /* 妫�娴嬪埌浠庢墜鍔ㄥ垏鎹㈠埌鑷姩妯″紡鐨勪笂鍗囨部锛氶噸缃姸鎬佹満浠庡ご寮�濮� */
+ 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);
+ }
+ s_last_auto_condition = current_auto_condition;
+
+ if (current_auto_condition == HIDO_TRUE)
{
MC_Compute(&g_motion_state, 1.0f / MC_CFG_CONTROL_HZ, &output);
}
@@ -193,6 +238,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)
@@ -256,13 +304,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;
--
Gitblit v1.10.0