From 346cc7d685283df529aadbcf9c156de040ce44f9 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期一, 24 十一月 2025 09:51:48 +0800
Subject: [PATCH] 仿真跑起来了，但是起点转向不太对。
---
STM32H743/FML/motion_control.c | 24 ++++++++++++++++++++++--
1 files changed, 22 insertions(+), 2 deletions(-)
diff --git a/STM32H743/FML/motion_control.c b/STM32H743/FML/motion_control.c
index b5ed3ee..7b19d09 100644
--- a/STM32H743/FML/motion_control.c
+++ b/STM32H743/FML/motion_control.c
@@ -176,6 +176,7 @@
float dist = mc_distance(start_point, state->pos);
float desired_heading = arm_atan2_f32(vec_y, vec_x);
+ out->target_heading_deg = desired_heading * RAD2DEG;
float heading_err = mc_wrap_angle(desired_heading - state->heading_rad);
float yaw_rate_cmd = state->config.heading_kp * heading_err;
yaw_rate_cmd = MC_CLAMP(yaw_rate_cmd, -state->config.max_turn_rate, state->config.max_turn_rate);
@@ -227,6 +228,7 @@
float vec_x = target[0] - state->pos[0];
float vec_y = target[1] - state->pos[1];
float desired_heading = arm_atan2_f32(vec_y, vec_x);
+ out->target_heading_deg = desired_heading * RAD2DEG;
float heading_err = mc_wrap_angle(desired_heading - state->heading_rad);
float heading_err_rate = (heading_err - state->last_heading_err) / (dt_s > 0.0f ? dt_s : 0.013f);
state->last_heading_err = heading_err;
@@ -313,8 +315,17 @@
_state->pos[0] = _enu[0];
_state->pos[1] = _enu[1];
_state->pos[2] = _enu[2];
- _state->heading_rad = mc_wrap_angle((_gprmi->m_fHeadingAngle) * DEG2RAD);
- _state->heading_deg = _gprmi->m_fHeadingAngle;
+ float gps_heading_rad = mc_wrap_angle((_gprmi->m_fHeadingAngle) * DEG2RAD);
+ if (_state->imu_valid == HIDO_TRUE)
+ {
+ float heading_err = mc_wrap_angle(gps_heading_rad - _state->heading_rad);
+ _state->heading_rad = mc_wrap_angle(_state->heading_rad + heading_err * MC_CFG_GPS_HEADING_BLEND);
+ }
+ else
+ {
+ _state->heading_rad = gps_heading_rad;
+ }
+ _state->heading_deg = _state->heading_rad * RAD2DEG;
_state->pitch_deg = _gprmi->m_fPitchAngle;
_state->roll_deg = _gprmi->m_fRollAngle;
_state->vel[0] = _gprmi->m_fEastVelocity;
@@ -364,11 +375,19 @@
return;
}
+ if (_state->imu_valid == HIDO_TRUE && _dt_s > 0.0f)
+ {
+ _state->heading_rad = mc_wrap_angle(_state->heading_rad + _state->yaw_rate_rad * _dt_s);
+ _state->heading_deg = _state->heading_rad * RAD2DEG;
+ }
+
if (_state->stage == MC_STAGE_IDLE)
{
_state->stage = MC_STAGE_GOTO_START;
}
+ _out->target_heading_deg = _state->heading_deg;
+
switch (_state->stage)
{
case MC_STAGE_GOTO_START:
@@ -379,6 +398,7 @@
break;
case MC_STAGE_FINISHED:
_out->active = HIDO_FALSE;
+ _out->target_heading_deg = _state->heading_deg;
break;
default:
break;
--
Gitblit v1.10.0