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/APL/motion_control_task.c | 13 +++++++++++++
1 files changed, 13 insertions(+), 0 deletions(-)
diff --git a/STM32H743/APL/motion_control_task.c b/STM32H743/APL/motion_control_task.c
index 3baf698..c822738 100644
--- a/STM32H743/APL/motion_control_task.c
+++ b/STM32H743/APL/motion_control_task.c
@@ -13,6 +13,8 @@
#include <string.h>
#include <math.h>
+#define ENABLE_MC_CTRL_LOG 1
+
#include "DBG.h"
#include "GPS.h"
#include "PythonLink.h"
@@ -414,6 +416,7 @@
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");
@@ -427,12 +430,14 @@
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);
@@ -465,6 +470,7 @@
fwd_int, fwd_frac,
turn_sign, turn_int, turn_frac,
g_motion_state.nearest_index);
+#endif
}
/* --- 3) 鏍规嵁鎺у埗閲忔洿鏂� PWM锛屽苟鎶� forward/turn 鍥炰紶缁� Python --- */
@@ -518,6 +524,13 @@
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;
--
Gitblit v1.10.0