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 | 256 +++++++++++++++++++++++++++++++++++----------------
1 files changed, 176 insertions(+), 80 deletions(-)
diff --git a/STM32H743/FML/PythonLink.c b/STM32H743/FML/PythonLink.c
index 3221c31..cc79cc2 100644
--- a/STM32H743/FML/PythonLink.c
+++ b/STM32H743/FML/PythonLink.c
@@ -11,8 +11,12 @@
#include "PythonLink.h"
#include "Uart.h"
#include "HIDO_Debug.h"
+#include "HIDO_Util.h"
#include <string.h>
#include <math.h>
+#include <stdio.h>
+
+#define ENABLE_PYTHONLINK_CTRL_LOG 1
/*******************************************************************************
* Global Variables *
@@ -75,6 +79,52 @@
return (HIDO_UINT16)(sum & 0xFFFF);
}
+static HIDO_UINT8 PythonLink_CalcAsciiChecksum(const HIDO_CHAR *_pcSentence)
+{
+ HIDO_UINT8 cs = 0;
+ if (_pcSentence == HIDO_NULL || _pcSentence[0] != '$')
+ {
+ return 0;
+ }
+ const HIDO_CHAR *p = _pcSentence + 1;
+ while (*p != '\0' && *p != '*')
+ {
+ cs ^= (HIDO_UINT8)(*p);
+ p++;
+ }
+ return cs;
+}
+
+static HIDO_VOID PythonLink_SendAsciiSentence(const HIDO_CHAR *_pcKeyword, const HIDO_CHAR *_pcPayload)
+{
+ if (_pcKeyword == HIDO_NULL)
+ {
+ return;
+ }
+
+ HIDO_CHAR sentence[256];
+ HIDO_INT32 len = 0;
+
+ if (_pcPayload != HIDO_NULL && _pcPayload[0] != '\0')
+ {
+ len = HIDO_UtilSnprintf(sentence, sizeof(sentence), "$%s,%s", _pcKeyword, _pcPayload);
+ }
+ else
+ {
+ len = HIDO_UtilSnprintf(sentence, sizeof(sentence), "$%s", _pcKeyword);
+ }
+
+ if (len <= 0 || len >= (HIDO_INT32)(sizeof(sentence) - 5))
+ {
+ return;
+ }
+
+ HIDO_UINT8 cs = PythonLink_CalcAsciiChecksum(sentence);
+ HIDO_UtilSnprintf(sentence + len, sizeof(sentence) - len, "*%02X\r\n", cs);
+
+ Uart_Send(UART_ID_PYTHON, (const HIDO_UINT8 *)sentence, strlen(sentence));
+}
+
#if (PYTHONLINK_FORCE_FIXED_PAYLOAD != 0)
/**
* @brief 璋冭瘯: 鐢熸垚鍥哄畾鍐呭鐨勮礋杞芥暟鎹紝渚夸簬瑙傛祴涓插彛鏄惁涔辩爜
@@ -471,69 +521,14 @@
*/
HIDO_INT32 PythonLink_SendGPSData(const ST_GPRMI *_pstGPRMI)
{
- ST_PythonLink_GPS gpsPacket;
- HIDO_INT32 ret;
-
- // 妫�鏌ュ弬鏁�
if (_pstGPRMI == NULL || _pstGPRMI->m_bValid == HIDO_FALSE)
{
return HIDO_ERR;
}
- // 濉厖GPS鏁版嵁鍖�
- memset(&gpsPacket, 0, sizeof(gpsPacket));
- gpsPacket.m_dLatitude = _pstGPRMI->m_dLatitude;
- gpsPacket.m_dLongitude = _pstGPRMI->m_dLongitude;
-
- // 榛樿浣跨敤GPRMI鎻愪緵鐨勮埅鍚戣, 鑻ヤ笉鍙敤鍒欏洖閫�鍒伴�熷害璁＄畻
- HIDO_FLOAT heading = _pstGPRMI->m_fHeadingAngle;
- if (!isfinite(heading))
- {
- heading = 0.0f;
- }
-
- if ((!isfinite(heading) || heading == 0.0f) &&
- (_pstGPRMI->m_fEastVelocity != 0.0f || _pstGPRMI->m_fNorthVelocity != 0.0f))
- {
- // atan2(涓�,鍖�) 寰楀埌鐩稿姝ｅ寳鏂瑰悜鐨勮搴�
- heading = atan2f(_pstGPRMI->m_fEastVelocity, _pstGPRMI->m_fNorthVelocity) * 180.0f / 3.14159265f;
- if (heading < 0.0f)
- {
- heading += 360.0f;
- }
- }
-
- gpsPacket.m_fHeadingAngle = heading;
- gpsPacket.m_fPitchAngle = _pstGPRMI->m_fPitchAngle;
- gpsPacket.m_fRollAngle = _pstGPRMI->m_fRollAngle;
- if (!isfinite(gpsPacket.m_fPitchAngle))
- {
- gpsPacket.m_fPitchAngle = 0.0f;
- }
- if (!isfinite(gpsPacket.m_fRollAngle))
- {
- gpsPacket.m_fRollAngle = 0.0f;
- }
-
- gpsPacket.m_fEastVelocity = _pstGPRMI->m_fEastVelocity;
- gpsPacket.m_fNorthVelocity = _pstGPRMI->m_fNorthVelocity;
- gpsPacket.m_fUpVelocity = _pstGPRMI->m_fUpVelocity;
- gpsPacket.m_fAltitude = _pstGPRMI->m_fAltitude;
- gpsPacket.m_u32UTCTime = _pstGPRMI->m_u32UTCTime;
- gpsPacket.m_u8PositionQuality = _pstGPRMI->m_u8PositionQuality;
- gpsPacket.m_u8SatelliteCount = _pstGPRMI->m_u8SatelliteCount;
-
- // 鍙戦�佹暟鎹抚
- ret = PythonLink_SendFrame(PYTHONLINK_TYPE_GPS,
- (const HIDO_UINT8 *)&gpsPacket,
- sizeof(gpsPacket));
-
- if (ret == HIDO_OK)
- {
- g_u32GPSPacketCount++;
- }
-
- return ret;
+ // ASCII 鍗忚涓嬩笉鍐嶉�氳繃 PythonLink 杈撳嚭 GPS 浜岃繘鍒舵暟鎹�
+ (void)_pstGPRMI;
+ return HIDO_OK;
}
/**
@@ -541,37 +536,138 @@
*/
HIDO_INT32 PythonLink_SendIMUData(const ST_GPIMU *_pstGPIMU)
{
- ST_PythonLink_IMU imuPacket;
- HIDO_INT32 ret;
-
- // 妫�鏌ュ弬鏁�
if (_pstGPIMU == NULL || _pstGPIMU->m_bValid == HIDO_FALSE)
{
return HIDO_ERR;
}
- // 濉厖IMU鏁版嵁鍖�
- memset(&imuPacket, 0, sizeof(imuPacket));
- imuPacket.m_fAccelX = _pstGPIMU->m_fAccelX;
- imuPacket.m_fAccelY = _pstGPIMU->m_fAccelY;
- imuPacket.m_fAccelZ = _pstGPIMU->m_fAccelZ;
- imuPacket.m_fGyroX = _pstGPIMU->m_fGyroX;
- imuPacket.m_fGyroY = _pstGPIMU->m_fGyroY;
- imuPacket.m_fGyroZ = _pstGPIMU->m_fGyroZ;
- imuPacket.m_fTemperature = _pstGPIMU->m_fTemperature;
- imuPacket.m_u32UTCTime = _pstGPIMU->m_u32UTCTime;
+ (void)_pstGPIMU;
+ return HIDO_OK;
+}
- // 鍙戦�佹暟鎹抚
- ret = PythonLink_SendFrame(PYTHONLINK_TYPE_IMU,
- (const HIDO_UINT8 *)&imuPacket,
- sizeof(imuPacket));
-
- if (ret == HIDO_OK)
+HIDO_VOID PythonLink_ReportControl(HIDO_FLOAT _forward_mps,
+ HIDO_FLOAT _turn_rate,
+ HIDO_FLOAT _freq_hz,
+ HIDO_UINT16 _steering_pwm,
+ HIDO_UINT16 _throttle_pwm,
+ const HIDO_CHAR *_pcStage,
+ 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[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)
{
- g_u32IMUPacketCount++;
+ 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,%.3f,%.2f,%.2f,%.3f,%.3f",
+ _forward_mps,
+ _turn_rate,
+ _freq_hz,
+ (unsigned int)_steering_pwm,
+ (unsigned int)_throttle_pwm,
+ stage,
+ (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
+}
- return ret;
+HIDO_VOID PythonLink_ReportPose(const HIDO_FLOAT _enu[3],
+ HIDO_FLOAT _heading_deg,
+ HIDO_FLOAT _pitch_deg,
+ HIDO_FLOAT _roll_deg,
+ const HIDO_FLOAT _target_xy[2],
+ HIDO_UINT32 _timestamp_ms)
+{
+ HIDO_FLOAT east = 0.0f;
+ HIDO_FLOAT north = 0.0f;
+ HIDO_FLOAT up = 0.0f;
+ if (_enu != HIDO_NULL)
+ {
+ east = _enu[0];
+ north = _enu[1];
+ up = _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_CHAR payload[192];
+ HIDO_UtilSnprintf(payload,
+ sizeof(payload),
+ "%.3f,%.3f,%.3f,%.2f,%.2f,%.2f,%.3f,%.3f,%lu",
+ east,
+ north,
+ up,
+ _heading_deg,
+ _pitch_deg,
+ _roll_deg,
+ target_e,
+ target_n,
+ (unsigned long)_timestamp_ms);
+ PythonLink_SendAsciiSentence(PYTHONLINK_ASCII_KEY_POSE, payload);
+}
+
+HIDO_VOID PythonLink_ReportState(const HIDO_CHAR *_pcStage,
+ HIDO_FLOAT _xte_m,
+ HIDO_FLOAT _heading_err_deg,
+ HIDO_UINT32 _timestamp_ms)
+{
+ HIDO_CHAR payload[128];
+ const HIDO_CHAR *stage = (_pcStage != HIDO_NULL) ? _pcStage : "unknown";
+ HIDO_UtilSnprintf(payload,
+ sizeof(payload),
+ "%s,%.3f,%.2f,%lu",
+ stage,
+ _xte_m,
+ _heading_err_deg,
+ (unsigned long)_timestamp_ms);
+ PythonLink_SendAsciiSentence(PYTHONLINK_ASCII_KEY_STATE, payload);
+}
+
+HIDO_VOID PythonLink_ReportStack(const HIDO_CHAR *_pcTaskName,
+ HIDO_UINT32 _u32StackHighWaterWords,
+ HIDO_UINT32 _u32HeapFreeBytes,
+ HIDO_UINT32 _u32HeapMinBytes)
+{
+ HIDO_CHAR payload[128];
+ const HIDO_CHAR *task = (_pcTaskName != HIDO_NULL) ? _pcTaskName : "NA";
+ HIDO_UtilSnprintf(payload,
+ sizeof(payload),
+ "%s,%lu,%lu,%lu",
+ task,
+ (unsigned long)_u32StackHighWaterWords,
+ (unsigned long)_u32HeapFreeBytes,
+ (unsigned long)_u32HeapMinBytes);
+ PythonLink_SendAsciiSentence(PYTHONLINK_ASCII_KEY_STACK, payload);
}
/**
--
Gitblit v1.10.0