Lawnmower_STM32H7.git

			@@ -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;