| | |
|---|
| | | #include "HIDO_ATLite.h" |
|---|
| | | #include "UDPClient.h" |
|---|
| | | #include "bluetooth.h" |
|---|
| | | #include "TCPClient.h" |
|---|
| | | #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; |
|---|
| | | |
|---|
| | | static osSemaphoreId_t g_semaphoreHandle = NULL; |
|---|
| | | extern uint8_t restart_num; |
|---|
| | | void IdleTask(void) |
|---|
| | | { |
|---|
| | |
|---|
| | | { |
|---|
| | | // printf("%s URTRestart",__debug_info__); |
|---|
| | | HAL_NVIC_SystemReset(); |
|---|
| | | // SCB->AIRCR = 0X05FA0000|(unsigned int)0x04; //软复位回到bootloader |
|---|
| | | // SCB->AIRCR = 0X05FA0000|(unsigned int)0x04; //����λ�ص�bootloader |
|---|
| | | } |
|---|
| | | |
|---|
| | | } |
|---|
| | | void app_task(void *pvParameters) |
|---|
| | | { |
|---|
| | | g_com_map[IP_0] =103; |
|---|
| | | g_com_map[IP_1] =155; |
|---|
| | | g_com_map[IP_2] =76; |
|---|
| | | g_com_map[IP_3] =198; |
|---|
| | | g_com_map[UDP_PORT] =6666; |
|---|
| | | Shell_Init(); |
|---|
| | | GPS_Init(); |
|---|
| | | Internet_Init(); |
|---|
| | | UDPClient_Init(); |
|---|
| | | |
|---|
| | | TCPClient_Init(); |
|---|
| | | 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_UINT32 timeout = 100; |
|---|
| | | HIDO_UINT32 timeout = 5; |
|---|
| | | if (xSemaphoreTake(g_semaphoreHandle, timeout / portTICK_PERIOD_MS) == pdTRUE) |
|---|
| | | { |
|---|
| | | |
|---|
| | | } |
|---|
| | | DBG_Poll(); |
|---|
| | | BT_Poll(); |
|---|
| | | SBUS_Poll(); |
|---|
| | | PythonLink_Poll(); |
|---|
| | | 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(); |
|---|
| | | IdleTask(); |
|---|
| | | HIDO_TimerPoll(); |
|---|
| | | } |
|---|
| | |
|---|
| | | { |
|---|
| | | MCUFlash_Init(); |
|---|
| | | parameter_init(); |
|---|
| | | printf("demo run!\r\n"); |
|---|
| | | TaskHandle_t xTaskHandle = NULL; |
|---|
| | | |
|---|
| | | { |
|---|
| | | uint16_t dev_id = g_com_map[DEV_ID]; |
|---|
| | | uint16_t udp_port = g_com_map[UDP_PORT]; |
|---|
| | | uint16_t tcp_port = g_com_map[TCP_PORT]; |
|---|
| | | uint16_t ip0 = g_com_map[IP_0]; |
|---|
| | | uint16_t ip1 = g_com_map[IP_1]; |
|---|
| | | uint16_t ip2 = g_com_map[IP_2]; |
|---|
| | | uint16_t ip3 = g_com_map[IP_3]; |
|---|
| | | uint16_t tip0 = g_com_map[TCP_IP_0]; |
|---|
| | | uint16_t tip1 = g_com_map[TCP_IP_1]; |
|---|
| | | uint16_t tip2 = g_com_map[TCP_IP_2]; |
|---|
| | | uint16_t tip3 = g_com_map[TCP_IP_3]; |
|---|
| | | |
|---|
| | | |
|---|
| | | uint16_t version = g_com_map[VERSION]; |
|---|
| | | |
|---|
| | | HIDO_Debug2("[BOOT] Version: %u\r\n", version); |
|---|
| | | HIDO_Debug2("[BOOT] DeviceID: %u\r\n", dev_id); |
|---|
| | | HIDO_Debug2("[BOOT] UDP Server: %u.%u.%u.%u:%u\r\n", ip0, ip1, ip2, ip3, udp_port); |
|---|
| | | HIDO_Debug2("[BOOT] TCP Server: %u.%u.%u.%u:%u\r\n", tip0, tip1, tip2, tip3, tcp_port); |
|---|
| | | } |
|---|
| | | osSemaphoreAttr_t semaphore_attr = { |
|---|
| | | .name = "MySemaphore", |
|---|
| | | .attr_bits = 0, |
|---|
| | | .cb_mem = NULL, |
|---|
| | | .cb_size = 0}; |
|---|
| | | |
|---|
| | | // 创建二进制信号量（初始计数为0，最大计数为1） |
|---|
| | | // |
|---|
| | | g_semaphoreHandle = osSemaphoreNew(1, 0, &semaphore_attr); |
|---|
| | | |
|---|
| | | xTaskCreate( |
|---|
| | |
|---|
| | | 2048, |
|---|
| | | NULL, |
|---|
| | | tskIDLE_PRIORITY + 1, |
|---|
| | | &xTaskHandle); |
|---|
| | | &g_app_task_handle); |
|---|
| | | } |
|---|
