/*******************************************************************************
|
* 文件名称 : motion_control_task.c
|
* 文件说明 : 割草机运动控制 FreeRTOS 任务封装
|
* 创建日期 : 2025-11-22
|
*******************************************************************************/
|
|
#include "motion_control_task.h"
|
|
#include "FreeRTOS.h"
|
#include "task.h"
|
|
#include "stm32h7xx_hal.h"
|
#include <string.h>
|
#include <math.h>
|
|
#include "DBG.h"
|
#include "GPS.h"
|
#include "PythonLink.h"
|
#include "geo_utils.h"
|
#include "motion_config.h"
|
#include "motion_control.h"
|
#include "motion_path_data.h"
|
#include "pwm_ctrol.h"
|
#include "motion_mode.h"
|
#include "SBUS.h"
|
#include "AppConfig.h"
|
|
#define RAD2DEG (57.29577951308232f)
|
|
#define MC_TASK_STACK_WORDS (2048U)
|
#define MC_TASK_PRIORITY (tskIDLE_PRIORITY + 4U)
|
#define MC_TASK_PERIOD_TICKS (pdMS_TO_TICKS((HIDO_UINT32)(1000.0f / MC_CFG_CONTROL_HZ)))
|
#define MC_CLAMP(v, lo, hi) ((v) < (lo) ? (lo) : (((v) > (hi)) ? (hi) : (v)))
|
|
extern TaskHandle_t g_app_task_handle;
|
|
static TaskHandle_t g_motion_task_handle = NULL;
|
static MC_State g_motion_state;
|
static MC_Config g_motion_config;
|
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;
|
static HIDO_UINT32 g_last_stack_report_ms = 0U;
|
static HIDO_FLOAT g_control_freq_hz = 0.0f;
|
static HIDO_UINT32 g_freq_sample_count = 0U;
|
static HIDO_UINT32 g_freq_sample_start_ms = 0U;
|
static HIDO_FLOAT g_last_enu[3] = {0.0f, 0.0f, 0.0f};
|
static HIDO_FLOAT g_last_heading_deg = 0.0f;
|
static HIDO_FLOAT g_last_pitch_deg = 0.0f;
|
static HIDO_FLOAT g_last_roll_deg = 0.0f;
|
static HIDO_BOOL g_last_pose_valid = HIDO_FALSE;
|
static HIDO_FLOAT g_last_target_xy[2] = {0.0f, 0.0f};
|
static HIDO_BOOL g_last_target_valid = HIDO_FALSE;
|
static HIDO_UINT16 g_last_steering_pwm = MC_CFG_PWM_CENTER_US;
|
static HIDO_UINT16 g_last_throttle_pwm = MC_CFG_PWM_CENTER_US;
|
|
static HIDO_UINT32 g_last_sensor_timestamp_ms = 0U;
|
|
static void MotionControl_TaskEntry(void *argument);
|
static void MotionControl_ApplyOutput(const MC_Output *output, HIDO_UINT16 *_pu16Steering, HIDO_UINT16 *_pu16Throttle);
|
static void MotionControl_StopOutputs(void);
|
static const HIDO_CHAR *MotionControl_StageLabel(E_MCStage stage);
|
|
/* 初始化控制任务并创建 FreeRTOS 线程（只做一次） */
|
HIDO_VOID MotionControl_TaskInit(HIDO_VOID)
|
{
|
if (g_motion_task_handle != NULL)
|
{
|
return;
|
}
|
|
MC_DefaultConfig(&g_motion_config);
|
Geo_OriginInit(&g_motion_origin,
|
MC_CFG_ORIGIN_LAT_DEG,
|
MC_CFG_ORIGIN_LON_DEG,
|
MC_CFG_ORIGIN_ALT_M);
|
|
MC_Init(&g_motion_state,
|
&g_motion_config,
|
g_motion_path_xy,
|
g_motion_path_point_count);
|
|
DBG_Printf("[MC_INIT] Path loaded: count=%u, first_point=(%.2f,%.2f)\r\n",
|
g_motion_path_point_count,
|
g_motion_path_xy[0],
|
g_motion_path_xy[1]);
|
|
BaseType_t ret = xTaskCreate(
|
MotionControl_TaskEntry,
|
"MotionCtrl",
|
MC_TASK_STACK_WORDS,
|
NULL,
|
MC_TASK_PRIORITY,
|
&g_motion_task_handle);
|
|
if (ret != pdPASS)
|
{
|
g_motion_task_handle = NULL;
|
DBG_Printf("[MotionCtrl] Task create failed\r\n");
|
}
|
}
|
|
/* 查询控制任务是否已经创建 */
|
HIDO_BOOL MotionControl_IsRunning(HIDO_VOID)
|
{
|
return (g_motion_task_handle != NULL) ? HIDO_TRUE : HIDO_FALSE;
|
}
|
|
/* 高优先级任务：采集 → 控制 → PWM 输出 → 反馈 */
|
static void MotionControl_TaskEntry(void *argument)
|
{
|
(void)argument;
|
const TickType_t period = (MC_TASK_PERIOD_TICKS == 0U) ? pdMS_TO_TICKS(13U) : MC_TASK_PERIOD_TICKS;
|
TickType_t last_wake = xTaskGetTickCount();
|
ST_GPRMI gprmi;
|
ST_GPIMU gpimu;
|
float enu[3];
|
|
DBG_Printf("[MotionCtrl] Task started (%.1f Hz)\r\n", MC_CFG_CONTROL_HZ);
|
|
for (;;)
|
{
|
vTaskDelayUntil(&last_wake, period);
|
|
/* --- 1) 采集最新 GPS/IMU 数据（轮询缓存，含超时保护） --- */
|
HIDO_BOOL gps_valid = (GPS_GetGPRMI(&gprmi) == HIDO_OK);
|
HIDO_BOOL imu_valid = (GPS_GetGPIMU(&gpimu) == HIDO_OK);
|
HIDO_UINT32 now = HAL_GetTick();
|
|
if (gps_valid == HIDO_TRUE)
|
{
|
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;
|
|
/* 检测pose_valid状态变化 */
|
if (g_last_pose_valid == HIDO_FALSE)
|
{
|
HIDO_Debug2("[MC_POSE]pose_valid: FALSE->TRUE (GPS recovered)\r\n");
|
}
|
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 (gps_valid == HIDO_FALSE && (now - g_last_gps_ms) > 200U)
|
{
|
/* 检测pose_valid状态变化 */
|
if (g_last_pose_valid == HIDO_TRUE)
|
{
|
HIDO_Debug2("[MC_POSE]pose_valid: TRUE->FALSE (GPS timeout, age=%ums)\r\n",
|
now - g_last_gps_ms);
|
}
|
g_motion_state.pose_valid = HIDO_FALSE;
|
g_last_pose_valid = HIDO_FALSE;
|
}
|
|
if (imu_valid == HIDO_TRUE)
|
{
|
MC_UpdateImu(&g_motion_state, &gpimu);
|
g_last_imu_ms = now;
|
if (gpimu.m_u32UTCTime != 0U)
|
{
|
g_last_sensor_timestamp_ms = gpimu.m_u32UTCTime;
|
}
|
}
|
else if ((now - g_last_imu_ms) > 200U)
|
{
|
g_motion_state.imu_valid = HIDO_FALSE;
|
}
|
|
/* --- 2) 调用运动控制器（固定 75 Hz dt） --- */
|
MC_Output output;
|
HIDO_BOOL sbus_valid = (SBUS_IsSignalValid(MOTION_SBUS_TIMEOUT_MS) == HIDO_TRUE);
|
HIDO_UINT16 ch8_raw = SBUS_GetChannel(MOTION_SBUS_AUTO_CHANNEL);
|
|
/* 过滤SBUS异常值：
|
* 1024 = SBUS_CENTER_VALUE（信号丢失时的默认返回值）
|
* <172 或 >1811 = SBUS有效范围之外（对应PWM 1000-2000）
|
* 当检测到异常值时，保持之前的有效值不变 */
|
static HIDO_UINT16 ch8 = 1000; /* 默认手动模式 */
|
static HIDO_UINT32 s_ch8_failsafe_count = 0;
|
static HIDO_UINT16 s_ch8_last_valid = 1000;
|
|
/* 判断是否为有效SBUS值：172-1811范围内，且不是精确的1024 */
|
HIDO_BOOL is_valid = (ch8_raw >= 172 && ch8_raw <= 1811 && ch8_raw != 1024);
|
|
if (is_valid)
|
{
|
ch8 = ch8_raw; /* 只更新有效值 */
|
s_ch8_last_valid = ch8_raw;
|
if (s_ch8_failsafe_count > 0)
|
{
|
DBG_Printf("[MC_CTRL] CH8 recovered from failsafe, count=%u, new value=%u\r\n",
|
s_ch8_failsafe_count, ch8);
|
s_ch8_failsafe_count = 0;
|
}
|
}
|
else
|
{
|
/* 异常值，保持之前的值，记录failsafe事件 */
|
s_ch8_failsafe_count++;
|
if (s_ch8_failsafe_count == 1)
|
{
|
DBG_Printf("[MC_CTRL] CH8 failsafe detected (%u), keeping previous value=%u\r\n",
|
ch8_raw, ch8);
|
}
|
}
|
|
/* 检查 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);
|
}
|
|
#if HITL_SIMULATION
|
/* 硬件在环仿真模式：
|
* 1. 强制 SBUS 信号有效
|
* 2. 模拟 CH8 通道：上电前 10 秒为 1000 (手动)，10 秒后变为 1800 (自动)
|
* 这样可以测试从手动切换到自动的逻辑 (状态机复位)
|
*/
|
sbus_valid = HIDO_TRUE;
|
|
/* 仿真模式下，只要收到过一次有效数据，就认为 GPS 就绪，避免因为标志位未置位导致不启动 */
|
if (gps_valid == HIDO_TRUE)
|
{
|
gps_ready = HIDO_TRUE;
|
}
|
|
static HIDO_UINT16 g_hitl_ch8 = 1000;
|
static HIDO_UINT32 s_hitl_start_ms = 0;
|
if (s_hitl_start_ms == 0)
|
{
|
s_hitl_start_ms = HAL_GetTick();
|
}
|
|
if (HAL_GetTick() - s_hitl_start_ms > 1000U)
|
{
|
g_hitl_ch8 = 1800; // 10秒后自动切入自动模式
|
}
|
ch8 = g_hitl_ch8;
|
#endif
|
|
/* 只有当满足所有条件时才执行自动控制：
|
* 1. SBUS 信号有效
|
* 2. CH8 > 阈值（自动模式开关，带迟滞）
|
* 3. GPS 初始化完成（FINIT_OK）
|
* 4. GPS 连接到 GNSS（GNSS_CONNECT）*/
|
static HIDO_BOOL s_last_auto_condition = HIDO_FALSE;
|
static HIDO_BOOL s_last_sbus_valid = HIDO_FALSE;
|
static HIDO_BOOL s_last_gps_ready = HIDO_FALSE;
|
static HIDO_UINT16 s_last_ch8 = 0;
|
static HIDO_BOOL s_ch8_auto_state = HIDO_FALSE; /* CH8迟滞状态 */
|
static HIDO_BOOL s_last_ch8_auto_state = HIDO_FALSE; /* 上一次的CH8迟滞状态 */
|
|
/* CH8迟滞：防止在阈值附近抖动时反复触发
|
* 进入自动模式：CH8 > 1600 (高阈值)
|
* 退出自动模式：CH8 < 1400 (低阈值)
|
* 死区：1400-1600，在此区间保持之前的状态
|
*
|
* 连续确认机制：需要连续3次检测到相同状态才切换
|
* 这是最稳定的防抖方案，防止瞬态干扰 */
|
#define CH8_THRESHOLD_ENTER 1600
|
#define CH8_THRESHOLD_EXIT 1400
|
#define CH8_CONFIRM_COUNT 3
|
|
static HIDO_UINT8 s_ch8_enter_count = 0; /* 连续检测到进入自动模式的次数 */
|
static HIDO_UINT8 s_ch8_exit_count = 0; /* 连续检测到退出自动模式的次数 */
|
|
if (s_ch8_auto_state == HIDO_FALSE)
|
{
|
/* 当前在手动模式，检查是否应该进入自动模式 */
|
if (ch8 > CH8_THRESHOLD_ENTER)
|
{
|
s_ch8_enter_count++;
|
s_ch8_exit_count = 0; /* 重置退出计数 */
|
|
if (s_ch8_enter_count >= CH8_CONFIRM_COUNT)
|
{
|
/* 连续3次确认，切换到自动模式 */
|
s_ch8_auto_state = HIDO_TRUE;
|
s_ch8_enter_count = 0;
|
DBG_Printf("[MC_CTRL] CH8 state confirmed: MANUAL -> AUTO (ch8=%u)\r\n", ch8);
|
}
|
}
|
else
|
{
|
s_ch8_enter_count = 0; /* 未满足条件，重置计数 */
|
}
|
}
|
else
|
{
|
/* 当前在自动模式，检查是否应该退出自动模式 */
|
if (ch8 < CH8_THRESHOLD_EXIT)
|
{
|
s_ch8_exit_count++;
|
s_ch8_enter_count = 0; /* 重置进入计数 */
|
|
if (s_ch8_exit_count >= CH8_CONFIRM_COUNT)
|
{
|
/* 连续3次确认，切换到手动模式 */
|
s_ch8_auto_state = HIDO_FALSE;
|
s_ch8_exit_count = 0;
|
DBG_Printf("[MC_CTRL] CH8 state confirmed: AUTO -> MANUAL (ch8=%u)\r\n", ch8);
|
}
|
}
|
else
|
{
|
s_ch8_exit_count = 0; /* 未满足条件，重置计数 */
|
}
|
}
|
|
HIDO_BOOL current_auto_condition = (sbus_valid == HIDO_TRUE && s_ch8_auto_state == HIDO_TRUE && gps_ready == HIDO_TRUE);
|
|
/* 检测到从手动切换到自动模式的上升沿：重置状态机从头开始
|
* 增加稳定性要求：只有在IDLE或FINISHED状态时才允许重新初始化
|
* 如果正在执行任务（GOTO_START或FOLLOW_PATH），忽略模式切换抖动 */
|
if (s_last_auto_condition == HIDO_FALSE && current_auto_condition == HIDO_TRUE)
|
{
|
E_MCStage current_stage = g_motion_state.stage;
|
|
if (current_stage == MC_STAGE_IDLE || current_stage == MC_STAGE_FINISHED)
|
{
|
DBG_Printf("[MC_CTRL] *** Auto mode triggered! Resetting to GOTO_START ***\r\n");
|
DBG_Printf("[MC_CTRL] SBUS: %d->%d, CH8: %u->%u (state=%d->%d), GPS_READY: %d->%d\r\n",
|
s_last_sbus_valid, sbus_valid,
|
s_last_ch8, ch8,
|
s_last_ch8_auto_state, s_ch8_auto_state,
|
s_last_gps_ready, gps_ready);
|
MC_Init(&g_motion_state,
|
&g_motion_config,
|
g_motion_path_xy,
|
g_motion_path_point_count);
|
}
|
else
|
{
|
/* 正在执行任务，忽略此次模式切换（可能是信号抖动） */
|
DBG_Printf("[MC_CTRL] WARNING: Auto mode re-triggered during %d, ignoring (SBUS interference)\r\n",
|
current_stage);
|
}
|
}
|
|
/* 检测条件变化并记录 */
|
if (s_last_sbus_valid != sbus_valid)
|
{
|
DBG_Printf("[MC_CTRL] SBUS valid changed: %d -> %d\r\n", s_last_sbus_valid, sbus_valid);
|
}
|
if (s_last_gps_ready != gps_ready)
|
{
|
DBG_Printf("[MC_CTRL] GPS ready changed: %d -> %d\r\n", s_last_gps_ready, gps_ready);
|
}
|
|
/* 记录状态变化（已在上面连续确认时记录，这里只更新last状态） */
|
s_last_ch8_auto_state = s_ch8_auto_state;
|
|
s_last_auto_condition = current_auto_condition;
|
s_last_sbus_valid = sbus_valid;
|
s_last_gps_ready = gps_ready;
|
s_last_ch8 = ch8;
|
|
if (current_auto_condition == HIDO_TRUE)
|
{
|
/* 记录状态切换 */
|
static E_MCStage s_last_stage = MC_STAGE_IDLE;
|
E_MCStage prev_stage = g_motion_state.stage;
|
|
MC_Compute(&g_motion_state, 1.0f / MC_CFG_CONTROL_HZ, &output);
|
|
if (prev_stage != output.stage)
|
{
|
const HIDO_CHAR *prev_label = MotionControl_StageLabel(prev_stage);
|
const HIDO_CHAR *curr_label = MotionControl_StageLabel(output.stage);
|
DBG_Printf("[MC_CTRL] *** Stage transition: %s -> %s ***\r\n", prev_label, curr_label);
|
|
if (output.stage == MC_STAGE_GOTO_START && prev_stage == MC_STAGE_FOLLOW_PATH)
|
{
|
/* 异常：从follow_path回到goto_start！记录详细信息 */
|
DBG_Printf("[MC_CTRL] WARNING: Unexpected transition from FOLLOW_PATH to GOTO_START!\r\n");
|
DBG_Printf("[MC_CTRL] nearest_idx=%u, path_count=%u, pos=(%.2f,%.2f)\r\n",
|
g_motion_state.nearest_index,
|
g_motion_state.path_count,
|
g_motion_state.pos[0],
|
g_motion_state.pos[1]);
|
}
|
}
|
s_last_stage = output.stage;
|
}
|
else
|
{
|
/* 手动模式或条件不满足：重置输出并让出控制权 */
|
memset(&output, 0, sizeof(MC_Output));
|
output.active = HIDO_FALSE;
|
|
/* 移除这里的强制回中，改为在下方根据具体原因处理 */
|
// MotionControl_StopOutputs();
|
|
/* 定期打印状态提示 */
|
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 - (float)fwd_int) * 100.0f);
|
/* 修复：保留turn的符号，使用带符号格式输出 */
|
float turn_abs = fabsf(output.turn_rate);
|
int turn_int = (int)turn_abs;
|
int turn_frac = (int)((turn_abs - (float)turn_int) * 100.0f);
|
char turn_sign = (output.turn_rate < 0.0f) ? '-' : '+';
|
|
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=%c%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_sign, turn_int, turn_frac,
|
g_motion_state.nearest_index);
|
}
|
|
/* --- 3) 根据控制量更新 PWM，并把 forward/turn 回传给 Python --- */
|
HIDO_UINT16 applied_steering = MC_CFG_PWM_CENTER_US;
|
HIDO_UINT16 applied_throttle = MC_CFG_PWM_CENTER_US;
|
if (output.active == HIDO_TRUE)
|
{
|
MotionControl_ApplyOutput(&output, &applied_steering, &applied_throttle);
|
}
|
else
|
{
|
/* 非自动模式下，MotionControl_StopOutputs() 已在上面调用，这里不再重复设置
|
* 如果需要手动模式直通，应该在 SBUS 模块处理，或者在这里读取 SBUS 手动通道值并输出
|
* 目前逻辑是：非自动模式下，运动控制任务输出回中（安全停止），手动控制由 SBUS 回调或其他机制接管
|
* 如果系统设计是 MotionControl 任务在手动模式下也负责透传遥控器信号，则需要修改此处。
|
* 假设：手动模式下，SBUS 模块的中断回调会直接控制电机，或者有其他任务处理。
|
* 为防止冲突，这里仅保持记录。但上面已经调用 StopOutputs 设置了 PWM 为中值。
|
* 如果 SBUS 直接控制电机，这里的 StopOutputs 会与 SBUS 冲突吗？
|
* 通常做法：自动模式下本任务控制 PWM；手动模式下本任务不控制 PWM（或者输出无效值）。
|
* 但 StopOutputs 显式设置了 1500。如果 SBUS 也是设置 PWM，会有竞争。
|
*
|
* 修正：如果 ch8 < 1500 (手动模式)，本任务不应该 Set_Motor_Pulse(1500)，
|
* 否则会覆盖 SBUS 的手动控制信号。
|
*
|
* 修改逻辑：
|
* 1. 自动模式 (current_auto_condition == TRUE): ApplyOutput -> Set_Pulse
|
* 2. 异常停止 (sbus_valid=FALSE 或 gps_ready=FALSE): StopOutputs -> Set_Pulse(1500)
|
* 3. 手动模式 (sbus_valid=TRUE && ch8 < 1500): 不操作 PWM，让权给手动控制逻辑
|
*/
|
|
if (sbus_valid == HIDO_FALSE)
|
{
|
/* 遥控器信号丢失：强制停车 */
|
MotionControl_StopOutputs();
|
applied_steering = MC_CFG_PWM_CENTER_US;
|
applied_throttle = MC_CFG_PWM_CENTER_US;
|
}
|
else if (ch8 <= MOTION_SBUS_AUTO_THRESHOLD_US)
|
{
|
/* 手动模式：不输出 PWM，仅记录（假设 SBUS 模块或其他逻辑在控制电机）
|
* 为了回显正确，这里赋值为 1500 或读取当前 PWM（如果能读到）
|
*/
|
applied_steering = MC_CFG_PWM_CENTER_US;
|
applied_throttle = MC_CFG_PWM_CENTER_US;
|
/* 注意：不要调用 MotionControl_StopOutputs()，否则会在此处把手动油门强行归零 */
|
}
|
else if (gps_ready == HIDO_FALSE)
|
{
|
/* 自动模式开关打开，但 GPS 未就绪：安全停车 */
|
MotionControl_StopOutputs();
|
applied_steering = MC_CFG_PWM_CENTER_US;
|
applied_throttle = MC_CFG_PWM_CENTER_US;
|
}
|
}
|
g_last_steering_pwm = applied_steering;
|
g_last_throttle_pwm = applied_throttle;
|
|
memcpy(g_last_enu, output.pos_enu, sizeof(g_last_enu));
|
g_last_heading_deg = output.heading_deg;
|
g_last_pitch_deg = output.pitch_deg;
|
g_last_roll_deg = output.roll_deg;
|
g_last_pose_valid = g_motion_state.pose_valid;
|
|
/* 检测目标点跳变到(0,0)的情况 */
|
static HIDO_BOOL s_warn_zero_target = HIDO_FALSE;
|
if (output.target_valid == HIDO_TRUE)
|
{
|
/* 检测目标点突然跳到接近(0,0)的情况 */
|
HIDO_BOOL is_near_zero = (fabsf(output.target_xy[0]) < 0.1f && fabsf(output.target_xy[1]) < 0.1f);
|
HIDO_BOOL was_far_from_zero = (fabsf(g_last_target_xy[0]) > 1.0f || fabsf(g_last_target_xy[1]) > 1.0f);
|
|
if (is_near_zero && was_far_from_zero && g_last_target_valid)
|
{
|
int old_x_int = (int)g_last_target_xy[0];
|
int old_x_frac = (int)(fabsf(g_last_target_xy[0] - old_x_int) * 100);
|
int old_y_int = (int)g_last_target_xy[1];
|
int old_y_frac = (int)(fabsf(g_last_target_xy[1] - old_y_int) * 100);
|
|
HIDO_Debug2("[MC_TGT]WARNING: Target jumped to (0,0)! Previous target=(%d.%02d,%d.%02d) stage=%d\r\n",
|
old_x_int, old_x_frac, old_y_int, old_y_frac, output.stage);
|
s_warn_zero_target = HIDO_TRUE;
|
}
|
else if (!is_near_zero && s_warn_zero_target)
|
{
|
int new_x_int = (int)output.target_xy[0];
|
int new_x_frac = (int)(fabsf(output.target_xy[0] - new_x_int) * 100);
|
int new_y_int = (int)output.target_xy[1];
|
int new_y_frac = (int)(fabsf(output.target_xy[1] - new_y_int) * 100);
|
|
HIDO_Debug2("[MC_TGT]Target recovered from (0,0) to (%d.%02d,%d.%02d)\r\n",
|
new_x_int, new_x_frac, new_y_int, new_y_frac);
|
s_warn_zero_target = HIDO_FALSE;
|
}
|
|
g_last_target_xy[0] = output.target_xy[0];
|
g_last_target_xy[1] = output.target_xy[1];
|
g_last_target_valid = HIDO_TRUE;
|
}
|
else
|
{
|
/* target_valid变为FALSE */
|
if (g_last_target_valid)
|
{
|
HIDO_Debug2("[MC_TGT]target_valid: TRUE->FALSE in task (will report 0,0)\r\n");
|
}
|
g_last_target_valid = HIDO_FALSE;
|
}
|
|
g_freq_sample_count++;
|
if (g_freq_sample_start_ms == 0U)
|
{
|
g_freq_sample_start_ms = now;
|
}
|
else
|
{
|
HIDO_UINT32 window = now - g_freq_sample_start_ms;
|
if (window >= 1000U)
|
{
|
if (window > 0U)
|
{
|
g_control_freq_hz = (HIDO_FLOAT)g_freq_sample_count * 1000.0f / (HIDO_FLOAT)window;
|
}
|
g_freq_sample_count = 0U;
|
g_freq_sample_start_ms = now;
|
}
|
}
|
|
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,
|
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;
|
PythonLink_ReportState(stage_label,
|
output.cross_track_error,
|
output.heading_error * RAD2DEG,
|
g_last_sensor_timestamp_ms);
|
}
|
|
if ((now - g_last_pose_report_ms) >= 1000U && g_last_pose_valid == HIDO_TRUE)
|
{
|
g_last_pose_report_ms = now;
|
PythonLink_ReportPose(g_last_enu,
|
g_last_heading_deg,
|
g_last_pitch_deg,
|
g_last_roll_deg,
|
g_last_target_valid ? g_last_target_xy : NULL,
|
g_last_sensor_timestamp_ms);
|
}
|
|
if ((now - g_last_stack_report_ms) >= 10000U)
|
{
|
g_last_stack_report_ms = now;
|
UBaseType_t motion_hw = uxTaskGetStackHighWaterMark(g_motion_task_handle);
|
UBaseType_t app_hw = (g_app_task_handle != NULL) ? uxTaskGetStackHighWaterMark(g_app_task_handle) : 0U;
|
UBaseType_t heap_free = xPortGetFreeHeapSize();
|
UBaseType_t heap_min = xPortGetMinimumEverFreeHeapSize();
|
PythonLink_ReportStack("Motion", (HIDO_UINT32)motion_hw, (HIDO_UINT32)heap_free, (HIDO_UINT32)heap_min);
|
if (g_app_task_handle != NULL)
|
{
|
PythonLink_ReportStack("App", (HIDO_UINT32)app_hw, (HIDO_UINT32)heap_free, (HIDO_UINT32)heap_min);
|
}
|
}
|
}
|
}
|
|
/* 将控制输出映射为 PWM（使用校准模型反解）*/
|
static void MotionControl_ApplyOutput(const MC_Output *output,
|
HIDO_UINT16 *_pu16Steering,
|
HIDO_UINT16 *_pu16Throttle)
|
{
|
/* ========== 前向速度 → 油门PWM（使用校准模型反解）========== */
|
/* 校准模型：v = MC_CFG_FORWARD_K × (1500 - pwm) + MC_CFG_FORWARD_BIAS
|
* 反解：pwm = 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_f = 1000.0f;
|
}
|
else if (throttle_f > 2000.0f)
|
{
|
throttle_f = 2000.0f;
|
}
|
|
/* 死区处理：接近1500时直接设为1500 */
|
if (fabsf(throttle_f - 1500.0f) < (float)MC_CFG_FORWARD_DEADZONE_PWM)
|
{
|
throttle_f = 1500.0f;
|
}
|
|
/* ========== 转向角速度 → 转向PWM（使用校准模型反解）========== */
|
/* 校准模型：ω_left = MC_CFG_STEERING_K_LEFT × (1500 - pwm) 当 pwm < 1500 (左转)
|
* ω_right = -MC_CFG_STEERING_K_RIGHT × (pwm - 1500) 当 pwm > 1500 (右转)
|
* 反解：pwm_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;
|
|
/* 调试LOG：打印转向映射过程（低频） */
|
|
if (target_yaw_rate > 0.001f) // 左转（正角速度）
|
{
|
steering_f = 1500.0f - target_yaw_rate / MC_CFG_STEERING_K_LEFT;
|
if (steering_f < 1000.0f)
|
{
|
steering_f = 1000.0f;
|
}
|
}
|
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 // 直行：应用转向补偿修正机械偏差（死区 ±0.001 rad/s）
|
{
|
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)
|
{
|
*_pu16Steering = (HIDO_UINT16)steering;
|
}
|
if (_pu16Throttle != NULL)
|
{
|
*_pu16Throttle = (HIDO_UINT16)throttle;
|
}
|
}
|
|
|
/* 失效保护：回中 PWM 并上报零控制量 */
|
static void MotionControl_StopOutputs(void)
|
{
|
Set_Motor_Pulse(MC_CFG_PWM_CENTER_US);
|
Set_Steering_Pulse(MC_CFG_PWM_CENTER_US);
|
g_last_steering_pwm = MC_CFG_PWM_CENTER_US;
|
g_last_throttle_pwm = MC_CFG_PWM_CENTER_US;
|
}
|
|
static const HIDO_CHAR *MotionControl_StageLabel(E_MCStage stage)
|
{
|
switch (stage)
|
{
|
case MC_STAGE_GOTO_START:
|
return "goto_start";
|
case MC_STAGE_FOLLOW_PATH:
|
return "follow_path";
|
case MC_STAGE_FINISHED:
|
return "finished";
|
default:
|
return "idle";
|
}
|
}
|
