| | |
|---|
| | | 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; |
|---|
| | |
|---|
| | | } |
|---|
| | | |
|---|
| | | 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); |
|---|
| | | 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; |
|---|
