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/FML/PythonLink.c | 39 +++++++++++++++++++++++++++++++++++----
1 files changed, 35 insertions(+), 4 deletions(-)
diff --git a/STM32H743/FML/PythonLink.c b/STM32H743/FML/PythonLink.c
index ede54d0..cc79cc2 100644
--- a/STM32H743/FML/PythonLink.c
+++ b/STM32H743/FML/PythonLink.c
@@ -16,6 +16,8 @@
#include <math.h>
#include <stdio.h>
+#define ENABLE_PYTHONLINK_CTRL_LOG 1
+
/*******************************************************************************
* Global Variables *
*******************************************************************************/
@@ -549,21 +551,50 @@
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);
+#if ENABLE_PYTHONLINK_CTRL_LOG
PythonLink_SendAsciiSentence(PYTHONLINK_ASCII_KEY_CTRL, payload);
+#endif
}
HIDO_VOID PythonLink_ReportPose(const HIDO_FLOAT _enu[3],
--
Gitblit v1.10.0