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_calibration_task.c | 230 ++++++++++++++++++++++++++++++++++++++++-----------------
1 files changed, 160 insertions(+), 70 deletions(-)
diff --git a/STM32H743/APL/motion_calibration_task.c b/STM32H743/APL/motion_calibration_task.c
index 2b7ce6f..b34a0d3 100644
--- a/STM32H743/APL/motion_calibration_task.c
+++ b/STM32H743/APL/motion_calibration_task.c
@@ -13,6 +13,7 @@
#include "GPS.h"
#include "SBUS.h"
#include "pwm_ctrol.h"
+#include "motion_mode.h"
/*******************************************************************************
* 閰嶇疆鍙傛暟 *
@@ -23,13 +24,40 @@
#define CAL_TASK_PRIORITY (tskIDLE_PRIORITY + 3U)
#define CAL_TASK_PERIOD_MS (50U) // 20Hz閲囨牱
-/* PWM閰嶇疆 */
-#define CAL_PWM_CENTER (1500U)
-#define CAL_PWM_LOW_THROTTLE (1600U) // 浣庨��
-#define CAL_PWM_MID_THROTTLE (1700U) // 涓��
-#define CAL_PWM_HIGH_THROTTLE (1800U) // 楂橀��
-#define CAL_PWM_LIGHT_TURN (1600U) // 杞昏浆鍚�
-#define CAL_PWM_HEAVY_TURN (1700U) // 閲嶈浆鍚�
+/* PWM閰嶇疆 - 鍙屽悜鎺у埗锛�1000=鏈�澶у墠杩�/宸﹁浆锛�1500=鍋滄锛�2000=鏈�澶у悗閫�/鍙宠浆锛�*/
+#define CAL_PWM_CENTER (1500U) // 鍋滄
+
+/* 杞悜琛ュ伩鍊硷紙鐢ㄤ簬淇鐩磋鍋忓樊锛屾鍊�=鍚戝彸琛ュ伩锛岃礋鍊�=鍚戝乏琛ュ伩锛�*/
+/* 杞﹁締鍚戝乏鍋� 鈫� 璁剧疆姝ｅ�硷紙濡�+5鍒�+15锛夋潵鍚戝彸琛ュ伩 */
+/* 杞﹁締鍚戝彸鍋� 鈫� 璁剧疆璐熷�硷紙濡�-5鍒�-15锛夋潵鍚戝乏琛ュ伩 */
+#define CAL_PWM_STEERING_TRIM (5) // 杞悜琛ュ伩锛氬悜鍙宠ˉ鍋�10锛堜慨姝ｅ乏鍋忥級
+
+/* 鍓嶈繘娴嬭瘯锛�1500鈫�1000锛夛細6涓。浣� */
+#define CAL_PWM_FORWARD_VLOW (1400U) // 杞诲害鍓嶈繘
+#define CAL_PWM_FORWARD_LOW (1300U) // 涓綆閫熷墠杩�
+#define CAL_PWM_FORWARD_MID (1200U) // 涓�熷墠杩�
+#define CAL_PWM_FORWARD_HIGH (1100U) // 楂橀�熷墠杩�
+#define CAL_PWM_FORWARD_VHIGH (1050U) // 瓒呴珮閫熷墠杩�
+#define CAL_PWM_FORWARD_MAX (1000U) // 鏈�澶у墠杩�
+
+/* 鍚庨��娴嬭瘯锛�1500鈫�2000锛夛細3涓。浣嶏紙鍙�夛級 */
+#define CAL_PWM_REVERSE_LOW (1600U) // 杞诲害鍚庨��
+#define CAL_PWM_REVERSE_MID (1700U) // 涓�熷悗閫�
+#define CAL_PWM_REVERSE_HIGH (1800U) // 楂橀�熷悗閫�
+
+/* 宸﹁浆娴嬭瘯锛�1500鈫�1000锛夛細5涓。浣� */
+#define CAL_PWM_TURN_LEFT_VLOW (1400U) // 杞诲害宸﹁浆
+#define CAL_PWM_TURN_LEFT_LOW (1300U) // 涓害宸﹁浆
+#define CAL_PWM_TURN_LEFT_MID (1200U) // 閲嶅害宸﹁浆
+#define CAL_PWM_TURN_LEFT_HIGH (1100U) // 瓒呴噸宸﹁浆
+#define CAL_PWM_TURN_LEFT_MAX (1000U) // 鏈�澶у乏杞�
+
+/* 鍙宠浆娴嬭瘯锛�1500鈫�2000锛夛細5涓。浣� */
+#define CAL_PWM_TURN_RIGHT_VLOW (1600U) // 杞诲害鍙宠浆
+#define CAL_PWM_TURN_RIGHT_LOW (1700U) // 涓害鍙宠浆
+#define CAL_PWM_TURN_RIGHT_MID (1800U) // 閲嶅害鍙宠浆
+#define CAL_PWM_TURN_RIGHT_HIGH (1900U) // 瓒呴噸鍙宠浆
+#define CAL_PWM_TURN_RIGHT_MAX (2000U) // 鏈�澶у彸杞�
/* 娴嬭瘯鏃堕暱锛堟绉掞級 */
#define CAL_DURATION_WARMUP (2000U) // 棰勭儹
@@ -51,22 +79,43 @@
{
CAL_STATE_IDLE = 0, // 绌洪棽锛岀瓑寰呭惎鍔�
CAL_STATE_WARMUP, // 棰勭儹锛堝師鍦伴潤姝紝妫�鏌ヤ紶鎰熷櫒锛�
+ CAL_STATE_ACCEL_VLOW, // 瓒呬綆閫熷姞閫�
+ CAL_STATE_CRUISE_VLOW, // 瓒呬綆閫熷贰鑸�
+ CAL_STATE_REST_1, // 闂存瓏1
CAL_STATE_ACCEL_LOW, // 浣庨�熷姞閫�
CAL_STATE_CRUISE_LOW, // 浣庨�熷贰鑸�
- CAL_STATE_REST_1, // 闂存瓏1
+ CAL_STATE_REST_2, // 闂存瓏2
+ CAL_STATE_ACCEL_MLOW, // 涓綆閫熷姞閫�
+ CAL_STATE_CRUISE_MLOW, // 涓綆閫熷贰鑸�
+ CAL_STATE_REST_3, // 闂存瓏3
CAL_STATE_ACCEL_MID, // 涓�熷姞閫�
CAL_STATE_CRUISE_MID, // 涓�熷贰鑸�
- CAL_STATE_REST_2, // 闂存瓏2
+ CAL_STATE_REST_4, // 闂存瓏4
CAL_STATE_ACCEL_HIGH, // 楂橀�熷姞閫�
CAL_STATE_CRUISE_HIGH, // 楂橀�熷贰鑸�
- CAL_STATE_REST_3, // 闂存瓏3
- CAL_STATE_TURN_LEFT_LIGHT, // 杞诲乏杞�
- CAL_STATE_REST_4, // 闂存瓏4
- CAL_STATE_TURN_RIGHT_LIGHT, // 杞诲彸杞�
CAL_STATE_REST_5, // 闂存瓏5
- CAL_STATE_TURN_LEFT_HEAVY, // 閲嶅乏杞�
+ CAL_STATE_ACCEL_VHIGH, // 瓒呴珮閫熷姞閫�
+ CAL_STATE_CRUISE_VHIGH, // 瓒呴珮閫熷贰鑸�
CAL_STATE_REST_6, // 闂存瓏6
+ CAL_STATE_TURN_LEFT_VLIGHT, // 瓒呰交宸﹁浆
+ CAL_STATE_REST_7, // 闂存瓏7
+ CAL_STATE_TURN_RIGHT_VLIGHT, // 瓒呰交鍙宠浆
+ CAL_STATE_REST_8, // 闂存瓏8
+ CAL_STATE_TURN_LEFT_LIGHT, // 杞诲乏杞�
+ CAL_STATE_REST_9, // 闂存瓏9
+ CAL_STATE_TURN_RIGHT_LIGHT, // 杞诲彸杞�
+ CAL_STATE_REST_10, // 闂存瓏10
+ CAL_STATE_TURN_LEFT_MID, // 涓乏杞�
+ CAL_STATE_REST_11, // 闂存瓏11
+ CAL_STATE_TURN_RIGHT_MID, // 涓彸杞�
+ CAL_STATE_REST_12, // 闂存瓏12
+ CAL_STATE_TURN_LEFT_HEAVY, // 閲嶅乏杞�
+ CAL_STATE_REST_13, // 闂存瓏13
CAL_STATE_TURN_RIGHT_HEAVY, // 閲嶅彸杞�
+ CAL_STATE_REST_14, // 闂存瓏14
+ CAL_STATE_TURN_LEFT_VHEAVY, // 瓒呴噸宸﹁浆
+ CAL_STATE_REST_15, // 闂存瓏15
+ CAL_STATE_TURN_RIGHT_VHEAVY, // 瓒呴噸鍙宠浆
CAL_STATE_BRAKE, // 鍒跺姩
CAL_STATE_FINISHED, // 瀹屾垚
CAL_STATE_ERROR // 閿欒锛堝畨鍏ㄥ仠姝級
@@ -92,46 +141,92 @@
static HIDO_BOOL g_cal_running = HIDO_FALSE;
static HIDO_UINT32 g_sample_count = 0U;
-/* 鐘舵�佹満閰嶇疆琛� */
+/* 鐘舵�佹満閰嶇疆琛� - 鍙屽悜PWM鏍″噯 (1000=鏈�澶ф鍚�, 1500=鍋滄, 2000=鏈�澶у弽鍚�) */
static const ST_CalStateConfig g_state_configs[] = {
- {CAL_STATE_WARMUP, 0, CAL_DURATION_WARMUP, CAL_PWM_CENTER, CAL_PWM_CENTER, "WARMUP"},
- {CAL_STATE_ACCEL_LOW, 0, CAL_DURATION_ACCEL, CAL_PWM_LOW_THROTTLE, CAL_PWM_CENTER, "ACCEL_LOW"},
- {CAL_STATE_CRUISE_LOW, 0, CAL_DURATION_CRUISE, CAL_PWM_LOW_THROTTLE, CAL_PWM_CENTER, "CRUISE_LOW"},
- {CAL_STATE_REST_1, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_1"},
- {CAL_STATE_ACCEL_MID, 0, CAL_DURATION_ACCEL, CAL_PWM_MID_THROTTLE, CAL_PWM_CENTER, "ACCEL_MID"},
- {CAL_STATE_CRUISE_MID, 0, CAL_DURATION_CRUISE, CAL_PWM_MID_THROTTLE, CAL_PWM_CENTER, "CRUISE_MID"},
- {CAL_STATE_REST_2, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_2"},
- {CAL_STATE_ACCEL_HIGH, 0, CAL_DURATION_ACCEL, CAL_PWM_HIGH_THROTTLE, CAL_PWM_CENTER, "ACCEL_HIGH"},
- {CAL_STATE_CRUISE_HIGH, 0, CAL_DURATION_CRUISE, CAL_PWM_HIGH_THROTTLE, CAL_PWM_CENTER, "CRUISE_HIGH"},
- {CAL_STATE_REST_3, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_3"},
- {CAL_STATE_TURN_LEFT_LIGHT, 0, CAL_DURATION_TURN, CAL_PWM_LOW_THROTTLE, CAL_PWM_LIGHT_TURN, "TURN_LEFT_LIGHT"},
- {CAL_STATE_REST_4, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_4"},
- {CAL_STATE_TURN_RIGHT_LIGHT, 0, CAL_DURATION_TURN, CAL_PWM_LOW_THROTTLE, 1500-(CAL_PWM_LIGHT_TURN-1500), "TURN_RIGHT_LIGHT"},
- {CAL_STATE_REST_5, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_5"},
- {CAL_STATE_TURN_LEFT_HEAVY, 0, CAL_DURATION_TURN, CAL_PWM_LOW_THROTTLE, CAL_PWM_HEAVY_TURN, "TURN_LEFT_HEAVY"},
- {CAL_STATE_REST_6, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_6"},
- {CAL_STATE_TURN_RIGHT_HEAVY, 0, CAL_DURATION_TURN, CAL_PWM_LOW_THROTTLE, 1500-(CAL_PWM_HEAVY_TURN-1500), "TURN_RIGHT_HEAVY"},
- {CAL_STATE_BRAKE, 0, CAL_DURATION_BRAKE, CAL_PWM_CENTER, CAL_PWM_CENTER, "BRAKE"},
- {CAL_STATE_FINISHED, 0, 0, CAL_PWM_CENTER, CAL_PWM_CENTER, "FINISHED"}
+ {CAL_STATE_WARMUP, 0, CAL_DURATION_WARMUP, CAL_PWM_CENTER, CAL_PWM_CENTER, "WARMUP"},
+
+ /* 鍓嶈繘娴嬭瘯锛�6涓。浣� (1400鈫�1000)锛岃浆鍚戝簲鐢ㄨˉ鍋垮�间慨姝ｇ洿琛屽亸宸� */
+ {CAL_STATE_ACCEL_VLOW, 0, CAL_DURATION_ACCEL, CAL_PWM_FORWARD_VLOW, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_VLOW"},
+ {CAL_STATE_CRUISE_VLOW, 0, CAL_DURATION_CRUISE, CAL_PWM_FORWARD_VLOW, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_VLOW_C"},
+ {CAL_STATE_REST_1, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_1"},
+ {CAL_STATE_ACCEL_LOW, 0, CAL_DURATION_ACCEL, CAL_PWM_FORWARD_LOW, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_LOW"},
+ {CAL_STATE_CRUISE_LOW, 0, CAL_DURATION_CRUISE, CAL_PWM_FORWARD_LOW, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_LOW_C"},
+ {CAL_STATE_REST_2, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_2"},
+ {CAL_STATE_ACCEL_MLOW, 0, CAL_DURATION_ACCEL, CAL_PWM_FORWARD_MID, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_MID"},
+ {CAL_STATE_CRUISE_MLOW, 0, CAL_DURATION_CRUISE, CAL_PWM_FORWARD_MID, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_MID_C"},
+ {CAL_STATE_REST_3, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_3"},
+ {CAL_STATE_ACCEL_MID, 0, CAL_DURATION_ACCEL, CAL_PWM_FORWARD_HIGH, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_HIGH"},
+ {CAL_STATE_CRUISE_MID, 0, CAL_DURATION_CRUISE, CAL_PWM_FORWARD_HIGH, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_HIGH_C"},
+ {CAL_STATE_REST_4, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_4"},
+ {CAL_STATE_ACCEL_HIGH, 0, CAL_DURATION_ACCEL, CAL_PWM_FORWARD_VHIGH, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_VHIGH"},
+ {CAL_STATE_CRUISE_HIGH, 0, CAL_DURATION_CRUISE, CAL_PWM_FORWARD_VHIGH, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_VHIGH_C"},
+ {CAL_STATE_REST_5, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_5"},
+ {CAL_STATE_ACCEL_VHIGH, 0, CAL_DURATION_ACCEL, CAL_PWM_FORWARD_MAX, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_MAX"},
+ {CAL_STATE_CRUISE_VHIGH, 0, CAL_DURATION_CRUISE, CAL_PWM_FORWARD_MAX, CAL_PWM_CENTER+CAL_PWM_STEERING_TRIM, "FWD_MAX_C"},
+ {CAL_STATE_REST_6, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_6"},
+
+ /* 宸﹁浆娴嬭瘯锛�5涓。浣� (1400鈫�1000, 鍘熷湴杞悜) */
+ {CAL_STATE_TURN_LEFT_VLIGHT, 0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_LEFT_VLOW, "TURN_L_VLOW"},
+ {CAL_STATE_REST_7, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_7"},
+ {CAL_STATE_TURN_LEFT_LIGHT, 0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_LEFT_LOW, "TURN_L_LOW"},
+ {CAL_STATE_REST_8, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_8"},
+ {CAL_STATE_TURN_LEFT_MID, 0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_LEFT_MID, "TURN_L_MID"},
+ {CAL_STATE_REST_9, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_9"},
+ {CAL_STATE_TURN_LEFT_HEAVY, 0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_LEFT_HIGH, "TURN_L_HIGH"},
+ {CAL_STATE_REST_10, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_10"},
+ {CAL_STATE_TURN_LEFT_VHEAVY, 0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_LEFT_MAX, "TURN_L_MAX"},
+ {CAL_STATE_REST_11, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_11"},
+
+ /* 鍙宠浆娴嬭瘯锛�5涓。浣� (1600鈫�2000, 鍘熷湴杞悜) */
+ {CAL_STATE_TURN_RIGHT_VLIGHT,0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_RIGHT_VLOW, "TURN_R_VLOW"},
+ {CAL_STATE_REST_12, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_12"},
+ {CAL_STATE_TURN_RIGHT_LIGHT, 0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_RIGHT_LOW, "TURN_R_LOW"},
+ {CAL_STATE_REST_13, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_13"},
+ {CAL_STATE_TURN_RIGHT_MID, 0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_RIGHT_MID, "TURN_R_MID"},
+ {CAL_STATE_REST_14, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_14"},
+ {CAL_STATE_TURN_RIGHT_HEAVY, 0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_RIGHT_HIGH, "TURN_R_HIGH"},
+ {CAL_STATE_REST_15, 0, CAL_DURATION_REST, CAL_PWM_CENTER, CAL_PWM_CENTER, "REST_15"},
+ {CAL_STATE_TURN_RIGHT_VHEAVY,0, CAL_DURATION_TURN, CAL_PWM_CENTER, CAL_PWM_TURN_RIGHT_MAX, "TURN_R_MAX"},
+
+ {CAL_STATE_BRAKE, 0, CAL_DURATION_BRAKE, CAL_PWM_CENTER, CAL_PWM_CENTER, "BRAKE"},
+ {CAL_STATE_FINISHED, 0, 0, CAL_PWM_CENTER, CAL_PWM_CENTER, "FINISHED"}
};
/*******************************************************************************
* 鍐呴儴鍑芥暟 *
*******************************************************************************/
-/* 瀹夊叏妫�鏌ワ細SBUS CH8 < 1500鎵嶅厑璁歌繍琛� */
+/* 瀹夊叏妫�鏌ワ細SBUS CH8 > 1500 涓� GPS 灏辩华鎵嶅厑璁歌繍琛� */
static HIDO_BOOL Cal_SafetyCheck(HIDO_VOID)
{
/* 妫�鏌BUS淇″彿鏈夋晥鎬� */
- if (SBUS_IsSignalValid(CAL_SBUS_TIMEOUT_MS) == HIDO_FALSE)
+ if (SBUS_IsSignalValid(MOTION_SBUS_TIMEOUT_MS) == HIDO_FALSE)
{
return HIDO_FALSE;
}
- /* 妫�鏌H8锛堢储寮�7锛� < 1500 */
- HIDO_UINT16 ch8 = SBUS_GetChannel(7U);
- if (ch8 >= CAL_SBUS_SAFETY_THRESHOLD)
+ /* 妫�鏌� CH8锛堢储寮�7锛� > 1500 (鑷姩妯″紡闃堝��) */
+ HIDO_UINT16 ch8 = SBUS_GetChannel(MOTION_SBUS_AUTO_CHANNEL);
+ if (ch8 <= MOTION_SBUS_AUTO_THRESHOLD_US)
{
+ /* CH8 <= 1500锛屽浜庢墜鍔ㄦā寮忥紝绂佹鏍″噯浠诲姟杩愯 */
+ return HIDO_FALSE;
+ }
+
+ /* 妫�鏌� GPS 鐘舵�侊細蹇呴』鍒濆鍖栧畬鎴愪笖杩炴帴鍒� GNSS */
+ ST_GPRMI gprmi;
+ if (GPS_GetGPRMI(&gprmi) != HIDO_OK)
+ {
+ return HIDO_FALSE;
+ }
+
+ 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);
+
+ if (init_ok == HIDO_FALSE || gnss_connected == HIDO_FALSE)
+ {
+ /* GPS 鏈氨缁紝绂佹鏍″噯浠诲姟杩愯 */
return HIDO_FALSE;
}
@@ -145,7 +240,7 @@
Set_Steering_Pulse(CAL_PWM_CENTER);
g_cal_state = CAL_STATE_ERROR;
g_cal_running = HIDO_FALSE;
- HIDO_Debug2("[CAL] EMERGENCY STOP - CH8 >= 1500 or SBUS lost!\r\n");
+ HIDO_Debug2("[CAL] EMERGENCY STOP - Safety check failed!\r\n");
}
/* 杈撳嚭PWM鎺у埗 */
@@ -176,22 +271,13 @@
HIDO_BOOL gps_valid = (GPS_GetGPRMI(&gprmi) == HIDO_OK);
HIDO_BOOL imu_valid = (GPS_GetGPIMU(&gpimu) == HIDO_OK);
- /* 鏍煎紡锛�$CAL,seq,time_ms,state,throttle_pwm,steering_pwm,lat,lon,alt,hdg,pitch,roll,gx,gy,gz,ax,ay,az*XX */
- if (gps_valid == HIDO_TRUE && imu_valid == HIDO_TRUE)
+ float enu[3] = {0.0f, 0.0f, 0.0f};
+ HIDO_BOOL enu_valid = (GPS_GetCurrentENU(enu) == HIDO_OK);
+
+ /* 鏍煎紡锛�$CAL,seq,time_ms,state,throttle_pwm,steering_pwm,enu_x,enu_y,enu_z,hdg,pitch,roll,gx,gy,gz,ax,ay,az */
+ if (gps_valid == HIDO_TRUE && imu_valid == HIDO_TRUE && enu_valid == HIDO_TRUE)
{
- /* 浣跨敤鏁存暟琛ㄧず娉曟墦鍗� */
- int lat_deg = (int)gprmi.m_dLatitude;
- int lat_frac = (int)((gprmi.m_dLatitude - lat_deg) * 1000000.0);
- int lon_deg = (int)gprmi.m_dLongitude;
- int lon_frac = (int)((gprmi.m_dLongitude - lon_deg) * 1000000.0);
- int alt_int = (int)gprmi.m_fAltitude;
- int alt_frac = (int)((gprmi.m_fAltitude - alt_int) * 100.0f);
- int hdg_int = (int)gprmi.m_fHeadingAngle;
- int hdg_frac = (int)((gprmi.m_fHeadingAngle - hdg_int) * 100.0f);
- int pitch_int = (int)gprmi.m_fPitchAngle;
- int pitch_frac = (int)((gprmi.m_fPitchAngle - pitch_int) * 100.0f);
- int roll_int = (int)gprmi.m_fRollAngle;
- int roll_frac = (int)((gprmi.m_fRollAngle - roll_int) * 100.0f);
+ /* 鐩存帴浣跨敤娴偣鏁版牸寮忥紝閬垮厤璐熷皬鏁扮殑绗﹀彿闂 */
int gx_int = (int)(gpimu.m_fGyroX * 1000.0f);
int gy_int = (int)(gpimu.m_fGyroY * 1000.0f);
int gz_int = (int)(gpimu.m_fGyroZ * 1000.0f);
@@ -199,18 +285,18 @@
int ay_int = (int)(gpimu.m_fAccelY * 1000.0f);
int az_int = (int)(gpimu.m_fAccelZ * 1000.0f);
- HIDO_Debug2("$CAL,%u,%u,%s,%u,%u,%d.%06d,%d.%06d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d,%d,%d,%d,%d,%d\r\n",
+ HIDO_Debug2("$CAL,%u,%u,%s,%u,%u,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%d,%d,%d,%d,%d,%d\r\n",
g_sample_count,
now_ms,
state_name,
throttle_pwm,
steering_pwm,
- lat_deg, lat_frac,
- lon_deg, lon_frac,
- alt_int, alt_frac,
- hdg_int, hdg_frac,
- pitch_int, pitch_frac,
- roll_int, roll_frac,
+ (double)enu[0],
+ (double)enu[1],
+ (double)enu[2],
+ (double)gprmi.m_fHeadingAngle,
+ (double)gprmi.m_fPitchAngle,
+ (double)gprmi.m_fRollAngle,
gx_int, gy_int, gz_int,
ax_int, ay_int, az_int);
g_sample_count++;
@@ -274,7 +360,7 @@
static HIDO_BOOL last_trigger = HIDO_FALSE;
HIDO_Debug2("[CAL] Calibration task started\r\n");
- HIDO_Debug2("[CAL] Trigger: CH7 > 1800 && CH8 < 1500 to start\r\n");
+ HIDO_Debug2("[CAL] Trigger: CH8>1500 & GPS ready to start\r\n");
for (;;)
{
@@ -287,12 +373,16 @@
}
else
{
- /* 绌洪棽鏃舵娴嬪惎鍔ㄦ潯浠讹細CH7 > 1800 && CH8 < 1500 */
- if (SBUS_IsSignalValid(CAL_SBUS_TIMEOUT_MS) == HIDO_TRUE)
+ /* 绌洪棽鏃舵寔缁緭鍑篖OG锛屾柟渚挎煡鐪嬩紶鎰熷櫒鏁版嵁 */
+ HIDO_UINT32 now = HAL_GetTick();
+ Cal_LogData(now, CAL_PWM_CENTER, CAL_PWM_CENTER, "IDLE");
+
+ /* 妫�娴嬪惎鍔ㄦ潯浠讹細CH8浠庘墹1500鍙樹负>1500锛堜笂鍗囨部瑙﹀彂锛� */
+ if (SBUS_IsSignalValid(MOTION_SBUS_TIMEOUT_MS) == HIDO_TRUE)
{
- HIDO_UINT16 ch7 = SBUS_GetChannel(6U); /* CH7绱㈠紩6 */
- HIDO_UINT16 ch8 = SBUS_GetChannel(7U); /* CH8绱㈠紩7 */
- HIDO_BOOL trigger = (ch7 > 1800U) && (ch8 < CAL_SBUS_SAFETY_THRESHOLD);
+ HIDO_UINT16 ch8 = SBUS_GetChannel(MOTION_SBUS_AUTO_CHANNEL);
+ /* 瑙﹀彂鏉′欢锛欳H8 > 1500 */
+ HIDO_BOOL trigger = (ch8 > MOTION_SBUS_AUTO_THRESHOLD_US);
/* 涓婂崌娌胯Е鍙戯紙閬垮厤閲嶅鍚姩锛� */
if (trigger == HIDO_TRUE && last_trigger == HIDO_FALSE)
@@ -348,7 +438,7 @@
/* 瀹夊叏妫�鏌� */
if (Cal_SafetyCheck() == HIDO_FALSE)
{
- HIDO_Debug2("[CAL] Safety check failed! CH8 >= 1500 or SBUS invalid!\r\n");
+ HIDO_Debug2("[CAL] Safety check failed! (SBUS/CH8/GPS not ready)\r\n");
return HIDO_FALSE;
}
@@ -360,7 +450,7 @@
HIDO_Debug2("[CAL] =================================\r\n");
HIDO_Debug2("[CAL] Calibration sequence started!\r\n");
- HIDO_Debug2("[CAL] Safety: CH8 must < 1500 to run\r\n");
+ HIDO_Debug2("[CAL] Safety: CH8>1500 & GPS ready\r\n");
HIDO_Debug2("[CAL] =================================\r\n");
return HIDO_TRUE;
--
Gitblit v1.10.0