From 1ebe8cd1247982a2c9d1d75b9c72d214eed4d581 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期四, 27 十一月 2025 17:37:16 +0800
Subject: [PATCH] 增加运动模型采集功能，还没有测试，切换成P23M+IM23组合。
---
STM32H743/APL/motion_control_task.c | 48 +++++++++++++++++++++++++++++++++++++++++++++---
1 files changed, 45 insertions(+), 3 deletions(-)
diff --git a/STM32H743/APL/motion_control_task.c b/STM32H743/APL/motion_control_task.c
index f4196d7..584427f 100644
--- a/STM32H743/APL/motion_control_task.c
+++ b/STM32H743/APL/motion_control_task.c
@@ -36,6 +36,7 @@
static ST_GeoOrigin g_motion_origin;
static HIDO_UINT32 g_last_gps_ms = 0U;
static HIDO_UINT32 g_last_imu_ms = 0U;
+static HIDO_UINT32 g_last_gprmi_tow = 0U;
static HIDO_UINT32 g_last_control_report_ms = 0U;
static HIDO_UINT32 g_last_pose_report_ms = 0U;
static HIDO_UINT32 g_last_state_report_ms = 0U;
@@ -121,8 +122,9 @@
HIDO_BOOL imu_valid = (GPS_GetGPIMU(&gpimu) == HIDO_OK);
HIDO_UINT32 now = HAL_GetTick();
- if (gps_valid == HIDO_TRUE)
+ if (gps_valid == HIDO_TRUE && gprmi.m_u32TimeOfWeek != 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));
@@ -131,9 +133,10 @@
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;
}
- else if ((now - g_last_gps_ms) > 200U)
+ else if (gps_valid == HIDO_FALSE && (now - g_last_gps_ms) > 200U)
{
g_motion_state.pose_valid = HIDO_FALSE;
g_last_pose_valid = HIDO_FALSE;
@@ -157,6 +160,40 @@
MC_Output output;
MC_Compute(&g_motion_state, 1.0f / MC_CFG_CONTROL_HZ, &output);
+ static HIDO_UINT32 s_ctrl_log_idx = 0U;
+ if ((s_ctrl_log_idx++ % 10U) == 0U)
+ {
+ /* 浣跨敤鏁存暟琛ㄧず娉曟墦鍗版诞鐐规暟锛岄伩鍏嶆爤鎹熷潖 */
+ 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 - fwd_int) * 100);
+ int turn_int = (int)output.turn_rate;
+ int turn_frac = (int)(fabsf(output.turn_rate - turn_int) * 100);
+
+ 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",
+ 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_int, turn_frac,
+ g_motion_state.nearest_index);
+ }
+
/* --- 3) 鏍规嵁鎺у埗閲忔洿鏂� PWM锛屽苟鎶� forward/turn 鍥炰紶缁� Python --- */
HIDO_UINT16 applied_steering = MC_CFG_PWM_CENTER_US;
HIDO_UINT16 applied_throttle = MC_CFG_PWM_CENTER_US;
@@ -205,13 +242,18 @@
}
const HIDO_CHAR *stage_label = MotionControl_StageLabel(output.stage);
+ const HIDO_FLOAT *target_ptr = (output.target_valid == HIDO_TRUE) ? output.target_xy : HIDO_NULL;
PythonLink_ReportControl(output.forward_mps,
output.turn_rate,
g_control_freq_hz,
g_last_steering_pwm,
g_last_throttle_pwm,
stage_label,
- g_last_sensor_timestamp_ms);
+ g_last_sensor_timestamp_ms,
+ output.pos_enu,
+ output.heading_deg,
+ output.target_heading_deg,
+ target_ptr);
if ((now - g_last_state_report_ms) >= 1000U)
{
g_last_state_report_ms = now;
--
Gitblit v1.10.0