/*******************************************************************************
|
* 文件名称 : motion_control.c
|
* 文件说明 : 割草机纯追踪运动控制器（C 语言实现）
|
* 创建日期 : 2025-11-22
|
*******************************************************************************/
|
|
#include "motion_control.h"
|
|
#include <math.h>
|
#include <string.h>
|
|
#include "arm_math.h"
|
|
#include "motion_config.h"
|
|
#ifndef M_PI
|
#define M_PI (3.14159265358979323846f)
|
#endif
|
|
#define DEG2RAD (0.017453292519943295769236907684886f)
|
#define RAD2DEG (57.295779513082320876798154814105f)
|
#define MC_CLAMP(v, lo, hi) ((v) < (lo) ? (lo) : (((v) > (hi)) ? (hi) : (v)))
|
/* 当航向误差逼近 ±π 时的判决窗口（约 0.5°） */
|
#define MC_HEADING_PI_EPS (0.00872664626f)
|
|
/* 将角度约束到 [-pi, pi]，避免航向跳变 */
|
static float mc_wrap_angle(float angle)
|
{
|
while (angle > (float)M_PI)
|
{
|
angle -= 2.0f * (float)M_PI;
|
}
|
while (angle < -(float)M_PI)
|
{
|
angle += 2.0f * (float)M_PI;
|
}
|
return angle;
|
}
|
|
/* 在误差接近 ±π 时保持上一周期的转向方向，避免来回反转 */
|
static float mc_heading_error_with_hysteresis(MC_State *state, float desired_heading)
|
{
|
float err = mc_wrap_angle(desired_heading - state->heading_rad);
|
float abs_err = fabsf(err);
|
float pi_delta = (float)M_PI - abs_err;
|
|
if (pi_delta <= MC_HEADING_PI_EPS)
|
{
|
float preferred_sign = state->last_turn_sign;
|
if (preferred_sign == 0.0f)
|
{
|
preferred_sign = (err >= 0.0f) ? 1.0f : -1.0f;
|
}
|
float old_err = err;
|
err = preferred_sign * abs_err;
|
state->last_turn_sign = preferred_sign;
|
|
/* DEBUG: 输出滞后逻辑触发 */
|
extern void HIDO_Debug2(const char *format, ...);
|
HIDO_Debug2("[MC_DBG] HYSTERESIS: pi_delta=%.4f old_err=%.4f new_err=%.4f sign=%.1f\r\n",
|
pi_delta, old_err, err, preferred_sign);
|
}
|
else if (abs_err > 1.0e-6f)
|
{
|
state->last_turn_sign = (err > 0.0f) ? 1.0f : -1.0f;
|
}
|
|
return err;
|
}
|
|
/* 读取路径点，索引越界时截到末尾 */
|
static void mc_get_point(const MC_State *state, HIDO_UINT32 idx, float outPt[2])
|
{
|
if (idx >= state->path_count)
|
{
|
idx = state->path_count - 1U;
|
}
|
outPt[0] = state->path_xy[idx * 2U];
|
outPt[1] = state->path_xy[idx * 2U + 1U];
|
}
|
|
/* 计算两个二维点的平方距离 */
|
static float mc_distance_sq(const float a[2], const float b[2])
|
{
|
float dx = a[0] - b[0];
|
float dy = a[1] - b[1];
|
return dx * dx + dy * dy;
|
}
|
|
/* 计算两点距离（含 sqrt 防护） */
|
static float mc_distance(const float a[2], const float b[2])
|
{
|
float dist_sq = mc_distance_sq(a, b);
|
float dist = 0.0f;
|
if (dist_sq > 0.0f)
|
{
|
arm_sqrt_f32(dist_sq, &dist);
|
}
|
return dist;
|
}
|
|
/* 数学坐标（东为 0°，逆时针为正）→ 罗盘坐标（北为 0°，顺时针为正） */
|
static float mc_math_rad_to_compass_deg(float heading_rad)
|
{
|
float compass_deg = 90.0f - (heading_rad * RAD2DEG);
|
while (compass_deg >= 360.0f)
|
{
|
compass_deg -= 360.0f;
|
}
|
while (compass_deg < 0.0f)
|
{
|
compass_deg += 360.0f;
|
}
|
return compass_deg;
|
}
|
|
/* 罗盘坐标（北为 0°，顺时针为正）→ 数学坐标（东为 0°，逆时针为正） */
|
static float mc_compass_deg_to_math_rad(float heading_deg)
|
{
|
float math_deg = 90.0f - heading_deg;
|
return mc_wrap_angle(math_deg * DEG2RAD);
|
}
|
|
/* 搜索当前位置最近的路径点 */
|
static HIDO_UINT32 mc_find_nearest(const MC_State *state)
|
{
|
float best_dist = 1.0e9f;
|
HIDO_UINT32 best_idx = state->nearest_index;
|
float pos[2] = {state->pos[0], state->pos[1]};
|
|
for (HIDO_UINT32 i = 0; i < state->path_count; i++)
|
{
|
float point[2];
|
mc_get_point(state, i, point);
|
float dist_sq = mc_distance_sq(point, pos);
|
if (dist_sq < best_dist)
|
{
|
best_dist = dist_sq;
|
best_idx = i;
|
}
|
}
|
return best_idx;
|
}
|
|
/* 沿路径累积距离，直到达到前视目标 */
|
static HIDO_UINT32 mc_find_lookahead(const MC_State *state, HIDO_UINT32 start_idx, float lookahead_m)
|
{
|
float accum = 0.0f;
|
float current[2];
|
mc_get_point(state, start_idx, current);
|
|
for (HIDO_UINT32 i = start_idx; i < state->path_count - 1U; i++)
|
{
|
float next[2];
|
mc_get_point(state, i + 1U, next);
|
float seg = mc_distance(current, next);
|
accum += seg;
|
if (accum >= lookahead_m)
|
{
|
return i + 1U;
|
}
|
current[0] = next[0];
|
current[1] = next[1];
|
}
|
return state->path_count - 1U;
|
}
|
|
/* 计算带符号横向误差（左正右负） */
|
static float mc_cross_track_error(const MC_State *state, HIDO_UINT32 nearest_idx)
|
{
|
if (state->path_count < 2U)
|
{
|
return 0.0f;
|
}
|
|
HIDO_UINT32 idx0 = (nearest_idx == 0U) ? 0U : nearest_idx - 1U;
|
HIDO_UINT32 idx1 = (nearest_idx + 1U < state->path_count) ? nearest_idx + 1U : nearest_idx;
|
|
float p0[2];
|
float p1[2];
|
float pos[2] = {state->pos[0], state->pos[1]};
|
mc_get_point(state, idx0, p0);
|
mc_get_point(state, idx1, p1);
|
|
float vx = p1[0] - p0[0];
|
float vy = p1[1] - p0[1];
|
float wx = pos[0] - p0[0];
|
float wy = pos[1] - p0[1];
|
float seg_len_sq = vx * vx + vy * vy;
|
|
float t = 0.0f;
|
if (seg_len_sq > 0.0f)
|
{
|
t = (wx * vx + wy * vy) / seg_len_sq;
|
}
|
if (t < 0.0f)
|
{
|
t = 0.0f;
|
}
|
else if (t > 1.0f)
|
{
|
t = 1.0f;
|
}
|
|
float proj_x = p0[0] + vx * t;
|
float proj_y = p0[1] + vy * t;
|
float dx = pos[0] - proj_x;
|
float dy = pos[1] - proj_y;
|
float dist_sq = dx * dx + dy * dy;
|
float dist = 0.0f;
|
if (dist_sq > 0.0f)
|
{
|
arm_sqrt_f32(dist_sq, &dist);
|
}
|
float cross = vx * (pos[1] - p0[1]) - vy * (pos[0] - p0[0]);
|
return (cross >= 0.0f) ? dist : -dist;
|
}
|
|
/* 阶段：旋转/接近起点，使车准备好跟踪路径 */
|
static void mc_compute_goto_start(MC_State *state, float dt_s, MC_Output *out)
|
{
|
float start_point[2];
|
mc_get_point(state, 0U, start_point);
|
state->current_target_xy[0] = start_point[0];
|
state->current_target_xy[1] = start_point[1];
|
out->target_valid = HIDO_TRUE;
|
out->target_xy[0] = start_point[0];
|
out->target_xy[1] = start_point[1];
|
float vec_x = start_point[0] - state->pos[0];
|
float vec_y = start_point[1] - state->pos[1];
|
float dist = mc_distance(start_point, state->pos);
|
|
/* DEBUG: 在调用atan2前输出参数 */
|
extern void HIDO_Debug2(const char *format, ...);
|
HIDO_Debug2("[MC_DBG] ATAN2_IN: vec_y=%.6f vec_x=%.6f\r\n", vec_y, vec_x);
|
|
/* 直接调用atan2f测试 */
|
extern float atan2f(float y, float x);
|
float test_direct = atan2f(vec_y, vec_x);
|
HIDO_Debug2("[MC_DBG] Direct atan2f(vec_y, vec_x) = %.6f\r\n", test_direct);
|
|
/* 使用新的DSP库API: arm_atan2_f32(y, x, result) */
|
float desired_heading;
|
arm_status status = arm_atan2_f32(vec_y, vec_x, &desired_heading);
|
if (status != ARM_MATH_SUCCESS) {
|
HIDO_Debug2("[MC_DBG] WARNING: arm_atan2_f32 failed with status %d\r\n", status);
|
}
|
|
/* DEBUG: 测试不同的输出方式 */
|
float test_val = desired_heading;
|
int int_part = (int)test_val;
|
float frac_part = (test_val - (float)int_part) * 1000.0f;
|
HIDO_Debug2("[MC_DBG] ATAN2_OUT: arm_atan2_f32=%d.%03d rad\r\n",
|
int_part, (int)frac_part);
|
HIDO_Debug2("[MC_DBG] ATAN2_OUT: in degrees = %.2f\r\n",
|
desired_heading * RAD2DEG);
|
|
/* 测试已知值 */
|
float test_atan2;
|
arm_atan2_f32(1.0f, 1.0f, &test_atan2);
|
float test_direct2 = atan2f(1.0f, 1.0f);
|
HIDO_Debug2("[MC_DBG] TEST: arm_atan2(1,1)=%.6f atan2f(1,1)=%.6f (should be ~0.785)\r\n",
|
test_atan2, test_direct2);
|
out->target_heading_deg = mc_math_rad_to_compass_deg(desired_heading);
|
float heading_err = mc_heading_error_with_hysteresis(state, desired_heading);
|
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);
|
|
/* DEBUG: 输出goto_start控制计算 */
|
extern void HIDO_Debug2(const char *format, ...);
|
HIDO_Debug2("[MC_DBG] GOTO: pos=(%.2f,%.2f) tgt=(%.2f,%.2f) vec=(%.2f,%.2f) des_hdg_rad=%.4f\r\n",
|
state->pos[0], state->pos[1], start_point[0], start_point[1],
|
vec_x, vec_y, desired_heading);
|
HIDO_Debug2("[MC_DBG] GOTO: cur_hdg=%.2f tgt_hdg=%.2f err=%.4f turn_sign=%.1f yaw_cmd=%.4f\r\n",
|
mc_math_rad_to_compass_deg(state->heading_rad),
|
out->target_heading_deg,
|
heading_err,
|
state->last_turn_sign,
|
yaw_rate_cmd);
|
|
float forward = 0.0f;
|
if (dist > state->config.start_pos_tolerance_m)
|
{
|
if (fabsf(heading_err) < state->config.start_heading_tolerance_rad)
|
{
|
float desired = state->config.base_speed_mps + 0.5f * dist;
|
forward = MC_CLAMP(desired,
|
state->config.min_follow_speed_mps,
|
state->config.max_forward_mps);
|
}
|
}
|
else if (fabsf(heading_err) < state->config.start_heading_tolerance_rad)
|
{
|
state->stage = MC_STAGE_FOLLOW_PATH;
|
}
|
|
out->forward_mps = forward;
|
out->turn_rate = yaw_rate_cmd;
|
out->heading_error = heading_err;
|
out->cross_track_error = dist;
|
out->active = (forward != 0.0f) || (fabsf(yaw_rate_cmd) > 0.01f);
|
(void)dt_s;
|
}
|
|
/* 阶段：纯追踪 + 航向/横向误差补偿 */
|
static void mc_compute_follow_path(MC_State *state, float dt_s, MC_Output *out)
|
{
|
HIDO_UINT32 nearest_idx = mc_find_nearest(state);
|
state->nearest_index = nearest_idx;
|
|
float lookahead = state->config.lookahead_min_m;
|
float speed_ratio = MC_CLAMP(state->speed_mps / state->config.max_forward_mps, 0.0f, 1.0f);
|
lookahead += (state->config.lookahead_max_m - state->config.lookahead_min_m) * speed_ratio;
|
HIDO_UINT32 lookahead_idx = mc_find_lookahead(state, nearest_idx, lookahead);
|
state->lookahead_index = lookahead_idx;
|
|
float target[2];
|
mc_get_point(state, lookahead_idx, target);
|
state->current_target_xy[0] = target[0];
|
state->current_target_xy[1] = target[1];
|
out->target_valid = HIDO_TRUE;
|
out->target_xy[0] = target[0];
|
out->target_xy[1] = target[1];
|
|
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, &desired_heading);
|
out->target_heading_deg = mc_math_rad_to_compass_deg(desired_heading);
|
float heading_err = mc_heading_error_with_hysteresis(state, desired_heading);
|
float heading_err_rate = (heading_err - state->last_heading_err) / (dt_s > 0.0f ? dt_s : 0.013f);
|
state->last_heading_err = heading_err;
|
|
float cross_track = mc_cross_track_error(state, nearest_idx);
|
float yaw_rate_cmd = state->config.heading_kp * heading_err
|
+ state->config.heading_kd * heading_err_rate
|
+ state->config.xtrack_kp * cross_track;
|
yaw_rate_cmd = MC_CLAMP(yaw_rate_cmd, -state->config.max_turn_rate, state->config.max_turn_rate);
|
|
/* DEBUG: 输出follow_path控制计算 */
|
extern void HIDO_Debug2(const char *format, ...);
|
HIDO_Debug2("[MC_DBG] FOLLOW: cur_hdg=%.2f tgt_hdg=%.2f err=%.4f turn_sign=%.1f yaw_cmd=%.4f xtrack=%.3f\r\n",
|
mc_math_rad_to_compass_deg(state->heading_rad),
|
out->target_heading_deg,
|
heading_err,
|
state->last_turn_sign,
|
yaw_rate_cmd,
|
cross_track);
|
|
float forward = state->config.base_speed_mps
|
- state->config.heading_speed_scale * fabsf(heading_err)
|
- state->config.xtrack_speed_scale * fabsf(cross_track);
|
forward = MC_CLAMP(forward,
|
state->config.min_follow_speed_mps,
|
state->config.max_forward_mps);
|
|
float goal[2];
|
mc_get_point(state, state->path_count - 1U, goal);
|
float goal_dist = mc_distance(goal, state->pos);
|
if (goal_dist < state->config.goal_tolerance_m)
|
{
|
state->stage = MC_STAGE_FINISHED;
|
forward = 0.0f;
|
yaw_rate_cmd = 0.0f;
|
}
|
|
out->forward_mps = forward;
|
out->turn_rate = yaw_rate_cmd;
|
out->heading_error = heading_err;
|
out->cross_track_error = cross_track;
|
out->active = (state->stage != MC_STAGE_FINISHED);
|
}
|
|
/* 填充默认配置 */
|
HIDO_VOID MC_DefaultConfig(MC_Config *_cfg)
|
{
|
if (_cfg == HIDO_NULL)
|
{
|
return;
|
}
|
_cfg->max_forward_mps = MC_CFG_MAX_FORWARD_MPS;
|
_cfg->max_reverse_mps = MC_CFG_MAX_REVERSE_MPS;
|
_cfg->max_turn_rate = MC_CFG_MAX_TURN_RATE_RAD;
|
_cfg->base_speed_mps = MC_CFG_BASE_SPEED_MPS;
|
_cfg->min_follow_speed_mps = MC_CFG_MIN_FOLLOW_SPEED_MPS;
|
_cfg->lookahead_min_m = MC_CFG_LOOKAHEAD_MIN_M;
|
_cfg->lookahead_max_m = MC_CFG_LOOKAHEAD_MAX_M;
|
_cfg->start_pos_tolerance_m = MC_CFG_START_POS_TOL_M;
|
_cfg->start_heading_tolerance_rad = MC_CFG_START_HEADING_TOL_RAD;
|
_cfg->goal_tolerance_m = MC_CFG_GOAL_TOL_M;
|
_cfg->heading_kp = MC_CFG_HEADING_KP;
|
_cfg->heading_kd = MC_CFG_HEADING_KD;
|
_cfg->xtrack_kp = MC_CFG_XTRACK_KP;
|
_cfg->heading_speed_scale = MC_CFG_HEADING_SPEED_SCALE;
|
_cfg->xtrack_speed_scale = MC_CFG_XTRACK_SPEED_SCALE;
|
}
|
|
/* 初始化控制器，绑定路径与配置 */
|
HIDO_VOID MC_Init(MC_State *_state, const MC_Config *_cfg, const float *_path_xy, HIDO_UINT32 _point_count)
|
{
|
if (_state == HIDO_NULL || _cfg == HIDO_NULL || _path_xy == HIDO_NULL || _point_count < 2U)
|
{
|
return;
|
}
|
|
memset(_state, 0, sizeof(MC_State));
|
_state->config = *_cfg;
|
_state->path_xy = _path_xy;
|
_state->path_count = _point_count;
|
_state->stage = MC_STAGE_GOTO_START;
|
_state->nearest_index = 0U;
|
_state->lookahead_index = 0U;
|
}
|
|
/* 注入最新 ENU 位姿及航向/速度 */
|
HIDO_VOID MC_UpdateGps(MC_State *_state, const float _enu[3], const ST_GPRMI *_gprmi)
|
{
|
if (_state == HIDO_NULL || _enu == HIDO_NULL || _gprmi == HIDO_NULL)
|
{
|
return;
|
}
|
|
_state->pos[0] = _enu[0];
|
_state->pos[1] = _enu[1];
|
_state->pos[2] = _enu[2];
|
float old_heading_deg = _state->heading_deg;
|
float gps_heading_rad = mc_compass_deg_to_math_rad(_gprmi->m_fHeadingAngle);
|
_state->heading_rad = gps_heading_rad;
|
_state->heading_deg = mc_math_rad_to_compass_deg(_state->heading_rad);
|
|
/* DEBUG: 输出GPS更新信息（包含弧度值以便调试） */
|
extern void HIDO_Debug2(const char *format, ...);
|
float old_heading_rad = mc_compass_deg_to_math_rad(old_heading_deg);
|
HIDO_Debug2("[MC_DBG] GPS: old_hdg=%.2f(old_rad=%.6f) gps_hdg=%.2f(gps_rad=%.6f) new_hdg=%.2f(new_rad=%.6f)\r\n",
|
old_heading_deg, old_heading_rad,
|
_gprmi->m_fHeadingAngle, gps_heading_rad,
|
_state->heading_deg, _state->heading_rad);
|
_state->pitch_deg = _gprmi->m_fPitchAngle;
|
_state->roll_deg = _gprmi->m_fRollAngle;
|
_state->vel[0] = _gprmi->m_fEastVelocity;
|
_state->vel[1] = _gprmi->m_fNorthVelocity;
|
float speed_sq = _state->vel[0] * _state->vel[0] + _state->vel[1] * _state->vel[1];
|
float speed = 0.0f;
|
if (speed_sq > 0.0f)
|
{
|
arm_sqrt_f32(speed_sq, &speed);
|
}
|
_state->speed_mps = speed;
|
_state->pose_valid = HIDO_TRUE;
|
}
|
|
/* 更新 IMU 陀螺 Z 轴（yaw 角速度） */
|
HIDO_VOID MC_UpdateImu(MC_State *_state, const ST_GPIMU *_gpimu)
|
{
|
if (_state == HIDO_NULL || _gpimu == HIDO_NULL)
|
{
|
return;
|
}
|
|
/* GyroZ符号定义：顺时针正，逆时针负（从上往下看Z轴，右手螺旋定则）
|
* 数学坐标系：东为0°，逆时针为正（CCW>0）
|
* 所以：GyroZ正值（顺时针）→ yaw_rate_rad负值，GyroZ负值（逆时针）→ yaw_rate_rad正值
|
* 需要取反：yaw_rate_rad = -GyroZ * DEG2RAD */
|
_state->yaw_rate_rad = -(_gpimu->m_fGyroZ * DEG2RAD);
|
_state->imu_valid = HIDO_TRUE;
|
|
/* DEBUG: 输出IMU更新信息 */
|
extern void HIDO_Debug2(const char *format, ...);
|
HIDO_Debug2("[MC_DBG] IMU: GyroZ=%.2f deg/s, yaw_rate_rad=%.4f\r\n",
|
_gpimu->m_fGyroZ, _state->yaw_rate_rad);
|
}
|
|
/* 入口：根据阶段计算控制量 */
|
HIDO_VOID MC_Compute(MC_State *_state, float _dt_s, MC_Output *_out)
|
{
|
if (_state == HIDO_NULL || _out == HIDO_NULL)
|
{
|
return;
|
}
|
|
memset(_out, 0, sizeof(MC_Output));
|
_out->target_valid = HIDO_FALSE;
|
_out->target_xy[0] = 0.0f;
|
_out->target_xy[1] = 0.0f;
|
|
if (_state->path_xy == HIDO_NULL || _state->path_count < 2U)
|
{
|
return;
|
}
|
|
if (_state->pose_valid == HIDO_FALSE)
|
{
|
return;
|
}
|
|
/* DEBUG: 输出计算开始状态（每50次输出一次，避免刷屏） */
|
static HIDO_UINT32 dbg_counter = 0;
|
dbg_counter++;
|
if (dbg_counter % 50 == 0)
|
{
|
extern void HIDO_Debug2(const char *format, ...);
|
HIDO_Debug2("[MC_DBG] COMPUTE: pose_valid=%d imu_valid=%d dt=%.4f hdg=%.2f yaw_rate=%.4f\r\n",
|
_state->pose_valid, _state->imu_valid, _dt_s,
|
_state->heading_deg, _state->yaw_rate_rad);
|
}
|
|
if (_state->imu_valid == HIDO_TRUE && _dt_s > 0.0f)
|
{
|
float old_heading_rad = _state->heading_rad;
|
float old_heading_deg = _state->heading_deg;
|
float delta_rad = _state->yaw_rate_rad * _dt_s;
|
_state->heading_rad = mc_wrap_angle(_state->heading_rad + delta_rad);
|
float new_heading_rad = _state->heading_rad;
|
_state->heading_deg = mc_math_rad_to_compass_deg(_state->heading_rad);
|
|
/* DEBUG: 输出积分过程（包含弧度值以便调试） */
|
extern void HIDO_Debug2(const char *format, ...);
|
HIDO_Debug2("[MC_DBG] INTEG: dt=%.4f yaw_rate=%.4f delta_rad=%.6f old_rad=%.6f new_rad=%.6f old_hdg=%.2f new_hdg=%.2f delta_deg=%.2f\r\n",
|
_dt_s, _state->yaw_rate_rad, delta_rad, old_heading_rad, new_heading_rad,
|
old_heading_deg, _state->heading_deg,
|
_state->heading_deg - old_heading_deg);
|
}
|
|
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:
|
mc_compute_goto_start(_state, _dt_s, _out);
|
break;
|
case MC_STAGE_FOLLOW_PATH:
|
mc_compute_follow_path(_state, _dt_s, _out);
|
break;
|
case MC_STAGE_FINISHED:
|
_out->active = HIDO_FALSE;
|
_out->target_heading_deg = _state->heading_deg;
|
break;
|
default:
|
break;
|
}
|
|
_out->turn_rate = -_out->turn_rate;
|
_out->stage = _state->stage;
|
_out->pos_enu[0] = _state->pos[0];
|
_out->pos_enu[1] = _state->pos[1];
|
_out->pos_enu[2] = _state->pos[2];
|
_out->heading_deg = _state->heading_deg;
|
_out->pitch_deg = _state->pitch_deg;
|
_out->roll_deg = _state->roll_deg;
|
if (_state->stage == MC_STAGE_FOLLOW_PATH || _state->stage == MC_STAGE_GOTO_START)
|
{
|
_out->target_valid = HIDO_TRUE;
|
_out->target_xy[0] = _state->current_target_xy[0];
|
_out->target_xy[1] = _state->current_target_xy[1];
|
}
|
else
|
{
|
_out->target_valid = HIDO_FALSE;
|
_out->target_xy[0] = 0.0f;
|
_out->target_xy[1] = 0.0f;
|
}
|
}
|
