#include "cmsis_os2.h"
|
#include "FreeRTOS.h"
|
#include "task.h"
|
#include "semphr.h"
|
#include "HIDO_Input.h"
|
#include "HIDO_Timer.h"
|
#include "GPS.h"
|
#include "DBG.h"
|
#include "Internet.h"
|
#include "global_param.h"
|
#include "MCUFlash.h"
|
#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;
|
|
extern uint8_t restart_num;
|
void IdleTask(void)
|
{
|
if(g_com_map[CNT_RESTART]==1)
|
{
|
g_com_map[CNT_RESTART]=0;
|
// printf("%s URTRestart",__debug_info__);
|
HAL_NVIC_SystemReset();
|
}
|
if(g_com_map[MAP_SIGN_INDEX]!=0x55AA)
|
{
|
// printf("%s URTRestart",__debug_info__);
|
HAL_NVIC_SystemReset();
|
// SCB->AIRCR = 0X05FA0000|(unsigned int)0x04; //����λ�ص�bootloader
|
}
|
|
}
|
void app_task(void *pvParameters)
|
{
|
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 = 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();
|
}
|
}
|
|
void app_trigger_from_isr(void)
|
{
|
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
|
if (g_semaphoreHandle != NULL)
|
{
|
BaseType_t result = xSemaphoreGiveFromISR(g_semaphoreHandle, &xHigherPriorityTaskWoken);
|
|
if (xHigherPriorityTaskWoken == pdTRUE)
|
{
|
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
|
}
|
}
|
}
|
|
void app_trigger(void)
|
{
|
if (g_semaphoreHandle != NULL)
|
{
|
xSemaphoreGive(g_semaphoreHandle);
|
}
|
}
|
|
void app_main(void)
|
{
|
MCUFlash_Init();
|
parameter_init();
|
|
{
|
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};
|
|
//
|
g_semaphoreHandle = osSemaphoreNew(1, 0, &semaphore_attr);
|
|
xTaskCreate(
|
app_task,
|
"AppTask",
|
2048,
|
NULL,
|
tskIDLE_PRIORITY + 1,
|
&g_app_task_handle);
|
}
|
