From 346cc7d685283df529aadbcf9c156de040ce44f9 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期一, 24 十一月 2025 09:51:48 +0800
Subject: [PATCH] 仿真跑起来了，但是起点转向不太对。
---
STM32H743/FML/PythonLink.c | 35 +++++++++++++++++++++++++++++++----
1 files changed, 31 insertions(+), 4 deletions(-)
diff --git a/STM32H743/FML/PythonLink.c b/STM32H743/FML/PythonLink.c
index ede54d0..c66dcb2 100644
--- a/STM32H743/FML/PythonLink.c
+++ b/STM32H743/FML/PythonLink.c
@@ -549,20 +549,47 @@
HIDO_UINT16 _steering_pwm,
HIDO_UINT16 _throttle_pwm,
const HIDO_CHAR *_pcStage,
- HIDO_UINT32 _timestamp_ms)
+ HIDO_UINT32 _timestamp_ms,
+ const HIDO_FLOAT *_pos_enu,
+ HIDO_FLOAT _heading_deg,
+ HIDO_FLOAT _target_heading_deg,
+ const HIDO_FLOAT *_target_xy)
{
- HIDO_CHAR payload[160];
+ HIDO_CHAR payload[256];
const HIDO_CHAR *stage = (_pcStage != HIDO_NULL) ? _pcStage : "unknown";
+ HIDO_FLOAT east = 0.0f;
+ HIDO_FLOAT north = 0.0f;
+ HIDO_FLOAT up = 0.0f;
+ if (_pos_enu != HIDO_NULL)
+ {
+ east = _pos_enu[0];
+ north = _pos_enu[1];
+ up = _pos_enu[2];
+ }
+ HIDO_FLOAT target_e = 0.0f;
+ HIDO_FLOAT target_n = 0.0f;
+ if (_target_xy != HIDO_NULL)
+ {
+ target_e = _target_xy[0];
+ target_n = _target_xy[1];
+ }
HIDO_UtilSnprintf(payload,
sizeof(payload),
- "%.3f,%.3f,%.2f,%u,%u,%s,%lu",
+ "%.3f,%.3f,%.2f,%u,%u,%s,%lu,%.3f,%.3f,%.3f,%.2f,%.2f,%.3f,%.3f",
_forward_mps,
_turn_rate,
_freq_hz,
(unsigned int)_steering_pwm,
(unsigned int)_throttle_pwm,
stage,
- (unsigned long)_timestamp_ms);
+ (unsigned long)_timestamp_ms,
+ east,
+ north,
+ up,
+ _heading_deg,
+ _target_heading_deg,
+ target_e,
+ target_n);
PythonLink_SendAsciiSentence(PYTHONLINK_ASCII_KEY_CTRL, payload);
}
--
Gitblit v1.10.0