| | |
|---|
| | | #include "SBUS.h" |
|---|
| | | #include "pwm_ctrol.h" |
|---|
| | | #include "PythonLink.h" |
|---|
| | | #include "motion_mode.h" |
|---|
| | | #include "motion_control_task.h" |
|---|
| | | #include "motion_calibration_task.h" |
|---|
| | | osSemaphoreId_t g_semaphoreHandle = NULL; |
|---|
| | | TaskHandle_t g_app_task_handle = NULL; |
|---|
| | | |
|---|
| | |
|---|
| | | BT_Init(); |
|---|
| | | SBUS_Init(); |
|---|
| | | PythonLink_Init(); |
|---|
| | | #if (MOTION_ACTIVE_MODE == MOTION_MODE_CONTROL) |
|---|
| | | MotionControl_TaskInit(); |
|---|
| | | #elif (MOTION_ACTIVE_MODE == MOTION_MODE_CALIBRATION) |
|---|
| | | MotionCalibration_TaskInit(); |
|---|
| | | #else |
|---|
| | | #error "Unsupported MOTION_ACTIVE_MODE" |
|---|
| | | #endif |
|---|
| | | /* ENU调试打印（非校准模式） */ |
|---|
| | | static HIDO_UINT32 debug_print_counter = 0; |
|---|
| | | const HIDO_UINT32 DEBUG_PRINT_INTERVAL = 100; /* 每100次循环打印一次 (约500ms) */ |
|---|
| | | |
|---|
| | | /* GPS GGA上传定时器 */ |
|---|
| | | static HIDO_UINT32 gps_upload_counter = 0; |
|---|
| | | const HIDO_UINT32 GPS_UPLOAD_INTERVAL = 200; /* 每200次循环上传一次 (约1000ms) */ |
|---|
| | | |
|---|
| | | while (1) |
|---|
| | | { |
|---|
| | | //HIDO_UINT32 timeout = HIDO_TimerGetNearestTimeout(1000); |
|---|
| | |
|---|
| | | HIDO_ATLitePoll(); |
|---|
| | | Internet_Poll(); |
|---|
| | | GPS_Poll(); |
|---|
| | | |
|---|
| | | /* 定时上传GGA报文到4G（每秒一次） */ |
|---|
| | | if (++gps_upload_counter >= GPS_UPLOAD_INTERVAL) |
|---|
| | | { |
|---|
| | | gps_upload_counter = 0; |
|---|
| | | GPS_UploadGGA(); |
|---|
| | | } |
|---|
| | | |
|---|
| | | /* 周期性打印ENU坐标和IMU数据（用于调试GPS解析） */ |
|---|
| | | #if (MOTION_ACTIVE_MODE != MOTION_MODE_CALIBRATION) |
|---|
| | | if (++debug_print_counter >= DEBUG_PRINT_INTERVAL) |
|---|
| | | { |
|---|
| | | debug_print_counter = 0; |
|---|
| | | |
|---|
| | | ST_GPRMI gprmi; |
|---|
| | | ST_GPIMU gpimu; |
|---|
| | | float enu[3]; |
|---|
| | | |
|---|
| | | if (GPS_GetGPRMI(&gprmi) == HIDO_OK && |
|---|
| | | GPS_GetGPIMU(&gpimu) == HIDO_OK && |
|---|
| | | GPS_GetCurrentENU(enu) == HIDO_OK) |
|---|
| | | { |
|---|
| | | HIDO_Debug2("$DEBUG,ENU,%.2f,%.2f,%.2f,HDG,%.2f,ACC,%.3f,%.3f,%.3f,GYRO,%.3f,%.3f,%.3f\r\n", |
|---|
| | | enu[0], enu[1], enu[2], /* ENU坐标 (m) */ |
|---|
| | | gprmi.m_fHeadingAngle, /* 航向角 (度) */ |
|---|
| | | gpimu.m_fAccelX, gpimu.m_fAccelY, gpimu.m_fAccelZ, /* 加速度 (m/s²) */ |
|---|
| | | gpimu.m_fGyroX, gpimu.m_fGyroY, gpimu.m_fGyroZ); /* 角速度 (rad/s) */ |
|---|
| | | } |
|---|
| | | } |
|---|
| | | #endif |
|---|
| | | |
|---|
| | | UDPClient_Poll(); |
|---|
| | | TCPClient_Poll(); |
|---|
| | | // TCPClient_Poll(); |
|---|
| | | IdleTask(); |
|---|
| | | HIDO_TimerPoll(); |
|---|
| | | } |
|---|
