From f88f3da8f132cd1dd321dfc584a1fe68b6eb2138 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期四, 04 十二月 2025 21:49:01 +0800
Subject: [PATCH] 在跑校准了，还是有些问题,GPS坐标有时候不更新
---
STM32H743/APL/motion_control_task.c | 199 +++++++++++++++++++++++++++++++++++++++++--------
1 files changed, 166 insertions(+), 33 deletions(-)
diff --git a/STM32H743/APL/motion_control_task.c b/STM32H743/APL/motion_control_task.c
index f4196d7..e21109c 100644
--- a/STM32H743/APL/motion_control_task.c
+++ b/STM32H743/APL/motion_control_task.c
@@ -11,6 +11,7 @@
#include "stm32h7xx_hal.h"
#include <string.h>
+#include <math.h>
#include "DBG.h"
#include "GPS.h"
@@ -20,6 +21,8 @@
#include "motion_control.h"
#include "motion_path_data.h"
#include "pwm_ctrol.h"
+#include "motion_mode.h"
+#include "SBUS.h"
#define RAD2DEG (57.29577951308232f)
@@ -36,6 +39,7 @@
static ST_GeoOrigin g_motion_origin;
static HIDO_UINT32 g_last_gps_ms = 0U;
static HIDO_UINT32 g_last_imu_ms = 0U;
+static HIDO_UINT32 g_last_gprmi_tow = 0U;
static HIDO_UINT32 g_last_control_report_ms = 0U;
static HIDO_UINT32 g_last_pose_report_ms = 0U;
static HIDO_UINT32 g_last_state_report_ms = 0U;
@@ -123,17 +127,23 @@
if (gps_valid == HIDO_TRUE)
{
- Geo_GprmiToENU(&gprmi, &g_motion_origin, enu);
- MC_UpdateGps(&g_motion_state, enu, &gprmi);
- memcpy(g_last_enu, enu, sizeof(enu));
- g_last_heading_deg = gprmi.m_fHeadingAngle;
- g_last_pitch_deg = gprmi.m_fPitchAngle;
- g_last_roll_deg = gprmi.m_fRollAngle;
- g_last_pose_valid = HIDO_TRUE;
- g_last_gps_ms = now;
- g_last_sensor_timestamp_ms = gprmi.m_u32TimeOfWeek;
+ HIDO_UINT32 gps_timestamp = (gprmi.m_u32UTCTime != 0U) ? gprmi.m_u32UTCTime : (HIDO_UINT32)now;
+ if ((gprmi.m_u32UTCTime == 0U) || (gps_timestamp != g_last_gprmi_tow))
+ {
+ /* 鏂扮殑GPS鏁版嵁鍒版潵锛堟椂闂存埑鍙樺寲锛夛紝鎵嶆洿鏂拌埅鍚� */
+ Geo_GprmiToENU(&gprmi, &g_motion_origin, enu);
+ MC_UpdateGps(&g_motion_state, enu, &gprmi);
+ memcpy(g_last_enu, enu, sizeof(enu));
+ g_last_heading_deg = gprmi.m_fHeadingAngle;
+ g_last_pitch_deg = gprmi.m_fPitchAngle;
+ g_last_roll_deg = gprmi.m_fRollAngle;
+ g_last_pose_valid = HIDO_TRUE;
+ g_last_gps_ms = now;
+ g_last_gprmi_tow = gps_timestamp;
+ g_last_sensor_timestamp_ms = gps_timestamp;
+ }
}
- else if ((now - g_last_gps_ms) > 200U)
+ else if (gps_valid == HIDO_FALSE && (now - g_last_gps_ms) > 200U)
{
g_motion_state.pose_valid = HIDO_FALSE;
g_last_pose_valid = HIDO_FALSE;
@@ -155,7 +165,87 @@
/* --- 2) 璋冪敤杩愬姩鎺у埗鍣紙鍥哄畾 75 Hz dt锛� --- */
MC_Output output;
- MC_Compute(&g_motion_state, 1.0f / MC_CFG_CONTROL_HZ, &output);
+ HIDO_BOOL sbus_valid = (SBUS_IsSignalValid(MOTION_SBUS_TIMEOUT_MS) == HIDO_TRUE);
+ HIDO_UINT16 ch8 = SBUS_GetChannel(MOTION_SBUS_AUTO_CHANNEL);
+
+ /* 妫�鏌� GPS 鐘舵�侊細蹇呴』鍒濆鍖栧畬鎴愪笖杩炴帴鍒� GNSS */
+ HIDO_BOOL gps_ready = HIDO_FALSE;
+ if (gps_valid == HIDO_TRUE)
+ {
+ HIDO_UINT32 status = gprmi.m_u32StatusFlags;
+ HIDO_BOOL init_ok = ((status & IM23A_STATUS_READY) != 0U);
+ HIDO_BOOL gnss_connected = ((status & IM23A_STATUS_GNSS_CONNECT) != 0U);
+ gps_ready = (init_ok == HIDO_TRUE) && (gnss_connected == HIDO_TRUE);
+ }
+
+ /* 鍙湁褰撴弧瓒虫墍鏈夋潯浠舵椂鎵嶆墽琛岃嚜鍔ㄦ帶鍒讹細
+ * 1. SBUS 淇″彿鏈夋晥
+ * 2. CH8 > 闃堝�硷紙鑷姩妯″紡寮�鍏筹級
+ * 3. GPS 鍒濆鍖栧畬鎴愶紙FINIT_OK锛�
+ * 4. GPS 杩炴帴鍒� GNSS锛圙NSS_CONNECT锛�*/
+ if (sbus_valid == HIDO_TRUE && ch8 > MOTION_SBUS_AUTO_THRESHOLD_US && gps_ready == HIDO_TRUE)
+ {
+ MC_Compute(&g_motion_state, 1.0f / MC_CFG_CONTROL_HZ, &output);
+ }
+ else
+ {
+ /* 鎵嬪姩妯″紡鎴栨潯浠朵笉婊¤冻锛氶噸缃緭鍑哄苟璁╁嚭鎺у埗鏉� */
+ memset(&output, 0, sizeof(MC_Output));
+ output.active = HIDO_FALSE;
+
+ /* 瀹氭湡鎵撳嵃鐘舵�佹彁绀� */
+ static HIDO_UINT32 s_status_log = 0U;
+ if ((s_status_log++ % 100U) == 0U)
+ {
+ if (sbus_valid == HIDO_FALSE)
+ {
+ HIDO_Debug2("[MC_CTRL] Waiting: SBUS signal lost\r\n");
+ }
+ else if (ch8 <= MOTION_SBUS_AUTO_THRESHOLD_US)
+ {
+ HIDO_Debug2("[MC_CTRL] Waiting: CH8=%u (manual mode)\r\n", ch8);
+ }
+ else if (gps_ready == HIDO_FALSE)
+ {
+ HIDO_UINT32 status = gps_valid ? gprmi.m_u32StatusFlags : 0U;
+ HIDO_Debug2("[MC_CTRL] Waiting: GPS not ready (status=0x%08X)\r\n", status);
+ }
+ }
+ }
+
+ static HIDO_UINT32 s_ctrl_log_idx = 0U;
+ if ((s_ctrl_log_idx++ % 10U) == 0U)
+ {
+ /* 浣跨敤鏁存暟琛ㄧず娉曟墦鍗版诞鐐规暟锛岄伩鍏嶆爤鎹熷潖 */
+ int pos_x_int = (int)g_motion_state.pos[0];
+ int pos_x_frac = (int)(fabsf(g_motion_state.pos[0] - pos_x_int) * 100);
+ int pos_y_int = (int)g_motion_state.pos[1];
+ int pos_y_frac = (int)(fabsf(g_motion_state.pos[1] - pos_y_int) * 100);
+ int pos_z_int = (int)g_motion_state.pos[2];
+ int pos_z_frac = (int)(fabsf(g_motion_state.pos[2] - pos_z_int) * 100);
+ int hdg_int = (int)g_motion_state.heading_deg;
+ int hdg_frac = (int)(fabsf(g_motion_state.heading_deg - hdg_int) * 100);
+ int tgt_x_int = (int)g_motion_state.current_target_xy[0];
+ int tgt_x_frac = (int)(fabsf(g_motion_state.current_target_xy[0] - tgt_x_int) * 100);
+ int tgt_y_int = (int)g_motion_state.current_target_xy[1];
+ int tgt_y_frac = (int)(fabsf(g_motion_state.current_target_xy[1] - tgt_y_int) * 100);
+ int fwd_int = (int)output.forward_mps;
+ int fwd_frac = (int)(fabsf(output.forward_mps - fwd_int) * 100);
+ int turn_int = (int)output.turn_rate;
+ int turn_frac = (int)(fabsf(output.turn_rate - turn_int) * 100);
+
+ HIDO_Debug2("[MC_CTRL] stage=%d pos=(%d.%02d,%d.%02d,%d.%02d) hdg=%d.%02d tgt=(%d.%02d,%d.%02d) fwd=%d.%02d turn=%d.%02d path_idx=%u\n",
+ g_motion_state.stage,
+ pos_x_int, pos_x_frac,
+ pos_y_int, pos_y_frac,
+ pos_z_int, pos_z_frac,
+ hdg_int, hdg_frac,
+ tgt_x_int, tgt_x_frac,
+ tgt_y_int, tgt_y_frac,
+ fwd_int, fwd_frac,
+ turn_int, turn_frac,
+ g_motion_state.nearest_index);
+ }
/* --- 3) 鏍规嵁鎺у埗閲忔洿鏂� PWM锛屽苟鎶� forward/turn 鍥炰紶缁� Python --- */
HIDO_UINT16 applied_steering = MC_CFG_PWM_CENTER_US;
@@ -166,9 +256,13 @@
}
else
{
+ if(ch8 > MOTION_SBUS_AUTO_THRESHOLD_US)
+ {
MotionControl_StopOutputs();
applied_steering = MC_CFG_PWM_CENTER_US;
applied_throttle = MC_CFG_PWM_CENTER_US;
+ }
+ /* 鏄惧紡鍥為��鍒版墜鍔ㄦā寮忥紙閬ユ帶鍣ㄧ洿閫氬湪 SBUS 妯″潡涓鐞嗭紝鎴栬�呭湪姝ゅ瀹屽叏涓嶈緭鍑猴級 */
}
g_last_steering_pwm = applied_steering;
g_last_throttle_pwm = applied_throttle;
@@ -205,13 +299,18 @@
}
const HIDO_CHAR *stage_label = MotionControl_StageLabel(output.stage);
+ const HIDO_FLOAT *target_ptr = (output.target_valid == HIDO_TRUE) ? output.target_xy : HIDO_NULL;
PythonLink_ReportControl(output.forward_mps,
output.turn_rate,
g_control_freq_hz,
g_last_steering_pwm,
g_last_throttle_pwm,
stage_label,
- g_last_sensor_timestamp_ms);
+ g_last_sensor_timestamp_ms,
+ output.pos_enu,
+ output.heading_deg,
+ output.target_heading_deg,
+ target_ptr);
if ((now - g_last_state_report_ms) >= 1000U)
{
g_last_state_report_ms = now;
@@ -248,39 +347,72 @@
}
}
-/* 灏嗘帶鍒惰緭鍑烘槧灏勪负 PWM锛屽苟鍙戝洖 Python 绔� */
+/* 灏嗘帶鍒惰緭鍑烘槧灏勪负 PWM锛堜娇鐢ㄦ牎鍑嗘ā鍨嬪弽瑙ｏ級*/
static void MotionControl_ApplyOutput(const MC_Output *output,
HIDO_UINT16 *_pu16Steering,
HIDO_UINT16 *_pu16Throttle)
{
- float forward_ratio = output->forward_mps / g_motion_config.max_forward_mps;
- float turn_ratio = output->turn_rate / g_motion_config.max_turn_rate;
- forward_ratio = MC_CLAMP(forward_ratio, -1.0f, 1.0f);
- turn_ratio = MC_CLAMP(turn_ratio, -1.0f, 1.0f);
-
- int32_t throttle = (int32_t)MC_CFG_PWM_CENTER_US + (int32_t)(forward_ratio * (float)MC_CFG_PWM_SPAN_US);
- int32_t steering = (int32_t)MC_CFG_PWM_CENTER_US + (int32_t)(turn_ratio * (float)MC_CFG_PWM_SPAN_US);
-
- if (throttle < 1000)
+ /* ========== 鍓嶅悜閫熷害 鈫� 娌归棬PWM锛堜娇鐢ㄦ牎鍑嗘ā鍨嬪弽瑙ｏ級========== */
+ /* 鏍″噯妯″瀷锛歷 = MC_CFG_FORWARD_K 脳 (1500 - pwm) + MC_CFG_FORWARD_BIAS
+ * 鍙嶈В锛歱wm = 1500 - (v - MC_CFG_FORWARD_BIAS) / MC_CFG_FORWARD_K
+ * 璇存槑锛�1000=鏈�澶у墠杩涳紝1500=鍋滄锛�2000=鏈�澶у悗閫�
+ */
+ float target_velocity = output->forward_mps;
+ float throttle_f = 1500.0f - (target_velocity - MC_CFG_FORWARD_BIAS) / MC_CFG_FORWARD_K;
+
+ /* 闄愬埗PWM鑼冨洿 */
+ if (throttle_f < 1000.0f)
{
- throttle = 1000;
+ throttle_f = 1000.0f;
}
- else if (throttle > 2000)
+ else if (throttle_f > 2000.0f)
{
- throttle = 2000;
+ throttle_f = 2000.0f;
}
-
- if (steering < 1000)
+
+ /* 姝诲尯澶勭悊锛氭帴杩�1500鏃剁洿鎺ヨ涓�1500 */
+ if (fabsf(throttle_f - 1500.0f) < (float)MC_CFG_FORWARD_DEADZONE_PWM)
{
- steering = 1000;
+ throttle_f = 1500.0f;
}
- else if (steering > 2000)
+
+ /* ========== 杞悜瑙掗�熷害 鈫� 杞悜PWM锛堜娇鐢ㄦ牎鍑嗘ā鍨嬪弽瑙ｏ級========== */
+ /* 鏍″噯妯″瀷锛毾塤left = MC_CFG_STEERING_K_LEFT 脳 (1500 - pwm) 褰� pwm < 1500 (宸﹁浆)
+ * 蠅_right = -MC_CFG_STEERING_K_RIGHT 脳 (pwm - 1500) 褰� pwm > 1500 (鍙宠浆)
+ * 鍙嶈В锛歱wm_left = 1500 - 蠅 / MC_CFG_STEERING_K_LEFT
+ * pwm_right = 1500 - 蠅 / MC_CFG_STEERING_K_RIGHT
+ * 璇存槑锛�1000=鏈�澶у乏杞紝1500=鐩磋锛�2000=鏈�澶у彸杞�
+ */
+ float target_yaw_rate = output->turn_rate; // rad/s锛屾鍊�=宸﹁浆锛岃礋鍊�=鍙宠浆
+ float steering_f = 1500.0f;
+
+ if (target_yaw_rate > 0.001f) // 宸﹁浆锛堟瑙掗�熷害锛�
{
- steering = 2000;
+ steering_f = 1500.0f - target_yaw_rate / MC_CFG_STEERING_K_LEFT;
+ if (steering_f < 1000.0f)
+ {
+ steering_f = 1000.0f;
+ }
}
-
- Set_Motor_Pulse((uint32_t)throttle);
- Set_Steering_Pulse((uint32_t)steering);
+ else if (target_yaw_rate < -0.001f) // 鍙宠浆锛堣礋瑙掗�熷害锛�
+ {
+ steering_f = 1500.0f - target_yaw_rate / MC_CFG_STEERING_K_RIGHT;
+ if (steering_f > 2000.0f)
+ {
+ steering_f = 2000.0f;
+ }
+ }
+ else // 鐩磋锛氬簲鐢ㄨ浆鍚戣ˉ鍋夸慨姝ｆ満姊板亸宸�
+ {
+ steering_f = 1500.0f + (float)MC_CFG_STEERING_TRIM;
+ }
+
+ /* 杞崲涓烘暣鏁板苟杈撳嚭 */
+ uint32_t throttle = (uint32_t)(throttle_f + 0.5f);
+ uint32_t steering = (uint32_t)(steering_f + 0.5f);
+
+ Set_Motor_Pulse(throttle);
+ Set_Steering_Pulse(steering);
if (_pu16Steering != NULL)
{
@@ -292,6 +424,7 @@
}
}
+
/* 澶辨晥淇濇姢锛氬洖涓� PWM 骞朵笂鎶ラ浂鎺у埗閲� */
static void MotionControl_StopOutputs(void)
{
--
Gitblit v1.10.0