加入vsmd test

src/main.cpp

@@ -1,6 +1,8 @@
 #include <ArduinoJson.h>
 #include "IRIS_Method.h"
+#include "vsmd_parser.h"
 
+VSMDParser parser;
 
 #define DEBUG_TX_PIN 17  // TX
 #define DEBUG_RX_PIN 16  // RX
@@ -57,16 +59,17 @@ void processJsonCommand(const char* jsonString);
 void processStringCommand(const char* stringData);
 void sendJsonResponse(const char* cmd, const char* status, int value = -1);
 void sendStringResponse(const char* response);
-void handleConfigCommand(JsonObject& doc);
+void handle_cfg_cmd();
-void handleEnableCommand();
+void handle_ena_cmd();
-void handleOffCommand();
+void handle_off_cmd();
-void handleMoveCommand();
+void handle_mov_cmd();
-void handlePositionCommand(int value);
+void handle_pos_cmd(int value);
-void handleRelativeMoveCommand(int value);
+void handle_rmv_cmd(int value);
-void handlePulsePerSecondCommand(int value = -1);
+void handle_pps_cmd(int value = -1);
-void handleOriginCommand();
+void handle_org_cmd();
-void handleStopCommand();
+void handle_stp_cmd();
-void handleZeroCommand(const char* value);
+void handle_zero_cmd(const char* value);
+void handle_dev_cmd();
 void printDeviceStatus();
 
 void setup() {
@@ -77,7 +80,7 @@ void setup() {
     Serial.begin(115200);
     
     DebugSerial.println("VSMD INIT");
-    DebugSerial.println("Commands: cfg, ena, off, mov, pos, rmv, pps, org, stp, zero");
+    DebugSerial.println("Commands:dev, cfg, ena, off, mov, pos, rmv, pps, org, stp, zero");
     printDeviceStatus();
 }
 
@@ -85,6 +88,22 @@ void loop() {
     if (DebugSerial.available() > 0) {
         int bytesRead = DebugSerial.readBytes(receiveBuffer, sizeof(receiveBuffer));
         
+        // // 添加调试信息
+        // DebugSerial.print("Bytes read: ");
+        // DebugSerial.println(bytesRead);
+        // DebugSerial.print("Raw data: ");
+        // for(int i = 0; i < bytesRead; i++) {
+        //     DebugSerial.print(receiveBuffer[i], HEX);
+        //     DebugSerial.print(" ");
+        // }
+        // DebugSerial.println();
+        
+        // bytesProcessed = IRIS_Cut_Befor_Header(receiveBuffer, bytesRead);
+        
+        // // 添加更多调试信息
+        // DebugSerial.print("Bytes after header cut: ");
+        // DebugSerial.println(bytesProcessed);
+
         bytesProcessed = IRIS_Cut_Befor_Header(receiveBuffer, bytesRead);
         
         if (bytesProcessed > 0) {
@@ -148,44 +167,51 @@ void processJsonCommand(const char* jsonString) {
         return;
     }
     
-    const char* cmd = doc["cmd"];
+    // 使用 command 字段替代 cmd
-    if (cmd == nullptr) {
+    const char* command = doc["command"];
-        sendJsonResponse("error", "NO_CMD_FIELD");
+    if (command == nullptr) {
+        sendJsonResponse("error", "NO_COMMAND_FIELD");
         return;
     }
     
-    // 根据cmd执行相应操作
+    // 获取 PA 参数对象（可选）
-    if (strcmp(cmd, "cfg") == 0) {
+    JsonObject params = doc["PA"];
-        JsonObject configObj = doc.as<JsonObject>();
+
-        handleConfigCommand(configObj);
+    // 根据command执行相应操作
-    } else if (strcmp(cmd, "ena") == 0) {
+    if (strcmp(command, "cfg") == 0) {
-        handleEnableCommand();
+        handle_cfg_cmd();
-    } else if (strcmp(cmd, "off") == 0) {
+    } else if (strcmp(command, "ena") == 0) {
-        handleOffCommand();
+        handle_ena_cmd();
-    } else if (strcmp(cmd, "mov") == 0) {
+    } else if (strcmp(command, "off") == 0) {
-        handleMoveCommand();
+        handle_off_cmd();
-    } else if (strcmp(cmd, "pos") == 0) {
+    } else if (strcmp(command, "mov") == 0) {
-        int value = doc["value"] | 0;
+        handle_mov_cmd();
-        handlePositionCommand(value);
+    } else if (strcmp(command, "pos") == 0) {
-    } else if (strcmp(cmd, "rmv") == 0) {
+        // 修正：正确的类型转换
-        int value = doc["value"] | 0;
+        int value = params.containsKey("value") ? params["value"].as<int>() : 0;
-        handleRelativeMoveCommand(value);
+        handle_pos_cmd(value);
-    } else if (strcmp(cmd, "pps") == 0) {
+    } else if (strcmp(command, "rmv") == 0) {
-        if (doc.containsKey("value")) {
+        int value = params.containsKey("value") ? params["value"].as<int>() : 0;
-            int value = doc["value"];
+        handle_rmv_cmd(value);
-            handlePulsePerSecondCommand(value);
+    } else if (strcmp(command, "pps") == 0) {
+        if (params.containsKey("value")) {
+            int value = params["value"].as<int>();
+            handle_pps_cmd(value);
         } else {
-            handlePulsePerSecondCommand();
+            handle_pps_cmd();
         }
-    } else if (strcmp(cmd, "org") == 0) {
+    } else if (strcmp(command, "org") == 0) {
-        handleOriginCommand();
+        handle_org_cmd();
-    } else if (strcmp(cmd, "stp") == 0) {
+    } else if (strcmp(command, "stp") == 0) {
-        handleStopCommand();
+        handle_stp_cmd();
-    } else if (strcmp(cmd, "zero") == 0) {
+    } else if (strcmp(command, "zero") == 0) {
-        const char* value = doc["value"] | "";
+        // 修正：正确的字符串类型转换
-        handleZeroCommand(value);
+        const char* value = params.containsKey("value") ? params["value"].as<const char*>() : "";
+        handle_zero_cmd(value);
+    } else if (strcmp(command, "dev") == 0) {
+        handle_dev_cmd();
     } else {
-        sendJsonResponse(cmd, "UNKNOWN_COMMAND");
+        sendJsonResponse(command, "UNKNOWN_COMMAND");
     }
 }
 
@@ -195,19 +221,83 @@ void processStringCommand(const char* stringData) {
     sendStringResponse(response.c_str());
 }
 
-void sendJsonResponse(const char* cmd, const char* status, int value) {
+void sendJsonResponse(const char* command, const char* status, int value) {
     StaticJsonDocument<JSON_BUFFER_SIZE> responseDoc;
-    responseDoc["cmd"] = cmd;
-    responseDoc["status"] = status;
     
+    // 修改：使用新的JSON格式
+    responseDoc["command"] = command;  // 使用 command 替代 cmd
+    responseDoc["ST"] = status;        // 使用 ST 替代 status
+    
+    // 如果有参数值，添加到 PA 对象中
     if (value != -1) {
-        responseDoc["value"] = value;
+        JsonObject params = responseDoc.createNestedObject("PA");
+        params["value"] = value;
     }
     
-    // 设备状态信息
+    // 可选：添加其他信息
-    responseDoc["enabled"] = deviceEnabled;
+    responseDoc["Other"] = "";
-    responseDoc["position"] = currentPosition;
+    
-    responseDoc["moving"] = isMoving;
+    char jsonResponse[JSON_BUFFER_SIZE];
+    serializeJson(responseDoc, jsonResponse, sizeof(jsonResponse));
+    
+    // 打包并发送JSON响应
+    DebugSerial.print("JSON Response: ");
+    packedLength = IRIS_Protocol_Pack(0x00, strlen(jsonResponse), (uint8_t*)jsonResponse, sendBuffer);
+    DebugSerial.write(sendBuffer, packedLength);
+
+    // DebugSerial.println();
+    
+    // DebugSerial.print("Sent JSON Response: ");
+    // DebugSerial.println(jsonResponse);
+}
+
+void sendStringResponse(const char* response) {
+    // 打包并发送字符串响应
+    packedLength = IRIS_Protocol_Pack(0x01, strlen(response), (uint8_t*)response, sendBuffer);
+    DebugSerial.write(sendBuffer, packedLength);
+    
+    DebugSerial.print("Sent String Response: ");
+    DebugSerial.println(response);
+}
+
+void handle_cfg_cmd() {
+    DebugSerial.println("Configuration requested");
+    Serial.println("cfg\n");
+    // 返回配置相关信息
+    sendJsonResponse("cfg", "OK");
+}
+
+// void handle_dev_cmd() {
+//     DebugSerial.println("dev_requested");
+//     Serial.println("dev\n");
+
+//     //test VSMD数据
+//     uint8_t testData[] = {
+//         0xFF, 0x00, 0x01, 0x56, 0x53, 0x4D, 0x44, 0x31, 0x34, 0x32, 0x2D, 0x30, 0x32, 0x35, 0x54, 0x2D, 0x31, 0x2E, 0x31, 0x2E, 0x30, 0x32, 0x30, 0x2E, 0x32, 0x31, 0x31, 0x31, 0x31, 0x35, 0x00, 0x42, 0xFE
+//     };
+//     // 解析
+//     DebugSerial.println("Parsing VSMD test data:");
+//     parser.parse(testData, sizeof(testData));
+//     // 返回设备相关信息
+//     sendJsonResponse("dev", "OK");
+// }
+void handle_dev_cmd() {
+    DebugSerial.println("dev_requested");
+    Serial.println("dev\n");
+    
+    // vsmd test data
+    uint8_t testData[] = {
+        0xFF, 0x00, 0x01, 0x56, 0x53, 0x4D, 0x44, 0x31, 0x34, 0x32, 0x2D, 0x30, 0x32, 0x35, 0x54, 0x2D,0x31, 0x2E, 0x31, 0x2E, 0x30, 0x32, 0x30, 0x2E, 0x32, 0x31, 0x31, 0x31, 0x31, 0x35, 0x00, 0x42, 0xFE
+    };
+    
+    // parseInfo获取解析结果
+    String deviceInfo = parser.parseInfo(testData, sizeof(testData));
+    
+    // 创建带有解析数据的JSON响应
+    StaticJsonDocument<JSON_BUFFER_SIZE> responseDoc;
+    responseDoc["command"] = "dev";
+    responseDoc["ST"] = "OK";
+    responseDoc["Other"] = deviceInfo;
     
     char jsonResponse[JSON_BUFFER_SIZE];
     serializeJson(responseDoc, jsonResponse, sizeof(jsonResponse));
@@ -222,162 +312,14 @@ void sendJsonResponse(const char* cmd, const char* status, int value) {
     DebugSerial.println(jsonResponse);
 }
 
-void sendStringResponse(const char* response) {
+void handle_ena_cmd() {
-    // 打包并发送字符串响应
-    packedLength = IRIS_Protocol_Pack(0x01, strlen(response), (uint8_t*)response, sendBuffer);
-    DebugSerial.write(sendBuffer, packedLength);
-    
-    DebugSerial.print("Sent String Response: ");
-    DebugSerial.println(response);
-}
-
-void handleConfigCommand(JsonObject& doc) {
-    DebugSerial.println("Processing configuration command...");
-    
-    // 创建响应JSON对象
-    StaticJsonDocument<JSON_BUFFER_SIZE> responseDoc;
-    responseDoc["cmd"] = "cfg";
-    responseDoc["status"] = "OK";
-    
-    // 添加设备状态信息
-    responseDoc["enabled"] = deviceEnabled;
-    responseDoc["position"] = currentPosition;
-    responseDoc["moving"] = isMoving;
-    
-    // 创建配置参数对象
-    JsonObject configParams = responseDoc.createNestedObject("config");
-    
-    // 处理配置参数
-    if (doc.containsKey("params")) {
-        JsonObject params = doc["params"];
-        
-        // 更新配置参数并添加到响应中
-        if (params.containsKey("cfg zmd")) {
-            deviceConfig.zmd = params["cfg zmd"];
-            configParams["cfg zmd"] = deviceConfig.zmd;
-        }
-        if (params.containsKey("cfg snr")) {
-            deviceConfig.snr = params["cfg snr"];
-            configParams["cfg snr"] = deviceConfig.snr;
-        }
-        if (params.containsKey("cfg osv")) {
-            deviceConfig.osv = params["cfg osv"];
-            configParams["cfg osv"] = deviceConfig.osv;
-        }
-        if (params.containsKey("cfg zsd")) {
-            deviceConfig.zsd = params["cfg zsd"];
-            configParams["cfg zsd"] = deviceConfig.zsd;
-        }
-        if (params.containsKey("cfg zsp")) {
-            deviceConfig.zsp = params["cfg zsp"];
-            configParams["cfg zsp"] = deviceConfig.zsp;
-        }
-        if (params.containsKey("cfg dmd")) {
-            deviceConfig.dmd = params["cfg dmd"];
-            configParams["cfg dmd"] = deviceConfig.dmd;
-        }
-        if (params.containsKey("cfg dar")) {
-            deviceConfig.dar = params["cfg dar"];
-            configParams["cfg dar"] = deviceConfig.dar;
-        }
-        if (params.containsKey("cfg msr")) {
-            deviceConfig.msr = params["cfg msr"];
-            configParams["cfg msr"] = deviceConfig.msr;
-        }
-        if (params.containsKey("cfg msv")) {
-            deviceConfig.msv = params["cfg msv"];
-            configParams["cfg msv"] = deviceConfig.msv;
-        }
-        if (params.containsKey("cfg psr")) {
-            deviceConfig.psr = params["cfg psr"];
-            configParams["cfg psr"] = deviceConfig.psr;
-        }
-        if (params.containsKey("cfg psv")) {
-            deviceConfig.psv = params["cfg psv"];
-            configParams["cfg psv"] = deviceConfig.psv;
-        }
-        if (params.containsKey("cfg bdr")) {
-            deviceConfig.bdr = params["cfg bdr"];
-            configParams["cfg bdr"] = deviceConfig.bdr;
-        }
-        if (params.containsKey("cfg cid")) {
-            deviceConfig.cid = params["cfg cid"];
-            configParams["cfg cid"] = deviceConfig.cid;
-        }
-        if (params.containsKey("cfg mcs")) {
-            deviceConfig.mcs = params["cfg mcs"];
-            configParams["cfg mcs"] = deviceConfig.mcs;
-        }
-        if (params.containsKey("cfg spd")) {
-            deviceConfig.spd = params["cfg spd"];
-            configParams["cfg spd"] = deviceConfig.spd;
-        }
-        if (params.containsKey("cfg acc")) {
-            deviceConfig.acc = params["cfg acc"];
-            configParams["cfg acc"] = deviceConfig.acc;
-        }
-        if (params.containsKey("cfg dec")) {
-            deviceConfig.dec = params["cfg dec"];
-            configParams["cfg dec"] = deviceConfig.dec;
-        }
-        if (params.containsKey("cfg cra")) {
-            deviceConfig.cra = params["cfg cra"];
-            configParams["cfg cra"] = deviceConfig.cra;
-        }
-        if (params.containsKey("cfg crn")) {
-            deviceConfig.crn = params["cfg crn"];
-            configParams["cfg crn"] = deviceConfig.crn;
-        }
-        if (params.containsKey("cfg crh")) {
-            deviceConfig.crh = params["cfg crh"];
-            configParams["cfg crh"] = deviceConfig.crh;
-        }
-        
-        DebugSerial.println("Configuration updated successfully");
-    } else {
-        // 如果没有params，返回所有当前配置
-        configParams["cfg zmd"] = deviceConfig.zmd;
-        configParams["cfg snr"] = deviceConfig.snr;
-        configParams["cfg osv"] = deviceConfig.osv;
-        configParams["cfg zsd"] = deviceConfig.zsd;
-        configParams["cfg zsp"] = deviceConfig.zsp;
-        configParams["cfg dmd"] = deviceConfig.dmd;
-        configParams["cfg dar"] = deviceConfig.dar;
-        configParams["cfg msr"] = deviceConfig.msr;
-        configParams["cfg msv"] = deviceConfig.msv;
-        configParams["cfg psr"] = deviceConfig.psr;
-        configParams["cfg psv"] = deviceConfig.psv;
-        configParams["cfg bdr"] = deviceConfig.bdr;
-        configParams["cfg cid"] = deviceConfig.cid;
-        configParams["cfg mcs"] = deviceConfig.mcs;
-        configParams["cfg spd"] = deviceConfig.spd;
-        configParams["cfg acc"] = deviceConfig.acc;
-        configParams["cfg dec"] = deviceConfig.dec;
-        configParams["cfg cra"] = deviceConfig.cra;
-        configParams["cfg crn"] = deviceConfig.crn;
-        configParams["cfg crh"] = deviceConfig.crh;
-    }
-    
-    // 序列化并发送响应
-    char jsonResponse[JSON_BUFFER_SIZE];
-    serializeJson(responseDoc, jsonResponse, sizeof(jsonResponse));
-    
-    // 打包并发送JSON响应
-    packedLength = IRIS_Protocol_Pack(0x00, strlen(jsonResponse), (uint8_t*)jsonResponse, sendBuffer);
-    DebugSerial.write(sendBuffer, packedLength);
-    
-    DebugSerial.print("Sent Config Response: ");
-    DebugSerial.println(jsonResponse);
-}
-
-void handleEnableCommand() {
     deviceEnabled = true;
     DebugSerial.println("Device enabled");
     Serial.print("ena\n");
     sendJsonResponse("ena", "OK");
 }
 
-void handleOffCommand() {
+void handle_off_cmd() {
     deviceEnabled = false;
     isMoving = false;
     DebugSerial.println("Device disabled");
@@ -385,7 +327,7 @@ void handleOffCommand() {
     sendJsonResponse("off", "OK");
 }
 
-void handleMoveCommand() {
+void handle_mov_cmd() {
     if (!deviceEnabled) {
         sendJsonResponse("mov", "DEVICE_DISABLED");
         return;
@@ -397,7 +339,7 @@ void handleMoveCommand() {
     sendJsonResponse("mov", "OK");
 }
 
-void handlePositionCommand(int value) {
+void handle_pos_cmd(int value) {
     if (!deviceEnabled) {
         sendJsonResponse("pos", "DEVICE_DISABLED");
         return;
@@ -410,7 +352,7 @@ void handlePositionCommand(int value) {
     sendJsonResponse("pos", "OK", value);
 }
 
-void handleRelativeMoveCommand(int value) {
+void handle_rmv_cmd(int value) {
     if (!deviceEnabled) {
         sendJsonResponse("rmv", "DEVICE_DISABLED");
         return;
@@ -424,7 +366,7 @@ void handleRelativeMoveCommand(int value) {
     sendJsonResponse("rmv", "OK", currentPosition);
 }
 
-void handlePulsePerSecondCommand(int value) {
+void handle_pps_cmd(int value) {
     if (value == -1) {
         // SET PPS
         Serial.print("pps\n");
@@ -438,7 +380,7 @@ void handlePulsePerSecondCommand(int value) {
     }
 }
 
-void handleOriginCommand() {
+void handle_org_cmd() {
     if (!deviceEnabled) {
         sendJsonResponse("org", "DEVICE_DISABLED");
         return;
@@ -451,14 +393,14 @@ void handleOriginCommand() {
     sendJsonResponse("org", "OK", 0);
 }
 
-void handleStopCommand() {
+void handle_stp_cmd() {
     isMoving = false;
     DebugSerial.println("Movement stopped");
     Serial.print("stp\n");
     sendJsonResponse("stp", "OK");
 }
 
-void handleZeroCommand(const char* value) {
+void handle_zero_cmd(const char* value) {
     if (strcmp(value, "start") == 0) {
         Serial.print("zero start\n");
         isBackZero = true;

src/vsmd_parser.cpp

@@ -1,7 +1,7 @@
 #include "vsmd_parser.h"
 #include "ble.h"
 
-extern HardwareSerial debugSerial;
+extern HardwareSerial DebugSerial;
 
 // BCC 校验函数
 uint8_t VSMDParser::bcc_checksum(uint8_t* data, int size) {
@@ -30,30 +30,218 @@ void VSMDParser::convert(uint8_t* data, value_info& info) {
     info.udata |= data[4];
 }
 
-// 解析函数
+// VSMD 解析函数
-// void VSMDParser::parse(uint8_t* data, int size) {
+void VSMDParser::parse(uint8_t* data, int size) {
-//     // if (size < 10) {
+    // if (size < 10) {
-//     //     debugSerial.println("data too short");
+    //     DebugSerial.println("data too short");
-//     //     return;
+    //     return;
-//     // }
+    // }
 
+    if (data[0] == 0xFF && data[size - 1] == 0xFE) {
+        uint8_t checksum = bcc_checksum(&data[1], size - 3);
+        if (checksum == data[size - 2]) {
+            switch (data[2]) {
+                case 1: { // dev 01
+                    DebugSerial.print("dev:");
+                    int start = 0;
+                    while (start < size && data[start] != 'V') start++;
+                    if (start < size) {
+                        int end = start;
+                        while (end < size && data[end] >= 32 && data[end] < 127) end++;
+                        for (int i = start; i < end; i++) {
+                            DebugSerial.print((char)data[i]);
+                        }
+                        DebugSerial.println();
+                    } else {
+                        DebugSerial.println("dev err");
+                    }
+                    break;
+                }
+                case 2: { // sts 02
+                    float spd = 0;
+                    int pos = 0;
+                    uint32_t sts = 0;
+                    value_info info;
+
+                    convert(&data[3], info);
+                    spd = info.fdata;
+
+                    convert(&data[8], info);
+                    pos = info.idata;
+
+                    convert(&data[13], info);
+                    sts = info.udata;
+            
+                    DebugSerial.print("Speed: ");
+                    DebugSerial.print(spd);
+                    DebugSerial.print(", Position: ");
+                    DebugSerial.print(pos);
+                    DebugSerial.print(", Status: ");
+                    DebugSerial.println(sts);
+
+                    break;
+                }
+                case 3: { // cfg 03
+                    DebugSerial.print("cfg:");
+                    int payloadStart = 3;
+                    int payloadEnd = size - 3;
+
+                    if (payloadStart < payloadEnd) {
+                        for (int i = payloadStart; i < payloadEnd; i++) {
+                            DebugSerial.print((char)data[i]);
+                        }
+                        DebugSerial.println();
+                    } else {
+                        DebugSerial.println(" 数据长度不足");
+                    }
+                    break;
+                }
+                case 4: { // demo 04
+                    DebugSerial.print("demo:");
+                    int payloadStart = 3;
+                    int payloadEnd = size - 3;
+
+                    if (payloadStart < payloadEnd) {
+                        for (int i = payloadStart; i < payloadEnd; i++) {
+                            DebugSerial.print((char)data[i]);
+                        }
+                        DebugSerial.println();
+                    } else {
+                        DebugSerial.println(" 数据长度不足");
+                    }
+                    break;
+                }
+                default:
+                    DebugSerial.print("Unknow cmd: ");
+                    DebugSerial.println(data[2]);
+                    break;
+            }
+        } else {
+            return;
+            DebugSerial.println("BCC ERROR");
+            DebugSerial.print("CHECK BCC: ");
+            DebugSerial.println(checksum);
+            DebugSerial.print("RES BCC: ");
+            DebugSerial.println(data[size - 2], HEX);
+        }
+    } else {
+        DebugSerial.println("Head or  Tail ERROR");
+        DebugSerial.print("Receive Data: ");
+        for (int i = 0; i < size; i++) {
+            DebugSerial.print(data[i], HEX);
+            DebugSerial.print(" ");
+        }
+        DebugSerial.println();
+    }
+}
+
+
+// 新增parseInfo函数,返回解析结果
+String VSMDParser::parseInfo(uint8_t* data, int size) {
+    String result = "";
+    
+    if (data[0] == 0xFF && data[size - 1] == 0xFE) {
+        uint8_t checksum = bcc_checksum(&data[1], size - 3);
+        if (checksum == data[size - 2]) {
+            switch (data[2]) {
+                case 1: { // dev 01
+                    int start = 0;
+                    while (start < size && data[start] != 'V') start++;
+                    if (start < size) {
+                        int end = start;
+                        while (end < size && data[end] >= 32 && data[end] < 127) end++;
+                        for (int i = start; i < end; i++) {
+                            result += (char)data[i];
+                        }
+                    } else {
+                        result = "dev err";
+                    }
+                    break;
+                }
+                case 2: { // sts 02
+                    float spd = 0;
+                    int pos = 0;
+                    uint32_t sts = 0;
+                    value_info info;
+
+                    convert(&data[3], info);
+                    spd = info.fdata;
+
+                    convert(&data[8], info);
+                    pos = info.idata;
+
+                    convert(&data[13], info);
+                    sts = info.udata;
+            
+                    result = "Speed: " + String(spd) + ", Position: " + String(pos) + ", Status: " + String(sts);
+                    break;
+                }
+                case 3: { // cfg 03
+                    int payloadStart = 3;
+                    int payloadEnd = size - 3;
+
+                    if (payloadStart < payloadEnd) {
+                        for (int i = payloadStart; i < payloadEnd; i++) {
+                            result += (char)data[i];
+                        }
+                    } else {
+                        result = "数据长度不足";
+                    }
+                    break;
+                }
+                case 4: { // demo 04
+                    int payloadStart = 3;
+                    int payloadEnd = size - 3;
+
+                    if (payloadStart < payloadEnd) {
+                        for (int i = payloadStart; i < payloadEnd; i++) {
+                            result += (char)data[i];
+                        }
+                    } else {
+                        result = "Not enough data";
+                    }
+                    break;
+                }
+                default:
+                    result = "Unknown cmd: " + String(data[2]);
+                    break;
+            }
+        } else {
+            result = "BCC ERROR";
+        }
+    } else {
+        result = "Head or Tail ERROR";
+    }
+    
+    return result;
+}
+
+// ... existing code ...
+// /**************************** ble read ************************************ */
+// void VSMDParser::parse(uint8_t* data, int size) {
 //     if (data[0] == 0xFF && data[size - 1] == 0xFE) {
 //         uint8_t checksum = bcc_checksum(&data[1], size - 3);
 //         if (checksum == data[size - 2]) {
 //             switch (data[2]) {
 //                 case 1: { // dev
-//                     debugSerial.print("dev:");
+//                     String response = "dev:";
 //                     int start = 0;
 //                     while (start < size && data[start] != 'V') start++;
 //                     if (start < size) {
 //                         int end = start;
 //                         while (end < size && data[end] >= 32 && data[end] < 127) end++;
 //                         for (int i = start; i < end; i++) {
-//                             debugSerial.print((char)data[i]);
+//                             response += (char)data[i];
 //                         }
-//                         debugSerial.println();
+//                         response += "\n";
 //                     } else {
-//                         debugSerial.println("dev err");
+//                         response += "dev err\n";
+//                     }
+
+//                     // 发送通过蓝牙
+//                     if (pCharacteristic) {
+//                         pCharacteristic->setValue(response.c_str());
+//                         pCharacteristic->notify();
 //                     }
 //                     break;
 //                 }
@@ -72,201 +260,95 @@ void VSMDParser::convert(uint8_t* data, value_info& info) {
 //                     convert(&data[13], info);
 //                     sts = info.udata;
 
-//                     debugSerial.print("Speed: ");
+//                     String response = "Speed: " + String(spd) + ", Position: " + String(pos) + ", Status: " + String(sts) + "\n";
-//                     debugSerial.print(spd);
-//                     debugSerial.print(", Position: ");
-//                     debugSerial.print(pos);
-//                     debugSerial.print(", Status: ");
-//                     debugSerial.println(sts);
 
+//                     // 发送通过蓝牙
+//                     if (pCharacteristic) {
+//                         pCharacteristic->setValue(response.c_str());
+//                         pCharacteristic->notify();
+//                     }
 //                     break;
 //                 }
 //                 case 3: { // cfg
-//                     debugSerial.print("cfg:");
+//                     String response = "cfg:";
 //                     int payloadStart = 3;
 //                     int payloadEnd = size - 3;
 
 //                     if (payloadStart < payloadEnd) {
 //                         for (int i = payloadStart; i < payloadEnd; i++) {
-//                             debugSerial.print((char)data[i]);
+//                             response += (char)data[i];
 //                         }
-//                         debugSerial.println();
+//                         response += "\n";
 //                     } else {
-//                         debugSerial.println(" 数据长度不足");
+//                         response += " 数据长度不足\n";
+//                     }
+
+//                     // 发送通过蓝牙
+//                     if (pCharacteristic) {
+//                         pCharacteristic->setValue(response.c_str());
+//                         pCharacteristic->notify();
 //                     }
 //                     break;
 //                 }
 //                 case 4: { // demo
-//                     debugSerial.print("demo:");
+//                     String response = "demo:";
 //                     int payloadStart = 3;
 //                     int payloadEnd = size - 3;
 
 //                     if (payloadStart < payloadEnd) {
 //                         for (int i = payloadStart; i < payloadEnd; i++) {
-//                             debugSerial.print((char)data[i]);
+//                             response += (char)data[i];
 //                         }
-//                         debugSerial.println();
+//                         response += "\n";
 //                     } else {
-//                         debugSerial.println(" 数据长度不足");
+//                         response += " 数据长度不足\n";
+//                     }
+
+//                     // 发送通过蓝牙
+//                     if (pCharacteristic) {
+//                         pCharacteristic->setValue(response.c_str());
+//                         pCharacteristic->notify();
 //                     }
 //                     break;
 //                 }
 //                 default:
-//                     debugSerial.print("未知命令: ");
+//                     String response = "未知命令: ";
-//                     debugSerial.println(data[2]);
+//                     response += data[2];
+//                     response += "\n";
+
+//                     // 发送通过蓝牙
+//                     if (pCharacteristic) {
+//                         pCharacteristic->setValue(response.c_str());
+//                         pCharacteristic->notify();
+//                     }
 //                     break;
 //             }
 //         } else {
-//             return;
+//             // BCC 校验失败
-//             debugSerial.println("BCC ERROR");
+//             String response = "BCC ERROR\nCHECK BCC: ";
-//             debugSerial.print("CHECK BCC: ");
+//             response += checksum;
-//             debugSerial.println(checksum);
+//             response += "\nRES BCC: ";
-//             debugSerial.print("RES BCC: ");
+//             response += data[size - 2], HEX;
-//             debugSerial.println(data[size - 2], HEX);
+//             response += "\n";
+
+//             // 发送通过蓝牙
+//             if (pCharacteristic) {
+//                 pCharacteristic->setValue(response.c_str());
+//                 pCharacteristic->notify();
+//             }
 //         }
 //     } else {
-//         debugSerial.println("Head or  Tail ERROR");
+//         String response = "Head or Tail ERROR\nReceive Data: ";
-//         debugSerial.print("Receive Data: ");
 //         for (int i = 0; i < size; i++) {
-//             debugSerial.print(data[i], HEX);
+//             response += data[i], HEX;
-//             debugSerial.print(" ");
+//             response += " ";
+//         }
+//         response += "\n";
+
+//         // 发送通过蓝牙
+//         if (pCharacteristic) {
+//             pCharacteristic->setValue(response.c_str());
+//             pCharacteristic->notify();
 //         }
-//         debugSerial.println();
 //     }
 // }
-
-/**************************** ble read ************************************ */
-void VSMDParser::parse(uint8_t* data, int size) {
-    if (data[0] == 0xFF && data[size - 1] == 0xFE) {
-        uint8_t checksum = bcc_checksum(&data[1], size - 3);
-        if (checksum == data[size - 2]) {
-            switch (data[2]) {
-                case 1: { // dev
-                    String response = "dev:";
-                    int start = 0;
-                    while (start < size && data[start] != 'V') start++;
-                    if (start < size) {
-                        int end = start;
-                        while (end < size && data[end] >= 32 && data[end] < 127) end++;
-                        for (int i = start; i < end; i++) {
-                            response += (char)data[i];
-                        }
-                        response += "\n";
-                    } else {
-                        response += "dev err\n";
-                    }
-
-                    // 发送通过蓝牙
-                    if (pCharacteristic) {
-                        pCharacteristic->setValue(response.c_str());
-                        pCharacteristic->notify();
-                    }
-                    break;
-                }
-                case 2: { // sts
-                    float spd = 0;
-                    int pos = 0;
-                    uint32_t sts = 0;
-                    value_info info;
-
-                    convert(&data[3], info);
-                    spd = info.fdata;
-
-                    convert(&data[8], info);
-                    pos = info.idata;
-
-                    convert(&data[13], info);
-                    sts = info.udata;
-
-                    String response = "Speed: " + String(spd) + ", Position: " + String(pos) + ", Status: " + String(sts) + "\n";
-
-                    // 发送通过蓝牙
-                    if (pCharacteristic) {
-                        pCharacteristic->setValue(response.c_str());
-                        pCharacteristic->notify();
-                    }
-                    break;
-                }
-                case 3: { // cfg
-                    String response = "cfg:";
-                    int payloadStart = 3;
-                    int payloadEnd = size - 3;
-
-                    if (payloadStart < payloadEnd) {
-                        for (int i = payloadStart; i < payloadEnd; i++) {
-                            response += (char)data[i];
-                        }
-                        response += "\n";
-                    } else {
-                        response += " 数据长度不足\n";
-                    }
-
-                    // 发送通过蓝牙
-                    if (pCharacteristic) {
-                        pCharacteristic->setValue(response.c_str());
-                        pCharacteristic->notify();
-                    }
-                    break;
-                }
-                case 4: { // demo
-                    String response = "demo:";
-                    int payloadStart = 3;
-                    int payloadEnd = size - 3;
-
-                    if (payloadStart < payloadEnd) {
-                        for (int i = payloadStart; i < payloadEnd; i++) {
-                            response += (char)data[i];
-                        }
-                        response += "\n";
-                    } else {
-                        response += " 数据长度不足\n";
-                    }
-
-                    // 发送通过蓝牙
-                    if (pCharacteristic) {
-                        pCharacteristic->setValue(response.c_str());
-                        pCharacteristic->notify();
-                    }
-                    break;
-                }
-                default:
-                    String response = "未知命令: ";
-                    response += data[2];
-                    response += "\n";
-
-                    // 发送通过蓝牙
-                    if (pCharacteristic) {
-                        pCharacteristic->setValue(response.c_str());
-                        pCharacteristic->notify();
-                    }
-                    break;
-            }
-        } else {
-            // BCC 校验失败
-            String response = "BCC ERROR\nCHECK BCC: ";
-            response += checksum;
-            response += "\nRES BCC: ";
-            response += data[size - 2], HEX;
-            response += "\n";
-
-            // 发送通过蓝牙
-            if (pCharacteristic) {
-                pCharacteristic->setValue(response.c_str());
-                pCharacteristic->notify();
-            }
-        }
-    } else {
-        String response = "Head or Tail ERROR\nReceive Data: ";
-        for (int i = 0; i < size; i++) {
-            response += data[i], HEX;
-            response += " ";
-        }
-        response += "\n";
-
-        // 发送通过蓝牙
-        if (pCharacteristic) {
-            pCharacteristic->setValue(response.c_str());
-            pCharacteristic->notify();
-        }
-    }
-}

src/vsmd_parser.h

@@ -4,7 +4,6 @@
 #include <Arduino.h>
 #include "ble.h"
 
-
 // 联合体定义：用于数据转换
 typedef union {
     float fdata;
@@ -17,7 +16,8 @@ class VSMDParser {
 public:
     VSMDParser();
 
-    void parse(uint8_t* data, int size);
+    void parse(uint8_t* data, int size);    //串口打印
+    String parseInfo(uint8_t* data, int size);  // 新增方法,retuen Info
 
 private:
     // BCC校验函数