From 3c2021441490ae9b93f7a2ef0f379909b589edd9 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期六, 13 十二月 2025 18:53:16 +0800
Subject: [PATCH] 应该是拍视频的版本，外包在此基础上加了MQTT部分。
---
STM32H743/FML/motion_control.c | 346 ++++++++++++++++++++++++++++++++++++++++++++++++++-------
1 files changed, 303 insertions(+), 43 deletions(-)
diff --git a/STM32H743/FML/motion_control.c b/STM32H743/FML/motion_control.c
index b5ed3ee..3658ba2 100644
--- a/STM32H743/FML/motion_control.c
+++ b/STM32H743/FML/motion_control.c
@@ -20,6 +20,8 @@
#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)
@@ -35,9 +37,44 @@
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;
+
+ }
+ 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 (state->path_xy == HIDO_NULL || state->path_count == 0U)
+ {
+ outPt[0] = 0.0f;
+ outPt[1] = 0.0f;
+ return;
+ }
+
if (idx >= state->path_count)
{
idx = state->path_count - 1U;
@@ -66,48 +103,84 @@
return dist;
}
-/* 鎼滅储褰撳墠浣嶇疆鏈�杩戠殑璺緞鐐� */
-static HIDO_UINT32 mc_find_nearest(const MC_State *state)
+/* 鏁板鍧愭爣锛堜笢涓� 0掳锛岄�嗘椂閽堜负姝ｏ級鈫� 缃楃洏鍧愭爣锛堝寳涓� 0掳锛岄『鏃堕拡涓烘锛� */
+static float mc_math_rad_to_compass_deg(float heading_rad)
{
- 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 compass_deg = 90.0f - (heading_rad * RAD2DEG);
+ while (compass_deg >= 360.0f)
{
- 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;
- }
+ compass_deg -= 360.0f;
}
- return best_idx;
+ 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);
+}
+
+/* 鍒ゆ柇杞﹁締鏄惁宸茬粡"鍒拌揪"鏌愪釜璺緞鐐�
+ * 鏍囧噯锛氳窛绂昏鐐瑰皬浜庨槇鍊硷紙0.5绫筹級鍗充负鍒拌揪 */
+static HIDO_BOOL mc_is_point_reached(const MC_State *state, HIDO_UINT32 point_idx)
+{
+ if (point_idx >= state->path_count)
+ {
+ return HIDO_FALSE;
+ }
+
+ float pos[2] = {state->pos[0], state->pos[1]};
+ float point[2];
+ mc_get_point(state, point_idx, point);
+
+ float dist = mc_distance(pos, point);
+
+ /* 瀹氫箟"鍒拌揪"闃堝�间负0.5绫� */
+ const float REACH_THRESHOLD_M = 0.5f;
+
+ return (dist < REACH_THRESHOLD_M) ? HIDO_TRUE : HIDO_FALSE;
+}
+
+/* 鎼滅储褰撳墠浣嶇疆鏈�杩戠殑璺緞鐐癸紙鍙厑璁稿悜鍓嶆帹杩涳紝蹇呴』鍒拌揪鎵嶆帹杩涳級
+ * 绫讳技鑸偣瀵艰埅锛氬繀椤诲厛鍒拌揪鑸偣0锛屾墠鑳藉鑸埌鑸偣1 */
+static HIDO_UINT32 mc_find_nearest(const MC_State *state)
+{
+ HIDO_UINT32 current_idx = state->nearest_index;
+
+ /* 妫�鏌ユ槸鍚﹀埌杈惧綋鍓嶇偣 */
+ if (mc_is_point_reached(state, current_idx) == HIDO_TRUE)
+ {
+ /* 鍒拌揪浜嗭紝鎺ㄨ繘鍒颁笅涓�涓偣锛堝鏋滀笉鏄渶鍚庝竴涓級 */
+ if (current_idx < state->path_count - 1U)
+ {
+ return current_idx + 1U;
+ }
+ }
+
+ /* 鍚﹀垯淇濇寔褰撳墠绱㈠紩涓嶅彉 */
+ return current_idx;
+}
+
+/* 娌胯矾寰勭疮绉窛绂伙紝鐩村埌杈惧埌鍓嶈鐩爣
+ * 娉ㄦ剰锛歭ookahead鐐瑰缁堣嚦灏戞槸start_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++)
+ /* 濡傛灉宸茬粡鏄渶鍚庝竴涓偣锛岀洿鎺ヨ繑鍥� */
+ if (start_idx >= state->path_count - 1U)
{
- 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;
}
- return state->path_count - 1U;
+
+ /* lookahead鐐硅嚦灏戞槸start_idx鐨勪笅涓�涓偣锛堥『搴忓鑸細蹇呴』鍏堢粡杩囦笅涓�涓偣锛� */
+ HIDO_UINT32 next_idx = start_idx + 1U;
+
+ /* 绠�鍖栭�昏緫锛氱洿鎺ヨ繑鍥炰笅涓�涓偣锛岀‘淇濇寜椤哄簭璁块棶姣忎釜鑸偣 */
+ return next_idx;
}
/* 璁＄畻甯︾鍙锋í鍚戣宸紙宸︽鍙宠礋锛� */
@@ -166,6 +239,26 @@
{
float start_point[2];
mc_get_point(state, 0U, start_point);
+
+ /* 妫�娴嬬洰鏍囩偣鍙樺寲骞惰褰� */
+ static HIDO_UINT32 s_goto_log_idx = 0U;
+ HIDO_BOOL target_changed = (fabsf(state->current_target_xy[0] - start_point[0]) > 0.01f) ||
+ (fabsf(state->current_target_xy[1] - start_point[1]) > 0.01f);
+
+ if (target_changed || (s_goto_log_idx++ % 50U) == 0U)
+ {
+ int tgt_x_int = (int)start_point[0];
+ int tgt_x_frac = (int)(fabsf(start_point[0] - tgt_x_int) * 100);
+ int tgt_y_int = (int)start_point[1];
+ int tgt_y_frac = (int)(fabsf(start_point[1] - tgt_y_int) * 100);
+
+ if (target_changed)
+ {
+ HIDO_Debug2("[MC_TGT]GOTO_START target changed: (%d.%02d,%d.%02d)\r\n",
+ tgt_x_int, tgt_x_frac, tgt_y_int, tgt_y_frac);
+ }
+ }
+
state->current_target_xy[0] = start_point[0];
state->current_target_xy[1] = start_point[1];
out->target_valid = HIDO_TRUE;
@@ -175,11 +268,13 @@
float vec_y = start_point[1] - state->pos[1];
float dist = mc_distance(start_point, state->pos);
- float desired_heading = arm_atan2_f32(vec_y, vec_x);
- float heading_err = mc_wrap_angle(desired_heading - state->heading_rad);
+ float desired_heading;
+ arm_status status = 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 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);
-
+
float forward = 0.0f;
if (dist > state->config.start_pos_tolerance_m)
{
@@ -193,7 +288,9 @@
}
else if (fabsf(heading_err) < state->config.start_heading_tolerance_rad)
{
+ /* 鍒拌揪璧风偣涓旇埅鍚戝鍑嗭紝鍒囨崲鍒拌矾寰勮窡韪ā寮� */
state->stage = MC_STAGE_FOLLOW_PATH;
+ state->nearest_index = 0U; /* 閲嶇疆涓鸿矾寰勭涓�涓偣 */
}
out->forward_mps = forward;
@@ -207,17 +304,107 @@
/* 闃舵锛氱函杩借釜 + 鑸悜/妯悜璇樊琛ュ伩 */
static void mc_compute_follow_path(MC_State *state, float dt_s, MC_Output *out)
{
+ /* 瀹夊叏妫�鏌ワ細璺緞蹇呴』鑷冲皯鏈�1涓偣 */
+ if (state->path_count == 0U)
+ {
+ out->forward_mps = 0.0f;
+ out->turn_rate = 0.0f;
+ out->active = HIDO_FALSE;
+ return;
+ }
+
HIDO_UINT32 nearest_idx = mc_find_nearest(state);
+
+ /* 瀹夊叏妫�鏌ワ細nearest_idx 蹇呴』鍦ㄦ湁鏁堣寖鍥村唴 */
+ if (nearest_idx >= state->path_count)
+ {
+ nearest_idx = state->path_count - 1U;
+ }
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);
+
+ /* 瀹夊叏妫�鏌ワ細lookahead_idx 蹇呴』鍦ㄦ湁鏁堣寖鍥村唴 */
+ if (lookahead_idx >= state->path_count)
+ {
+ lookahead_idx = state->path_count - 1U;
+ }
state->lookahead_index = lookahead_idx;
+
+ /* 璋冭瘯锛氭瘡20甯ц緭鍑轰竴娆¤矾寰勮窡韪姸鎬� */
+ static HIDO_UINT32 s_path_log_idx = 0U;
+ static HIDO_UINT32 s_last_nearest = 0U;
+ static HIDO_UINT32 s_last_lookahead = 0U;
+
+ /* 濡傛灉绱㈠紩鍙戠敓鍙樺寲锛岀珛鍗宠緭鍑篖OG */
+ HIDO_BOOL index_changed = (nearest_idx != s_last_nearest) || (lookahead_idx != s_last_lookahead);
+
+ if ((s_path_log_idx++ % 20U) == 0U || index_changed == HIDO_TRUE)
+ {
+ float nearest_pt[2];
+ mc_get_point(state, nearest_idx, nearest_pt);
+ float dist_to_nearest = mc_distance(nearest_pt, state->pos);
+ HIDO_BOOL reached = mc_is_point_reached(state, nearest_idx);
+
+ /* 浣跨敤鏁存暟琛ㄧず娉曟墦鍗版诞鐐规暟 */
+ int pos_x_int = (int)state->pos[0];
+ int pos_x_frac = (int)(fabsf(state->pos[0] - pos_x_int) * 100);
+ int pos_y_int = (int)state->pos[1];
+ int pos_y_frac = (int)(fabsf(state->pos[1] - pos_y_int) * 100);
+ int near_x_int = (int)nearest_pt[0];
+ int near_x_frac = (int)(fabsf(nearest_pt[0] - near_x_int) * 100);
+ int near_y_int = (int)nearest_pt[1];
+ int near_y_frac = (int)(fabsf(nearest_pt[1] - near_y_int) * 100);
+ int dn_int = (int)dist_to_nearest;
+ int dn_frac = (int)((dist_to_nearest - dn_int) * 100);
+
+ if (index_changed == HIDO_TRUE)
+ {
+ HIDO_Debug2("[MC_PATH]CHG pos=(%d.%02d,%d.%02d) near=%u->%u look=%u->%u dn=%d.%02d reach=%d\r\n",
+ pos_x_int, pos_x_frac, pos_y_int, pos_y_frac,
+ s_last_nearest, nearest_idx,
+ s_last_lookahead, lookahead_idx,
+ dn_int, dn_frac, reached);
+ }
+ else
+ {
+ HIDO_Debug2("[MC_PATH] near=%u(%d.%02d,%d.%02d) look=%u dn=%d.%02d reach=%d\r\n",
+ nearest_idx, near_x_int, near_x_frac, near_y_int, near_y_frac,
+ lookahead_idx,
+ dn_int, dn_frac, reached);
+ }
+
+ s_last_nearest = nearest_idx;
+ s_last_lookahead = lookahead_idx;
+ }
float target[2];
mc_get_point(state, lookahead_idx, target);
+
+ /* 妫�娴嬬洰鏍囩偣鍙樺寲骞惰褰� */
+ HIDO_BOOL target_changed = (fabsf(state->current_target_xy[0] - target[0]) > 0.01f) ||
+ (fabsf(state->current_target_xy[1] - target[1]) > 0.01f);
+
+ if (target_changed)
+ {
+ int old_x_int = (int)state->current_target_xy[0];
+ int old_x_frac = (int)(fabsf(state->current_target_xy[0] - old_x_int) * 100);
+ int old_y_int = (int)state->current_target_xy[1];
+ int old_y_frac = (int)(fabsf(state->current_target_xy[1] - old_y_int) * 100);
+ int new_x_int = (int)target[0];
+ int new_x_frac = (int)(fabsf(target[0] - new_x_int) * 100);
+ int new_y_int = (int)target[1];
+ int new_y_frac = (int)(fabsf(target[1] - new_y_int) * 100);
+
+ HIDO_Debug2("[MC_TGT]FOLLOW target changed: (%d.%02d,%d.%02d)->(%d.%02d,%d.%02d) near=%u look=%u\r\n",
+ old_x_int, old_x_frac, old_y_int, old_y_frac,
+ new_x_int, new_x_frac, new_y_int, new_y_frac,
+ nearest_idx, lookahead_idx);
+ }
+
state->current_target_xy[0] = target[0];
state->current_target_xy[1] = target[1];
out->target_valid = HIDO_TRUE;
@@ -226,8 +413,10 @@
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);
- float heading_err = mc_wrap_angle(desired_heading - state->heading_rad);
+ 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;
@@ -236,7 +425,7 @@
+ 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);
-
+
float forward = state->config.base_speed_mps
- state->config.heading_speed_scale * fabsf(heading_err)
- state->config.xtrack_speed_scale * fabsf(cross_track);
@@ -290,6 +479,8 @@
{
if (_state == HIDO_NULL || _cfg == HIDO_NULL || _path_xy == HIDO_NULL || _point_count < 2U)
{
+ DBG_Printf("[MC_Init] ERROR: Invalid parameters (state=%p, cfg=%p, path=%p, count=%u)\r\n",
+ (void*)_state, (void*)_cfg, (void*)_path_xy, _point_count);
return;
}
@@ -300,6 +491,9 @@
_state->stage = MC_STAGE_GOTO_START;
_state->nearest_index = 0U;
_state->lookahead_index = 0U;
+
+ DBG_Printf("[MC_Init] OK: path_count=%u, first_point=(%.2f,%.2f)\r\n",
+ _point_count, _path_xy[0], _path_xy[1]);
}
/* 娉ㄥ叆鏈�鏂� ENU 浣嶅Э鍙婅埅鍚�/閫熷害 */
@@ -313,8 +507,12 @@
_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 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);
+
+ float old_heading_rad = mc_compass_deg_to_math_rad(old_heading_deg);
_state->pitch_deg = _gprmi->m_fPitchAngle;
_state->roll_deg = _gprmi->m_fRollAngle;
_state->vel[0] = _gprmi->m_fEastVelocity;
@@ -337,11 +535,16 @@
return;
}
- _state->yaw_rate_rad = _gpimu->m_fGyroZ * DEG2RAD;
+ /* 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;
+
}
-/* 鍏ュ彛锛氭牴鎹樁娈佃绠楁帶鍒堕噺 */
+/* 鍏ュ彛锛氭牴鎹樁娈佃绠楁帶鍒堕噺 */
HIDO_VOID MC_Compute(MC_State *_state, float _dt_s, MC_Output *_out)
{
if (_state == HIDO_NULL || _out == HIDO_NULL)
@@ -354,21 +557,52 @@
_out->target_xy[0] = 0.0f;
_out->target_xy[1] = 0.0f;
+ /* 璁板綍target_valid鍙樹负FALSE鐨勫師鍥� */
+ static HIDO_BOOL s_last_path_valid = HIDO_TRUE;
+ static HIDO_BOOL s_last_pose_valid = HIDO_TRUE;
+
if (_state->path_xy == HIDO_NULL || _state->path_count < 2U)
{
+ if (s_last_path_valid)
+ {
+ HIDO_Debug2("[MC_TGT]WARNING: target_valid=FALSE, path invalid (path=%p, count=%u)\r\n",
+ (void*)_state->path_xy, _state->path_count);
+ s_last_path_valid = HIDO_FALSE;
+ }
return;
}
+ s_last_path_valid = HIDO_TRUE;
if (_state->pose_valid == HIDO_FALSE)
{
+ if (s_last_pose_valid)
+ {
+ HIDO_Debug2("[MC_TGT]WARNING: target_valid=FALSE, pose_valid=FALSE\r\n");
+ s_last_pose_valid = HIDO_FALSE;
+ }
return;
}
+ s_last_pose_valid = HIDO_TRUE;
+
+ 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);
+
+ (void)new_heading_rad;
+ }
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,11 +613,15 @@
break;
case MC_STAGE_FINISHED:
_out->active = HIDO_FALSE;
+ _out->target_heading_deg = _state->heading_deg;
break;
default:
break;
}
+ /* 涓嶅啀鍙栧弽锛歽aw_rate_cmd锛堟暟瀛﹀潗鏍囩郴锛屾=CCW锛夌洿鎺ヤ綔涓簍urn_rate杈撳嚭
+ * motion_control_task.c涓紝姝ｅ��=宸﹁浆(閫嗘椂閽�)锛岃礋鍊�=鍙宠浆(椤烘椂閽�)锛屼笌鏁板鍧愭爣绯讳竴鑷� */
+ // _out->turn_rate = -_out->turn_rate; // 宸叉敞閲婏細姝ゅ彇鍙嶅鑷存柟鍚戦敊璇�
_out->stage = _state->stage;
_out->pos_enu[0] = _state->pos[0];
_out->pos_enu[1] = _state->pos[1];
@@ -391,17 +629,39 @@
_out->heading_deg = _state->heading_deg;
_out->pitch_deg = _state->pitch_deg;
_out->roll_deg = _state->roll_deg;
+ static HIDO_BOOL s_last_target_valid = HIDO_FALSE;
+ static E_MCStage s_last_stage_for_target = MC_STAGE_IDLE;
+
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];
+
+ if (!s_last_target_valid)
+ {
+ int x_int = (int)_out->target_xy[0];
+ int x_frac = (int)(fabsf(_out->target_xy[0] - x_int) * 100);
+ int y_int = (int)_out->target_xy[1];
+ int y_frac = (int)(fabsf(_out->target_xy[1] - y_int) * 100);
+ HIDO_Debug2("[MC_TGT]target_valid: FALSE->TRUE, stage=%d, target=(%d.%02d,%d.%02d)\r\n",
+ _state->stage, x_int, x_frac, y_int, y_frac);
+ }
}
else
{
_out->target_valid = HIDO_FALSE;
_out->target_xy[0] = 0.0f;
_out->target_xy[1] = 0.0f;
+
+ if (s_last_target_valid || _state->stage != s_last_stage_for_target)
+ {
+ HIDO_Debug2("[MC_TGT]target_valid: TRUE->FALSE, stage=%d->%d (will output 0,0)\r\n",
+ s_last_stage_for_target, _state->stage);
+ }
}
+
+ s_last_target_valid = _out->target_valid;
+ s_last_stage_for_target = _state->stage;
}
--
Gitblit v1.10.0