/******************************************************************************* * File Name : MQTTApp.c * Description : MQTT应用层 - 负责数据填充、JSON序列化/反序列化 *******************************************************************************/ /******************************************************************************* * Include Files * *******************************************************************************/ #include "MQTTApp.h" #include "MQTTClient.h" #include "global_param.h" #include "HIDO_Util.h" #include "HIDO_Debug.h" #include "HIDO_Timer.h" #include "HIDO_Json.h" #include "GPS.h" #include "string.h" #include "stdlib.h" /******************************************************************************* * Macro * *******************************************************************************/ #define JSON_PACK(_buf, _len, _size, ...) \ (_len) += HIDO_UtilSnprintf((HIDO_CHAR *)(_buf) + (_len), (_size) - (_len), __VA_ARGS__) /******************************************************************************* * Type Definition * *******************************************************************************/ /******************************************************************************* * Local Variable * *******************************************************************************/ static ST_MQTTAppConfig l_stAppConfig; static HIDO_UINT8 l_au8JsonBuffer[2048]; // JSON缓冲区 /******************************************************************************* * Local Function Declaration * *******************************************************************************/ /******************************************************************************* * Local Function * *******************************************************************************/ /******************************************************************************* * Function Name : MQTTApp_GenerateMsgID * Description : 生成消息ID * Input : _pcBuffer 缓冲区 * _u32Size 缓冲区大小 * _pcPrefix 前缀 * Output : None * Return : HIDO_OK/HIDO_ERR *******************************************************************************/ static HIDO_INT32 MQTTApp_GenerateMsgID(HIDO_CHAR *_pcBuffer, HIDO_UINT32 _u32Size, const HIDO_CHAR *_pcPrefix) { HIDO_UINT32 u32Timestamp = HIDO_TimerGetTick(); return HIDO_UtilSnprintf(_pcBuffer, _u32Size, "%s_%u", _pcPrefix, u32Timestamp); } /******************************************************************************* * Function Name : MQTTApp_PackGpsJson * Description : 封装GPS数据为JSON * Input : _pstGpsData GPS数据 * Output : _pu8JsonBuf JSON缓冲区 * _pu32JsonLen JSON长度 * Return : HIDO_OK/HIDO_ERR *******************************************************************************/ static HIDO_INT32 MQTTApp_PackGpsJson(ST_MQTTAppGpsData *_pstGpsData, HIDO_UINT8 *_pu8JsonBuf, HIDO_UINT32 *_pu32JsonLen) { HIDO_UINT32 u32Len = 0; HIDO_CHAR acMsgID[64]; if (HIDO_NULL == _pstGpsData || HIDO_NULL == _pu8JsonBuf || HIDO_NULL == _pu32JsonLen) { return HIDO_ERR; } // 生成消息ID MQTTApp_GenerateMsgID(acMsgID, sizeof(acMsgID), "hxzkgps"); // 构建JSON JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), "{"); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), "\"msg_id\":\"%s\"", acMsgID); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), ",\"timestamp\":%llu", _pstGpsData->timestamp); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), ",\"device_id\":\"%u\"", g_com_map[DEV_ID]); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), ",\"data_type\":\"gps\""); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), ",\"gps_raw\":\"%s\"", _pstGpsData->gps_raw); // IMU数据 JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), ",\"imu_data\":{"); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), "\"roll\":%.2f", _pstGpsData->imu_data.roll); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), ",\"pitch\":%.2f", _pstGpsData->imu_data.pitch); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), ",\"yaw\":%.2f", _pstGpsData->imu_data.yaw); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), "}"); // 状态数据 JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), ",\"status\":{"); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), "\"battery_level\":%u", _pstGpsData->status.battery_level); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), ",\"battery_voltage\":%.2f", _pstGpsData->status.battery_voltage); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), ",\"operation_mode\":\"%s\"", _pstGpsData->status.operation_mode); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), ",\"motor_status\":\"%s\"", _pstGpsData->status.motor_status); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), ",\"blade_status\":\"%s\"", _pstGpsData->status.blade_status); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), ",\"blade_height\":%u", _pstGpsData->status.blade_height); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), ",\"self_check_status\":%u", _pstGpsData->status.self_check_status); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), ",\"error_code\":%u", _pstGpsData->status.error_code); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), ",\"error_message\":\"%s\"", _pstGpsData->status.error_message); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), "}"); JSON_PACK(_pu8JsonBuf, u32Len, sizeof(l_au8JsonBuffer), "}"); *_pu32JsonLen = u32Len; return HIDO_OK; } /******************************************************************************* * Function Name : MQTTApp_GetJsonStringValue * Description : 从JSON中提取字符串值（简单实现，带边界检查） * Input : _pcJson JSON字符串 * _u32JsonLen JSON字符串长度 * _pcKey 键名 * Output : _pcValue 值缓冲区 * _u32ValueSize 值缓冲区大小 * Return : HIDO_OK/HIDO_ERR *******************************************************************************/ static HIDO_INT32 MQTTApp_GetJsonStringValue(HIDO_CHAR *_pcJson, HIDO_UINT32 _u32JsonLen, const HIDO_CHAR *_pcKey, HIDO_CHAR *_pcValue, HIDO_UINT32 _u32ValueSize) { HIDO_CHAR *pcKeyPos = HIDO_NULL; HIDO_CHAR *pcValueStart = HIDO_NULL; HIDO_CHAR *pcValueEnd = HIDO_NULL; HIDO_CHAR *pcJsonEnd = HIDO_NULL; HIDO_CHAR acSearchKey[64]; HIDO_UINT32 u32Len = 0; if (HIDO_NULL == _pcJson || HIDO_NULL == _pcKey || HIDO_NULL == _pcValue || _u32JsonLen == 0) { return HIDO_ERR; } // 计算JSON字符串的结束位置 pcJsonEnd = _pcJson + _u32JsonLen; // 构造搜索字符串 "key":" HIDO_UtilSnprintf(acSearchKey, sizeof(acSearchKey), "\"%s\":", _pcKey); // 查找键 pcKeyPos = strstr(_pcJson, acSearchKey); if (HIDO_NULL == pcKeyPos || pcKeyPos >= pcJsonEnd) { return HIDO_ERR; } // 跳过键和冒号，查找值的起始引号（限制搜索范围） pcValueStart = pcKeyPos + strlen(acSearchKey); if (pcValueStart >= pcJsonEnd) { return HIDO_ERR; } // 在剩余范围内查找起始引号 while (pcValueStart < pcJsonEnd && *pcValueStart != '\"') { pcValueStart++; } if (pcValueStart >= pcJsonEnd || *pcValueStart != '\"') { return HIDO_ERR; } pcValueStart++; // 跳过起始引号 // 查找值的结束引号（限制搜索范围） pcValueEnd = pcValueStart; while (pcValueEnd < pcJsonEnd && *pcValueEnd != '\"') { pcValueEnd++; } if (pcValueEnd >= pcJsonEnd || *pcValueEnd != '\"') { return HIDO_ERR; } // 计算值长度 u32Len = pcValueEnd - pcValueStart; if (u32Len >= _u32ValueSize) { u32Len = _u32ValueSize - 1; } // 复制值 memcpy(_pcValue, pcValueStart, u32Len); _pcValue[u32Len] = '\0'; return HIDO_OK; } /******************************************************************************* * Function Name : MQTTApp_ParseControlJson * Description : 解析控制指令JSON * Input : _pu8JsonBuf JSON缓冲区 * _u32JsonLen JSON长度 * Output : _pstCtrlData 控制数据 * Return : HIDO_OK/HIDO_ERR *******************************************************************************/ static HIDO_INT32 MQTTApp_ParseControlJson(HIDO_UINT8 *_pu8JsonBuf, HIDO_UINT32 _u32JsonLen, ST_MQTTAppControlData *_pstCtrlData) { if (HIDO_NULL == _pu8JsonBuf || HIDO_NULL == _pstCtrlData) { return HIDO_ERR; } memset(_pstCtrlData, 0, sizeof(ST_MQTTAppControlData)); // 解析 command MQTTApp_GetJsonStringValue((HIDO_CHAR *)_pu8JsonBuf, _u32JsonLen, "command", _pstCtrlData->command, sizeof(_pstCtrlData->command)); // 解析 request_id MQTTApp_GetJsonStringValue((HIDO_CHAR *)_pu8JsonBuf, _u32JsonLen, "request_id", _pstCtrlData->request_id, sizeof(_pstCtrlData->request_id)); // 解析 parameters (根据具体协议扩展) // TODO: 根据实际需求解析参数 return HIDO_OK; } /******************************************************************************* * Function Name : MQTTApp_ParsePathJson * Description : 解析路径数据JSON（使用HIDO_Json库，安全无越界风险） * Input : _pu8JsonBuf JSON缓冲区 * _u32JsonLen JSON长度 * Output : _pstPathData 路径数据 * Return : HIDO_OK/HIDO_ERR *******************************************************************************/ static HIDO_INT32 MQTTApp_ParsePathJson(HIDO_UINT8 *_pu8JsonBuf, HIDO_UINT32 _u32JsonLen, ST_MQTTAppPathData *_pstPathData) { HIDO_JsonNodeStruct *pstJsonRoot = HIDO_NULL; HIDO_JsonNodeStruct *pstBasestationNode = HIDO_NULL; HIDO_JsonNodeStruct *pstPathDataNode = HIDO_NULL; HIDO_JsonArraryNodeStruct *pstArrayNode = HIDO_NULL; HIDO_CHAR *pcValue = HIDO_NULL; HIDO_INT32 i32Value = 0; HIDO_UINT32 i = 0; if (HIDO_NULL == _pu8JsonBuf || HIDO_NULL == _pstPathData) { HIDO_DebugEx("[MQTTApp] ERROR: Invalid parameters\r\n"); return HIDO_ERR; } memset(_pstPathData, 0, sizeof(ST_MQTTAppPathData)); // 解析JSON pstJsonRoot = HIDO_JsonJsonParse((HIDO_CHAR *)_pu8JsonBuf); if (pstJsonRoot == HIDO_NULL) { HIDO_DebugEx("[MQTTApp] ERROR: JSON parse failed\r\n"); return HIDO_ERR; } // 解析顶层字段 if (HIDO_JsonGetString(pstJsonRoot, "msg_id", &pcValue) == HIDO_OK) { HIDO_UtilSnprintf(_pstPathData->msg_id, sizeof(_pstPathData->msg_id), "%s", pcValue); } if (HIDO_JsonGetString(pstJsonRoot, "timestamp", &pcValue) == HIDO_OK) { _pstPathData->timestamp = (HIDO_UINT64)atoll(pcValue); } if (HIDO_JsonGetString(pstJsonRoot, "user_id", &pcValue) == HIDO_OK) { HIDO_UtilSnprintf(_pstPathData->user_id, sizeof(_pstPathData->user_id), "%s", pcValue); } if (HIDO_JsonGetString(pstJsonRoot, "device_id", &pcValue) == HIDO_OK) { HIDO_UtilSnprintf(_pstPathData->device_id, sizeof(_pstPathData->device_id), "%s", pcValue); } if (HIDO_JsonGetString(pstJsonRoot, "command", &pcValue) == HIDO_OK) { HIDO_UtilSnprintf(_pstPathData->command, sizeof(_pstPathData->command), "%s", pcValue); } // 解析basestation_data对象 pstBasestationNode = HIDO_JsonGetValue(pstJsonRoot, "basestation_data"); if (pstBasestationNode != HIDO_NULL && pstBasestationNode->m_eType == HIDO_JSON_TYPE_JSON) { HIDO_JsonNodeStruct *pstBasestationJsonNode = HIDO_JsonJsonParse(pstBasestationNode->m_stValue.m_pcPtr); if (pstBasestationJsonNode != HIDO_NULL) { if (HIDO_JsonGetString(pstBasestationJsonNode, "latitude_dm", &pcValue) == HIDO_OK) { HIDO_UtilSnprintf(_pstPathData->basestation.latitude_dm, sizeof(_pstPathData->basestation.latitude_dm), "%s", pcValue); } if (HIDO_JsonGetString(pstBasestationJsonNode, "latitude_hemisphere", &pcValue) == HIDO_OK) { HIDO_UtilSnprintf(_pstPathData->basestation.latitude_hemisphere, sizeof(_pstPathData->basestation.latitude_hemisphere), "%s", pcValue); } if (HIDO_JsonGetString(pstBasestationJsonNode, "longitude_dm", &pcValue) == HIDO_OK) { HIDO_UtilSnprintf(_pstPathData->basestation.longitude_dm, sizeof(_pstPathData->basestation.longitude_dm), "%s", pcValue); } if (HIDO_JsonGetString(pstBasestationJsonNode, "longitude_hemisphere", &pcValue) == HIDO_OK) { HIDO_UtilSnprintf(_pstPathData->basestation.longitude_hemisphere, sizeof(_pstPathData->basestation.longitude_hemisphere), "%s", pcValue); } if (HIDO_JsonGetString(pstBasestationJsonNode, "altitude", &pcValue) == HIDO_OK) { _pstPathData->basestation.altitude = (HIDO_FLOAT)atof(pcValue); } HIDO_JsonNodeFree(pstBasestationJsonNode); } } // 解析path_data对象 pstPathDataNode = HIDO_JsonGetValue(pstJsonRoot, "path_data"); if (pstPathDataNode != HIDO_NULL && pstPathDataNode->m_eType == HIDO_JSON_TYPE_JSON) { HIDO_JsonNodeStruct *pstPathJsonNode = HIDO_JsonJsonParse(pstPathDataNode->m_stValue.m_pcPtr); if (pstPathJsonNode != HIDO_NULL) { // 解析path_data中的字段 if (HIDO_JsonGetString(pstPathJsonNode, "path_id", &pcValue) == HIDO_OK) { HIDO_UtilSnprintf(_pstPathData->path_id, sizeof(_pstPathData->path_id), "%s", pcValue); } if (HIDO_JsonGetString(pstPathJsonNode, "coordinate_system", &pcValue) == HIDO_OK) { HIDO_UtilSnprintf(_pstPathData->coordinate_system, sizeof(_pstPathData->coordinate_system), "%s", pcValue); } if (HIDO_JsonGetString(pstPathJsonNode, "mowing_pattern", &pcValue) == HIDO_OK) { HIDO_UtilSnprintf(_pstPathData->mowing_pattern, sizeof(_pstPathData->mowing_pattern), "%s", pcValue); } if (HIDO_JsonGetInteger(pstPathJsonNode, "boundary_point_count", &i32Value) == HIDO_OK) { _pstPathData->boundary_point_count = (HIDO_UINT32)i32Value; if (_pstPathData->boundary_point_count > MQTT_APP_MAX_PATH_POINTS1) { HIDO_DebugEx("[MQTTApp] WARNING: boundary_point_count=%u exceeds max=%u\r\n", _pstPathData->boundary_point_count, MQTT_APP_MAX_PATH_POINTS1); _pstPathData->boundary_point_count = MQTT_APP_MAX_PATH_POINTS1; } } if (HIDO_JsonGetInteger(pstPathJsonNode, "navigation_point_count", &i32Value) == HIDO_OK) { _pstPathData->navigation_point_count = (HIDO_UINT32)i32Value; if (_pstPathData->navigation_point_count > MQTT_APP_MAX_PATH_POINTS2) { HIDO_DebugEx("[MQTTApp] WARNING: navigation_point_count=%u exceeds max=%u\r\n", _pstPathData->navigation_point_count, MQTT_APP_MAX_PATH_POINTS2); _pstPathData->navigation_point_count = MQTT_APP_MAX_PATH_POINTS2; } } // 解析boundary_points数组 if (HIDO_JsonGetJsonArrary(pstPathJsonNode, "boundary_points", &pstArrayNode) == HIDO_OK) { HIDO_JsonArraryNodeStruct *pstCurrent = pstArrayNode; i = 0; while (pstCurrent != HIDO_NULL && i < MQTT_APP_MAX_PATH_POINTS1) { if (pstCurrent->m_pstJson != HIDO_NULL) { HIDO_FLOAT fX = 0.0f, fY = 0.0f; if (HIDO_JsonGetFloat(pstCurrent->m_pstJson, "x", &fX) == HIDO_OK && HIDO_JsonGetFloat(pstCurrent->m_pstJson, "y", &fY) == HIDO_OK) { _pstPathData->boundary_points[i].x = fX; _pstPathData->boundary_points[i].y = fY; i++; } } pstCurrent = pstCurrent->m_pstNext; } // 如果JSON数组中的实际点数与boundary_point_count不一致，以实际解析到的为准 if (i != _pstPathData->boundary_point_count) { HIDO_DebugEx("[MQTTApp] WARNING: boundary_point_count mismatch: declared=%u, actual=%u\r\n", _pstPathData->boundary_point_count, i); _pstPathData->boundary_point_count = i; } HIDO_JsonArrayNodeFree(pstArrayNode); } // 解析navigation_points数组 if (HIDO_JsonGetJsonArrary(pstPathJsonNode, "navigation_points", &pstArrayNode) == HIDO_OK) { HIDO_JsonArraryNodeStruct *pstCurrent = pstArrayNode; i = 0; while (pstCurrent != HIDO_NULL && i < MQTT_APP_MAX_PATH_POINTS2) { if (pstCurrent->m_pstJson != HIDO_NULL) { HIDO_FLOAT fX = 0.0f, fY = 0.0f; if (HIDO_JsonGetFloat(pstCurrent->m_pstJson, "x", &fX) == HIDO_OK && HIDO_JsonGetFloat(pstCurrent->m_pstJson, "y", &fY) == HIDO_OK) { _pstPathData->navigation_points[i].x = fX; _pstPathData->navigation_points[i].y = fY; i++; } } pstCurrent = pstCurrent->m_pstNext; } // 如果JSON数组中的实际点数与navigation_point_count不一致，以实际解析到的为准 if (i != _pstPathData->navigation_point_count) { HIDO_DebugEx("[MQTTApp] WARNING: navigation_point_count mismatch: declared=%u, actual=%u\r\n", _pstPathData->navigation_point_count, i); _pstPathData->navigation_point_count = i; } HIDO_JsonArrayNodeFree(pstArrayNode); } HIDO_JsonNodeFree(pstPathJsonNode); } } // 释放根节点 HIDO_JsonNodeFree(pstJsonRoot); HIDO_DebugEx("[MQTTApp] Path parsed successfully: path_id=%s, boundary=%u, navigation=%u\r\n", _pstPathData->path_id, _pstPathData->boundary_point_count, _pstPathData->navigation_point_count); return HIDO_OK; } /******************************************************************************* * Global Function * *******************************************************************************/ /******************************************************************************* * Function Name : MQTTApp_Init * Description : MQTT应用层初始化 * Input : _pstConfig 配置参数 * Output : None * Return : HIDO_OK/HIDO_ERR *******************************************************************************/ HIDO_INT32 MQTTApp_Init(ST_MQTTAppConfig *_pstConfig) { if (HIDO_NULL == _pstConfig) { return HIDO_ERR; } memcpy(&l_stAppConfig, _pstConfig, sizeof(ST_MQTTAppConfig)); HIDO_DebugEx("[MQTTApp] Init OK\r\n"); return HIDO_OK; } /******************************************************************************* * Function Name : MQTTApp_ReportGps * Description : 上报GPS数据 * Input : _pstGpsData GPS数据 * Output : None * Return : HIDO_OK/HIDO_ERR *******************************************************************************/ HIDO_INT32 MQTTApp_ReportGps(ST_MQTTAppGpsData *_pstGpsData) { HIDO_UINT32 u32JsonLen = 0; ST_ReportGps stReportGps; if (HIDO_NULL == _pstGpsData) { return HIDO_ERR; } // 封装JSON if (MQTTApp_PackGpsJson(_pstGpsData, l_au8JsonBuffer, &u32JsonLen) != HIDO_OK) { HIDO_DebugEx("[MQTTApp] Pack GPS JSON failed\r\n"); return HIDO_ERR; } // 填充上报结构体 memset(&stReportGps, 0, sizeof(stReportGps)); MQTTApp_GenerateMsgID(stReportGps.msg_id, sizeof(stReportGps.msg_id), "hxzkgps"); stReportGps.timestamp = _pstGpsData->timestamp; HIDO_UtilSnprintf(stReportGps.device_id, sizeof(stReportGps.device_id), "%u", g_com_map[DEV_ID]); HIDO_UtilSnprintf(stReportGps.data_type, sizeof(stReportGps.data_type), "gps"); HIDO_UtilSnprintf(stReportGps.gps_raw, sizeof(stReportGps.gps_raw), "%s", _pstGpsData->gps_raw); memcpy(&stReportGps.imu_data, &_pstGpsData->imu_data, sizeof(ST_IMUData)); memcpy(&stReportGps.status, &_pstGpsData->status, sizeof(ST_StatusData)); // 调用MQTT客户端发送 return MQTTClient_GpsReport(&stReportGps); } /******************************************************************************* * Function Name : MQTTApp_ParseMessage * Description : 解析下发消息 * Input : _pcTopic 主题 * _pu8Payload 消息负载 * _u32PayloadLen 负载长度 * Output : None * Return : HIDO_OK/HIDO_ERR *******************************************************************************/ HIDO_INT32 MQTTApp_ParseMessage(HIDO_CHAR *_pcTopic, HIDO_UINT8 *_pu8Payload, HIDO_UINT32 _u32PayloadLen) { if (HIDO_NULL == _pcTopic || HIDO_NULL == _pu8Payload) { return HIDO_ERR; } // 入口防护：检查payload长度 if (_u32PayloadLen == 0 || _u32PayloadLen >= sizeof(l_au8JsonBuffer)) { HIDO_DebugEx("[MQTTApp] ERROR: Payload length invalid: %u (max: %u)\r\n", _u32PayloadLen, sizeof(l_au8JsonBuffer) - 1); return HIDO_ERR; } // 安全复制payload到本地缓冲区，并确保'\0'结尾 memcpy(l_au8JsonBuffer, _pu8Payload, _u32PayloadLen); l_au8JsonBuffer[_u32PayloadLen] = '\0'; HIDO_DebugEx("[MQTTApp] Recv Topic: %s, Len: %u\r\n", _pcTopic, _u32PayloadLen); // 判断主题类型 if (strstr(_pcTopic, "/control") != HIDO_NULL) { // 控制指令 ST_MQTTAppControlData stCtrlData; if (MQTTApp_ParseControlJson(l_au8JsonBuffer, _u32PayloadLen, &stCtrlData) == HIDO_OK) { if (l_stAppConfig.fnControlCallback != HIDO_NULL) { l_stAppConfig.fnControlCallback(&stCtrlData, l_stAppConfig.pControlArg); } } } else if (strstr(_pcTopic, "/path") != HIDO_NULL) { // 路径数据 static ST_MQTTAppPathData stPathData; if (MQTTApp_ParsePathJson(l_au8JsonBuffer, _u32PayloadLen, &stPathData) == HIDO_OK) { if (l_stAppConfig.fnPathCallback != HIDO_NULL) { l_stAppConfig.fnPathCallback(&stPathData, l_stAppConfig.pPathArg); } } } else if (strstr(_pcTopic, "/basestation") != HIDO_NULL) { // 基站配置 // TODO: 实现基站配置解析 } return HIDO_OK; } /******************************************************************************* * Function Name : MQTTApp_FillGpsData * Description : 填充GPS数据（供应用层调用） * Input : _pcGpsRaw GPS原始数据 * _fRoll 横滚角 * _fPitch 俯仰角 * _fYaw 偏航角 * _u8BattLevel 电池电量 * _fBattVolt 电池电压 * Output : _pstGpsData GPS数据结构 * Return : HIDO_OK/HIDO_ERR *******************************************************************************/ HIDO_INT32 MQTTApp_FillGpsData(ST_MQTTAppGpsData *_pstGpsData, const HIDO_CHAR *_pcGpsRaw, HIDO_FLOAT _fRoll, HIDO_FLOAT _fPitch, HIDO_FLOAT _fYaw, HIDO_UINT8 _u8BattLevel, HIDO_FLOAT _fBattVolt) { if (HIDO_NULL == _pstGpsData || HIDO_NULL == _pcGpsRaw) { return HIDO_ERR; } memset(_pstGpsData, 0, sizeof(ST_MQTTAppGpsData)); // 时间戳 _pstGpsData->timestamp = HIDO_TimerGetTick(); // GPS原始数据 HIDO_UtilSnprintf(_pstGpsData->gps_raw, sizeof(_pstGpsData->gps_raw), "%s", _pcGpsRaw); // IMU数据 _pstGpsData->imu_data.roll = _fRoll; _pstGpsData->imu_data.pitch = _fPitch; _pstGpsData->imu_data.yaw = _fYaw; // 状态数据 _pstGpsData->status.battery_level = _u8BattLevel; _pstGpsData->status.battery_voltage = _fBattVolt; HIDO_UtilSnprintf(_pstGpsData->status.operation_mode, sizeof(_pstGpsData->status.operation_mode), "auto"); HIDO_UtilSnprintf(_pstGpsData->status.motor_status, sizeof(_pstGpsData->status.motor_status), "running"); HIDO_UtilSnprintf(_pstGpsData->status.blade_status, sizeof(_pstGpsData->status.blade_status), "rotating"); _pstGpsData->status.blade_height = 10; _pstGpsData->status.self_check_status = 1; _pstGpsData->status.error_code = 0; HIDO_UtilSnprintf(_pstGpsData->status.error_message, sizeof(_pstGpsData->status.error_message), ""); return HIDO_OK; } /******************************************************************************* * Function Name : MQTTApp_ReportGpsFromDevice * Description : 从FML GPS模块获取数据并上报（使用GGA格式） * Input : None * Output : None * Return : HIDO_OK/HIDO_ERR *******************************************************************************/ HIDO_INT32 MQTTApp_ReportGpsFromDevice(HIDO_VOID) { ST_GPRMI stGPRMI; ST_GPGIG stGPGIG; ST_MQTTAppGpsData stGpsData; HIDO_CHAR acGgaBuffer[256]; HIDO_UINT32 ggaLen; /* 获取GPRMI数据（fmin） */ if (GPS_GetGPRMI(&stGPRMI) != HIDO_OK) { HIDO_DebugEx("[MQTTApp] GPS GPRMI data not ready\r\n"); return HIDO_ERR; } /* 获取GPGIG数据（fmig） */ if (GPS_GetGPGIG(&stGPGIG) != HIDO_OK) { HIDO_DebugEx("[MQTTApp] GPS GPGIG data not ready\r\n"); return HIDO_ERR; } /* 生成GGA报文（经纬度优先使用fmig数据） */ ggaLen = GPS_FormatGGA(&stGPRMI, &stGPGIG, acGgaBuffer, sizeof(acGgaBuffer)); if (ggaLen == 0U) { HIDO_DebugEx("[MQTTApp] GPS_FormatGGA failed\r\n"); return HIDO_ERR; } /* 填充MQTT上报数据 */ memset(&stGpsData, 0, sizeof(ST_MQTTAppGpsData)); /* 时间戳 */ stGpsData.timestamp = HIDO_TimerGetTick(); /* GPS原始数据（GGA报文） */ HIDO_UtilSnprintf(stGpsData.gps_raw, sizeof(stGpsData.gps_raw), "%s", acGgaBuffer); /* IMU数据（从fmin中获取） */ stGpsData.imu_data.roll = stGPRMI.m_fRollAngle; stGpsData.imu_data.pitch = stGPRMI.m_fPitchAngle; stGpsData.imu_data.yaw = stGPRMI.m_fHeadingAngle; /* 状态数据（TODO: 需要从实际系统获取） */ stGpsData.status.battery_level = 85; /* TODO: 从电源管理模块获取 */ stGpsData.status.battery_voltage = 24.5f; /* TODO: 从电源管理模块获取 */ HIDO_UtilSnprintf(stGpsData.status.operation_mode, sizeof(stGpsData.status.operation_mode), "auto"); HIDO_UtilSnprintf(stGpsData.status.motor_status, sizeof(stGpsData.status.motor_status), "running"); HIDO_UtilSnprintf(stGpsData.status.blade_status, sizeof(stGpsData.status.blade_status), "rotating"); stGpsData.status.blade_height = 10; stGpsData.status.self_check_status = 1; stGpsData.status.error_code = 0; HIDO_UtilSnprintf(stGpsData.status.error_message, sizeof(stGpsData.status.error_message), ""); /* 上报数据 */ return MQTTApp_ReportGps(&stGpsData); } /******************************************************************************* * Function Name : MQTTApp_ReportGpsVirtual * Description : 虚拟GPS数据上报（用于测试） * Input : None * Output : None * Return : HIDO_OK/HIDO_ERR *******************************************************************************/ HIDO_INT32 MQTTApp_ReportGpsVirtual(HIDO_VOID) { ST_MQTTAppGpsData stGpsData; static HIDO_UINT32 s_ulSeq = 0; /* 序列号，用于模拟移动 */ /* 填充MQTT上报数据 */ memset(&stGpsData, 0, sizeof(ST_MQTTAppGpsData)); /* 时间戳 */ stGpsData.timestamp = HIDO_TimerGetTick(); /* 模拟GPS原始数据（GGA报文） * 模拟位置：北京市某地点 (39.9N, 116.4E) * 每次调用经度会轻微变化以模拟移动 */ { HIDO_FLOAT fLat = 39.9042f + (s_ulSeq % 100) * 0.00001f; /* 39.90420 ~ 39.90519 */ HIDO_FLOAT fLon = 116.4074f + (s_ulSeq % 100) * 0.00001f; /* 116.40740 ~ 116.40839 */ HIDO_UINT32 ulLatDeg = (HIDO_UINT32)fLat; HIDO_UINT32 ulLonDeg = (HIDO_UINT32)fLon; HIDO_FLOAT fLatMin = (fLat - ulLatDeg) * 60.0f; HIDO_FLOAT fLonMin = (fLon - ulLonDeg) * 60.0f; HIDO_UINT32 ulHour = (HIDO_TimerGetTick() / 3600000) % 24; HIDO_UINT32 ulMin = (HIDO_TimerGetTick() / 60000) % 60; HIDO_UINT32 ulSec = (HIDO_TimerGetTick() / 1000) % 60; /* 格式: $GPGGA,hhmmss.ss,ddmm.mmmm,N,dddmm.mmmm,E,1,08,1.0,45.0,M,0.0,M,,*checksum */ HIDO_UtilSnprintf(stGpsData.gps_raw, sizeof(stGpsData.gps_raw), "$GPGGA,%02u%02u%02u.00,%02u%07.4f,N,%03u%07.4f,E,1,08,1.0,45.0,M,0.0,M,,*47", ulHour, ulMin, ulSec, ulLatDeg, fLatMin, ulLonDeg, fLonMin); } /* 模拟IMU数据（轻微摆动） */ stGpsData.imu_data.roll = (s_ulSeq % 20 - 10) * 0.5f; /* -5.0 ~ +5.0 度 */ stGpsData.imu_data.pitch = (s_ulSeq % 30 - 15) * 0.3f; /* -4.5 ~ +4.5 度 */ stGpsData.imu_data.yaw = (s_ulSeq % 360) * 1.0f; /* 0 ~ 359 度 */ /* 模拟状态数据 */ stGpsData.status.battery_level = 85 - (s_ulSeq / 100) % 20; /* 85% ~ 65% 循环变化 */ stGpsData.status.battery_voltage = 24.5f - (s_ulSeq / 100) % 2 * 0.5f; /* 24.5V ~ 23.5V */ HIDO_UtilSnprintf(stGpsData.status.operation_mode, sizeof(stGpsData.status.operation_mode), "auto"); HIDO_UtilSnprintf(stGpsData.status.motor_status, sizeof(stGpsData.status.motor_status), (s_ulSeq % 2 == 0) ? "running" : "idle"); HIDO_UtilSnprintf(stGpsData.status.blade_status, sizeof(stGpsData.status.blade_status), (s_ulSeq % 3 == 0) ? "rotating" : "stopped"); stGpsData.status.blade_height = 10 + (s_ulSeq % 5); /* 10 ~ 14 mm */ stGpsData.status.self_check_status = 1; stGpsData.status.error_code = 0; HIDO_UtilSnprintf(stGpsData.status.error_message, sizeof(stGpsData.status.error_message), "Test Mode"); /* 更新序列号 */ s_ulSeq++; /* 上报数据 */ return MQTTApp_ReportGps(&stGpsData); }