路径点不重复的控制算法测试通了，但是好像不如之前路径点重复的版本好。增加了蓝牙接口部分。准备移植外包的MQTT。

yincheng.zhong

10 天以前 4adc78553c8d48ff122506195fa33641134bd7b1

 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);
}

/**