| | |
|---|
| | | MC_Output output; |
|---|
| | | MC_Compute(&g_motion_state, 1.0f / MC_CFG_CONTROL_HZ, &output); |
|---|
| | | |
|---|
| | | 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; |
|---|
| | | HIDO_UINT16 applied_throttle = MC_CFG_PWM_CENTER_US; |
|---|
