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

yincheng.zhong

10 天以前 4adc78553c8d48ff122506195fa33641134bd7b1

 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],