| | |
|---|
| | | 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; |
|---|
| | |
|---|
| | | HIDO_BOOL imu_valid = (GPS_GetGPIMU(&gpimu) == HIDO_OK); |
|---|
| | | HIDO_UINT32 now = HAL_GetTick(); |
|---|
| | | |
|---|
| | | if (gps_valid == HIDO_TRUE) |
|---|
| | | if (gps_valid == HIDO_TRUE && gprmi.m_u32TimeOfWeek != 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_roll_deg = gprmi.m_fRollAngle; |
|---|
| | | g_last_pose_valid = HIDO_TRUE; |
|---|
| | | g_last_gps_ms = now; |
|---|
| | | g_last_gprmi_tow = gprmi.m_u32TimeOfWeek; |
|---|
| | | g_last_sensor_timestamp_ms = gprmi.m_u32TimeOfWeek; |
|---|
| | | } |
|---|
| | | else if ((now - g_last_gps_ms) > 200U) |
|---|
| | | else if (gps_valid == HIDO_FALSE && (now - g_last_gps_ms) > 200U) |
|---|
| | | { |
|---|
| | | g_motion_state.pose_valid = HIDO_FALSE; |
|---|
| | | g_last_pose_valid = HIDO_FALSE; |
|---|
| | |
|---|
| | | 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; |
|---|
