仿真跑起来了，但是起点转向不太对。

yincheng.zhong

2025-11-24 346cc7d685283df529aadbcf9c156de040ce44f9

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