first

src/IRIS_Method.c

@@ -0,0 +1,208 @@
+/**
+  ******************************************************************************
+  * @file           : IRIS_Method.c
+  * @author         : xin
+  * @brief          : None
+  * @attention      : None
+  * @date           : 2024/2/1
+  ******************************************************************************
+  */
+
+//
+// Created by xin on 2024/2/1.
+//
+#include "IRIS_Method.h"
+
+// 成功返回打包后的数据长度
+// -1: Error
+int32_t IRIS_Protocol_Pack(uint8_t Command, uint16_t LenthofIn, uint8_t *BufferIn, uint8_t *PackData) {
+    if (PackData == NULL || (LenthofIn != 0 && BufferIn == NULL)) {
+        return -1;
+    }
+
+    PackData[0] = 0x55;
+    PackData[1] = 0xAA;
+    PackData[2] = Command;
+    uint16_t datalenth = LenthofIn;
+    PackData[3] = (datalenth >> 8) & 0xFF;
+    PackData[4] = datalenth & 0xFF;
+
+    if (LenthofIn != 0) {
+        memcpy(&PackData[5], BufferIn, LenthofIn);
+    }
+    uint16_t crcbytelenth = LenthofIn;
+    uint16_t CRC = IRIS_calcCRC(PackData + 5, crcbytelenth);
+    PackData[LenthofIn + 5] = (CRC >> 8) & 0xFF;
+    PackData[LenthofIn + 6] = CRC & 0xFF;
+    return LenthofIn + 7;
+
+
+}
+// int32_t IRIS_STM32_Protocol_Unpack(uint8_t *PackData, uint16_t LenthofIn, uint8_t *Command, uint8_t *BufferOut) {
+//     if (PackData == NULL || BufferOut == NULL) {
+//         return ERROR_INPUT;
+//     }
+//     if (PackData[0] != 0x55 || PackData[1] != 0xAA) {
+//         return ERROR_HEADER;
+//     }
+
+//     uint16_t LenthofOut = PackData[4] + (PackData[3] << 8);
+    
+//     // 修改长度检查：确保有足够的数据包含头部、数据和校验
+//     if (LenthofIn < 5 + LenthofOut + 2) {
+//         return ERROR_NOT_ENOUGH_DATA;
+//     }
+
+//     // 修正校验标识位置：应该是 PackData[5 + LenthofOut] 和 PackData[5 + LenthofOut + 1]
+//     if (PackData[5 + LenthofOut] == 0xEE && PackData[5 + LenthofOut + 1] == 0xEE) {
+//         // 特殊校验标识，跳过CRC检查
+//     } else {
+//         uint16_t CRC = IRIS_calcCRC(PackData + 5, LenthofOut);
+//         if (CRC != (PackData[5 + LenthofOut + 1] + (PackData[5 + LenthofOut] << 8))) {
+//             return ERROR_CRC;
+//         }
+//     }
+
+//     if (LenthofOut == 0) {
+//         return 0;
+//     }
+//     *Command = PackData[2];
+
+//     memcpy(BufferOut, &PackData[5], LenthofOut);
+//     return LenthofOut;
+// }
+int32_t IRIS_STM32_Protocol_Unpack(uint8_t *PackData, uint16_t LenthofIn, uint8_t *Command, uint8_t *BufferOut) {
+
+    if (PackData == NULL || BufferOut == NULL) {
+        return ERROR_INPUT;
+    }
+    if (PackData[0] != 0x55 || PackData[1] != 0xAA) {
+        return ERROR_HEADER;
+    }
+
+    uint16_t LenthofOut = PackData[4] + (PackData[3] << 8); //减去CRC的两个字节
+    if (LenthofOut > LenthofIn - 7) {
+        return ERROR_NOT_ENOUGH_DATA;
+    }
+
+    if (PackData[LenthofOut + 6] == 0xEE && PackData[LenthofOut + 5] == 0xEE) {
+
+    } else {
+        uint16_t CRC = IRIS_calcCRC(PackData + 5, LenthofOut);
+        if (CRC != (PackData[LenthofOut + 6] + (PackData[LenthofOut + 5] << 8))) {
+            return ERROR_CRC;
+        }
+    }
+
+
+    if (LenthofOut == 0) {
+        return 0;
+    }
+    *Command = PackData[2];
+
+    memcpy(BufferOut, &PackData[5], LenthofOut);
+    return LenthofOut;
+}
+
+
+int32_t IRIS_Protocol_Unpack(uint8_t *PackData, uint16_t LenthofIn, uint8_t Command, uint8_t *BufferOut) {
+    if (PackData == NULL || BufferOut == NULL) {
+        return ERROR_INPUT;
+    }
+    if (PackData[0] != 0x55 || PackData[1] != 0xAA) {
+        return ERROR_HEADER;
+    }
+    if (PackData[2] != Command) {
+        return ERROR_COMMAND;
+    }
+    uint16_t LenthofOut = PackData[4] + (PackData[3] << 8);
+    if (LenthofOut > LenthofIn - 7) {
+        return ERROR_NOT_ENOUGH_DATA;
+    }
+    if (PackData[LenthofOut + 6] == 0xEE && PackData[LenthofOut + 5] == 0xEE) {
+
+    } else {
+        uint16_t CRC = IRIS_calcCRC(PackData + 5, LenthofOut);
+        if (CRC != (PackData[LenthofOut + 6] + (PackData[LenthofOut + 5] << 8))) {
+            return ERROR_CRC;
+        }
+    }
+    if (LenthofOut == 0) {
+        return 0;
+    }
+    memcpy(BufferOut, &PackData[5], LenthofOut);
+    return LenthofOut;
+
+}
+
+
+int32_t IRIS_Cut_Befor_Header(uint8_t *PackData, uint16_t LenthofIn) {
+    if (PackData == NULL) {
+        return ERROR_INPUT;
+    }
+    uint16_t i = 0;
+    for (i = 0; i < LenthofIn; i++) {
+        if (PackData[i] == 0x55 && PackData[i + 1] == 0xAA) {
+            break;
+        }
+    }
+    if (i == LenthofIn) {
+        //清空数据
+        memset(PackData, 0, LenthofIn);
+        return 0;
+    }
+
+    uint16_t LenthofOut = LenthofIn - i;
+    memcpy(PackData, &PackData[i], LenthofOut);
+    return LenthofOut;
+}
+
+int32_t IRIS_Check_Data_Valid(uint8_t *PackData, uint16_t LenthofIn) {
+    if (PackData == NULL) {
+        return ERROR_INPUT;
+    }
+    if (LenthofIn < 7) {
+        return ERROR_NOT_ENOUGH_DATA;
+        /* code */
+    }
+
+    if (PackData[0] != 0x55 || PackData[1] != 0xAA) {
+        return ERROR_HEADER;
+    }
+    uint16_t LenthofOut = PackData[4] + (PackData[3] << 8);
+    if (LenthofOut > LenthofIn - 7) {
+        return ERROR_NOT_ENOUGH_DATA;
+    }
+    if (PackData[LenthofOut + 6] == 0xEE && PackData[LenthofOut + 5] == 0xEE) {
+
+    } else {
+        uint16_t CRC = IRIS_calcCRC(PackData + 5, LenthofOut);
+        if (CRC != (PackData[LenthofOut + 6] + (PackData[LenthofOut + 5] << 8))) {
+            return ERROR_CRC;
+        }
+    }
+    return 1;
+
+}
+
+
+uint16_t IRIS_calcCRC(const void *pBuffer, uint16_t bufferSize) {
+    const uint8_t *pBytesArray = (const uint8_t *) pBuffer;
+    uint16_t poly = 0x8408;
+    uint16_t crc = 0;
+    uint8_t carry;
+    uint8_t i_bits;
+    uint16_t j;
+    for (j = 0; j < bufferSize; j++) {
+        crc = crc ^ pBytesArray[j];
+        for (i_bits = 0; i_bits < 8; i_bits++) {
+            carry = crc & 1;
+            crc = crc / 2;
+            if (carry) {
+                crc = crc ^ poly;
+            }
+        }
+    }
+    return crc;
+}
+

src/IRIS_Method.h

@@ -0,0 +1,66 @@
+/**
+  ******************************************************************************
+  * @file           : IRIS_Method.h
+  * @author         : xin
+  * @brief          : None
+  * @attention      : None
+  * @date           : 2024/2/1
+  ******************************************************************************
+  */
+
+//
+// Created by xin on 2024/2/1.
+//
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// #ifndef IRIS_COMMUNICATION_PROTOCOL_IRIS_METHOD_H
+// #define IRIS_COMMUNICATION_PROTOCOL_IRIS_METHOD_H
+
+
+#define ERROR_NOT_ENOUGH_DATA -200
+#define ERROR_HEADER -300
+#define ERROR_COMMAND -400
+#define ERROR_INPUT -500
+#define ERROR_CRC -600
+#include <stdint.h>
+#include <Arduino.h>
+
+// 成功返回打包后的数据长度
+// -1: Error
+int32_t IRIS_Protocol_Pack(uint8_t Command, uint16_t LenthofIn, uint8_t *BufferIn, uint8_t *PackData);
+
+// 解包函数 PackData 是接收到的数据 LenthofIn 是数据长度 Command 是命令 BufferOut 是输出
+// 下位机使用的打包函数 Command 是输出
+// 成功返回解包后的数据长度
+// 0: 该命令返回无参数
+// 错误返回ERRor
+// 成功返回解包后的数据长度
+int32_t IRIS_STM32_Protocol_Unpack(uint8_t *PackData, uint16_t LenthofIn, uint8_t *Command, uint8_t *BufferOut);
+
+// 解包函数 PackData 是接收到的数据 LenthofIn 是数据长度 Command 是命令输入 BufferOut 是输出 上位机使用
+// 成功返回解包后的数据长度
+// 0: 该命令返回无参数
+// 错误返回ERRor
+// 成功返回解包后的数据长度
+int32_t IRIS_Protocol_Unpack(uint8_t *PackData, uint16_t LenthofIn, uint8_t Command, uint8_t *BufferOut);
+
+// 定义裁切命令
+// 成功返回裁切后的数据长度
+// -1: Error
+int32_t IRIS_Cut_Befor_Header(uint8_t *PackData, uint16_t LenthofIn);
+
+// 检查数据是否有效
+// 有效返回值1
+// 错误返回ERRor
+int32_t IRIS_Check_Data_Valid(uint8_t *PackData, uint16_t LenthofIn);
+
+// 返回CRC校验值
+uint16_t IRIS_calcCRC(const void *pBuffer, uint16_t bufferSize);
+
+#ifdef __cplusplus
+}
+#endif
+

src/ble.cpp

@@ -0,0 +1,85 @@
+#include "ble.h"
+
+// 定义 BLE 服务和特征值的 UUID
+#define SERVICE_UUID        "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
+#define CHARACTERISTIC_UUID "beb5483e-36e1-4688-b7f5-ea07361b26a8"
+
+BLEServer* pServer = nullptr;
+BLECharacteristic* pCharacteristic = nullptr;
+BLEAdvertising* pAdvertising; 
+
+bool isClientConnected = false;
+
+class MyServerCallbacks : public BLEServerCallbacks {
+    void onConnect(BLEServer* pServer) {
+        Serial.println("CONNECT");
+        isClientConnected = true;
+    }
+
+    void onDisconnect(BLEServer* pServer) {
+        Serial.println("DISCONNECT");
+        isClientConnected = false;
+        // 重新启动广播，使设备再次可被发现
+        BLEDevice::startAdvertising();
+    }
+};
+
+bool bleSentData = false; // 记录 BLE 是否已发送数据
+
+class MyCharacteristicCallbacks : public BLECharacteristicCallbacks {
+    void onWrite(BLECharacteristic* pCharacteristic) {
+        std::string value = pCharacteristic->getValue();
+
+        if (value.length() > 0) {
+            bleSentData = true;
+            //Serial.print("Receive: ");
+            for (int i = 0; i < value.length(); i++) {
+                Serial.print(value[i]);
+            }
+            Serial.println();
+        }
+        //bleSentData = false;
+    }
+};
+
+
+
+void bleInit() {
+    // 初始化 BLE 设备
+    BLEDevice::init("VSMD_BLE"); // 设置设备名称
+
+    // 创建 BLE 服务器
+    pServer = BLEDevice::createServer();
+    pServer->setCallbacks(new MyServerCallbacks());
+
+    // 创建 BLE 服务
+    BLEService* pService = pServer->createService(SERVICE_UUID);
+
+    // 创建 BLE 特征值
+    pCharacteristic = pService->createCharacteristic(
+        CHARACTERISTIC_UUID,
+        BLECharacteristic::PROPERTY_READ |
+        BLECharacteristic::PROPERTY_WRITE
+    );
+
+    // 添加特征值描述
+    pCharacteristic->setCallbacks(new MyCharacteristicCallbacks());
+    //pCharacteristic->setValue("Hello Client");
+
+    // 启动服务
+    pService->start();
+
+    // 创建广告数据
+    BLEAdvertising* pAdvertising = BLEDevice::getAdvertising();
+    pAdvertising->addServiceUUID(BLEUUID(SERVICE_UUID));
+    pAdvertising->setScanResponse(true);
+    pAdvertising->setMinPreferred(0x06); 
+    BLEDevice::startAdvertising();
+
+    Serial.println("Waiting for client connection...");
+
+    // 在初始化时设置回调
+    pServer->setCallbacks(new MyServerCallbacks());
+}
+
+

src/ble.h

@@ -0,0 +1,14 @@
+#ifndef BLE_H
+#define BLE_H
+
+#include <Arduino.h>
+#include <BLEDevice.h>
+#include <BLEUtils.h>
+#include <BLEServer.h>
+#include <BLECharacteristic.h>
+
+
+// 蓝牙初始化函数
+void bleInit();
+
+#endif

src/main.cpp

@@ -0,0 +1,493 @@
+#include <ArduinoJson.h>
+#include "IRIS_Method.h"
+
+
+#define DEBUG_TX_PIN 17  // TX
+#define DEBUG_RX_PIN 16  // RX
+
+
+HardwareSerial DebugSerial(1);  // 使用UART1
+
+#define BUFFER_SIZE 1024
+#define JSON_BUFFER_SIZE 512
+
+// 全局变量声明
+uint8_t receiveBuffer[BUFFER_SIZE];     // 接收缓冲区
+uint8_t sendBuffer[BUFFER_SIZE];        // 发送缓冲区
+uint8_t responseBuffer[BUFFER_SIZE];    // 响应缓冲区
+uint8_t command;                        // 存储命令
+int32_t bytesProcessed;                 // 处理的数据字节数
+int32_t unpackResult;                   // 解包结果
+int32_t packedLength;                   // 打包后的数据长度
+
+// 设备状态变量
+bool deviceEnabled = false;
+int32_t currentPosition = 0;
+int32_t targetPosition = 0;
+int32_t pulsePerSecond = 1200;
+bool isMoving = false;
+bool isBackZero = false;
+
+// 设备配置参数
+struct DeviceConfig {
+    int zmd = 1;        // cfg zmd
+    int snr = 0;        // cfg snr
+    int osv = 0;        // cfg osv
+    int zsd = 1200;     // cfg zsd
+    int zsp = 2400;     // cfg zsp
+    int dmd = 2;        // cfg dmd
+    int dar = 5;        // cfg dar
+    int msr = 1;        // cfg msr
+    int msv = 1;        // cfg msv
+    int psr = 1;        // cfg psr
+    int psv = 1;        // cfg psv
+    int bdr = 115200;   // cfg bdr
+    int cid = 1;        // cfg cid
+    int mcs = 5;        // cfg mcs
+    int spd = 1200;     // cfg spd
+    int acc = 12000;    // cfg acc
+    int dec = 12000;    // cfg dec
+    float cra = 0.8;    // cfg cra
+    float crn = 0.4;    // cfg crn
+    float crh = 0.0;    // cfg crh
+} deviceConfig;
+
+
+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 handleEnableCommand();
+void handleOffCommand();
+void handleMoveCommand();
+void handlePositionCommand(int value);
+void handleRelativeMoveCommand(int value);
+void handlePulsePerSecondCommand(int value = -1);
+void handleOriginCommand();
+void handleStopCommand();
+void handleZeroCommand(const char* value);
+void printDeviceStatus();
+
+void setup() {
+    // 调试串口
+    DebugSerial.begin(115200, SERIAL_8N1, DEBUG_RX_PIN, DEBUG_TX_PIN);
+    
+    // 主串口（用于数据通信）
+    Serial.begin(115200);
+    
+    DebugSerial.println("VSMD INIT");
+    DebugSerial.println("Commands: cfg, ena, off, mov, pos, rmv, pps, org, stp, zero");
+    printDeviceStatus();
+}
+
+void loop() {
+    if (DebugSerial.available() > 0) {
+        int bytesRead = DebugSerial.readBytes(receiveBuffer, sizeof(receiveBuffer));
+        
+        bytesProcessed = IRIS_Cut_Befor_Header(receiveBuffer, bytesRead);
+        
+        if (bytesProcessed > 0) {
+            unpackResult = IRIS_STM32_Protocol_Unpack(receiveBuffer, bytesProcessed, &command, responseBuffer);
+            
+            if (unpackResult >= 0) {
+                // 根据命令类型处理数据
+                switch (command) {
+                    case 0x00: { // JSON数据
+                        char jsonString[JSON_BUFFER_SIZE];
+                        memcpy(jsonString, responseBuffer, unpackResult);
+                        jsonString[unpackResult] = '\0'; // 确保字符串以null结尾
+                        
+                        DebugSerial.print("Received JSON: ");
+                        DebugSerial.println(jsonString);
+                        
+                        processJsonCommand(jsonString);
+                        break;
+                    }
+                    
+                    case 0x01: { // 字符串数据
+                        char stringData[BUFFER_SIZE];
+                        memcpy(stringData, responseBuffer, unpackResult);
+                        stringData[unpackResult] = '\0';
+                        
+                        DebugSerial.print("Received String: ");
+                        DebugSerial.println(stringData);
+                        
+                        processStringCommand(stringData);
+                        break;
+                    }
+                    
+                    default: { // 自定义二进制数据 (0x02-0xff)
+                        DebugSerial.print("Received Binary Data, Command: 0x");
+                        DebugSerial.print(command, HEX);
+                        DebugSerial.print(", Length: ");
+                        DebugSerial.println(unpackResult);
+                        
+                        // 回显
+                        packedLength = IRIS_Protocol_Pack(command, unpackResult, responseBuffer, sendBuffer);
+                        DebugSerial.write(sendBuffer, packedLength);
+                        break;
+                    }
+                }
+            } else {
+                DebugSerial.print("Unpack failed, error code: ");
+                DebugSerial.println(unpackResult);
+            }
+        }
+    }
+}
+
+void processJsonCommand(const char* jsonString) {
+    StaticJsonDocument<JSON_BUFFER_SIZE> doc;
+    DeserializationError error = deserializeJson(doc, jsonString);
+    
+    if (error) {
+        DebugSerial.print("JSON parsing failed: ");
+        DebugSerial.println(error.c_str());
+        sendJsonResponse("error", "JSON_PARSE_ERROR");
+        return;
+    }
+    
+    const char* cmd = doc["cmd"];
+    if (cmd == nullptr) {
+        sendJsonResponse("error", "NO_CMD_FIELD");
+        return;
+    }
+    
+    // 根据cmd执行相应操作
+    if (strcmp(cmd, "cfg") == 0) {
+        JsonObject configObj = doc.as<JsonObject>();
+        handleConfigCommand(configObj);
+    } else if (strcmp(cmd, "ena") == 0) {
+        handleEnableCommand();
+    } else if (strcmp(cmd, "off") == 0) {
+        handleOffCommand();
+    } else if (strcmp(cmd, "mov") == 0) {
+        handleMoveCommand();
+    } else if (strcmp(cmd, "pos") == 0) {
+        int value = doc["value"] | 0;
+        handlePositionCommand(value);
+    } else if (strcmp(cmd, "rmv") == 0) {
+        int value = doc["value"] | 0;
+        handleRelativeMoveCommand(value);
+    } else if (strcmp(cmd, "pps") == 0) {
+        if (doc.containsKey("value")) {
+            int value = doc["value"];
+            handlePulsePerSecondCommand(value);
+        } else {
+            handlePulsePerSecondCommand();
+        }
+    } else if (strcmp(cmd, "org") == 0) {
+        handleOriginCommand();
+    } else if (strcmp(cmd, "stp") == 0) {
+        handleStopCommand();
+    } else if (strcmp(cmd, "zero") == 0) {
+        const char* value = doc["value"] | "";
+        handleZeroCommand(value);
+    } else {
+        sendJsonResponse(cmd, "UNKNOWN_COMMAND");
+    }
+}
+
+void processStringCommand(const char* stringData) {
+    // 处理字符串命令
+    String response = "Echo: " + String(stringData);
+    sendStringResponse(response.c_str());
+}
+
+void sendJsonResponse(const char* cmd, const char* status, int value) {
+    StaticJsonDocument<JSON_BUFFER_SIZE> responseDoc;
+    responseDoc["cmd"] = cmd;
+    responseDoc["status"] = status;
+    
+    if (value != -1) {
+        responseDoc["value"] = value;
+    }
+    
+    // 设备状态信息
+    responseDoc["enabled"] = deviceEnabled;
+    responseDoc["position"] = currentPosition;
+    responseDoc["moving"] = isMoving;
+    
+    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.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 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() {
+    deviceEnabled = false;
+    isMoving = false;
+    DebugSerial.println("Device disabled");
+    Serial.print("off\n");
+    sendJsonResponse("off", "OK");
+}
+
+void handleMoveCommand() {
+    if (!deviceEnabled) {
+        sendJsonResponse("mov", "DEVICE_DISABLED");
+        return;
+    }
+    
+    isMoving = true;
+    DebugSerial.println("Starting movement");
+    Serial.print("mov\n");
+    sendJsonResponse("mov", "OK");
+}
+
+void handlePositionCommand(int value) {
+    if (!deviceEnabled) {
+        sendJsonResponse("pos", "DEVICE_DISABLED");
+        return;
+    }
+    
+    targetPosition = value;
+    currentPosition = value; // 模拟立即到达
+    DebugSerial.print("Position set to: ");
+    DebugSerial.println(value);
+    sendJsonResponse("pos", "OK", value);
+}
+
+void handleRelativeMoveCommand(int value) {
+    if (!deviceEnabled) {
+        sendJsonResponse("rmv", "DEVICE_DISABLED");
+        return;
+    }
+    
+    currentPosition += value;
+    DebugSerial.print("Relative move by: ");
+    DebugSerial.print(value);
+    DebugSerial.print(", new position: ");
+    DebugSerial.println(currentPosition);
+    sendJsonResponse("rmv", "OK", currentPosition);
+}
+
+void handlePulsePerSecondCommand(int value) {
+    if (value == -1) {
+        // SET PPS
+        Serial.print("pps\n");
+        sendJsonResponse("pps", "OK", pulsePerSecond);
+    } else {
+        // 设置PPS值
+        pulsePerSecond = value;
+        DebugSerial.print("Pulse per second set to: ");
+        DebugSerial.println(value);
+        sendJsonResponse("pps", "OK", value);
+    }
+}
+
+void handleOriginCommand() {
+    if (!deviceEnabled) {
+        sendJsonResponse("org", "DEVICE_DISABLED");
+        return;
+    }
+    
+    currentPosition = 0;
+    targetPosition = 0;
+    DebugSerial.println("Returned to origin");
+    Serial.print("org\n");
+    sendJsonResponse("org", "OK", 0);
+}
+
+void handleStopCommand() {
+    isMoving = false;
+    DebugSerial.println("Movement stopped");
+    Serial.print("stp\n");
+    sendJsonResponse("stp", "OK");
+}
+
+void handleZeroCommand(const char* value) {
+    if (strcmp(value, "start") == 0) {
+        Serial.print("zero start\n");
+        isBackZero = true;
+        DebugSerial.println("Zero started");
+        sendJsonResponse("zero", "ZERO_STARTED");
+    } else if (strcmp(value, "stop") == 0) {
+        Serial.print("zero stop\n");
+        isBackZero = false;
+        currentPosition = 0;
+        DebugSerial.println("Zero completed");
+        sendJsonResponse("zero", "ZERO_COMPLETED");
+    } else {
+        sendJsonResponse("zero", "INVALID_VALUE");
+    }
+}
+
+void printDeviceStatus() {
+    DebugSerial.println("\n=== Device Status ===");
+    DebugSerial.print("Enabled: ");
+    DebugSerial.println(deviceEnabled ? "Yes" : "No");
+    DebugSerial.print("Current Position: ");
+    DebugSerial.println(currentPosition);
+    DebugSerial.print("Target Position: ");
+    DebugSerial.println(targetPosition);
+    DebugSerial.print("Pulse Per Second: ");
+    DebugSerial.println(pulsePerSecond);
+    DebugSerial.print("Moving: ");
+    DebugSerial.println(isMoving ? "Yes" : "No");
+    DebugSerial.print("Zero Backing: ");
+    DebugSerial.println(isBackZero ? "Yes" : "No");
+    DebugSerial.println("==================\n");
+}

src/vsmd_parser.cpp

@@ -0,0 +1,272 @@
+#include "vsmd_parser.h"
+#include "ble.h"
+
+extern HardwareSerial debugSerial;
+
+// BCC 校验函数
+uint8_t VSMDParser::bcc_checksum(uint8_t* data, int size) {
+    uint8_t sum = 0;
+    for (int index = 0; index < size; index++) {
+        sum ^= data[index];
+    }
+    return sum;
+}
+
+// 构造函数
+VSMDParser::VSMDParser() {
+    // 可选初始化
+}
+
+// 数据转换函数
+void VSMDParser::convert(uint8_t* data, value_info& info) {
+    info.udata = data[0];
+    info.udata <<= 7;
+    info.udata |= data[1];
+    info.udata <<= 7;
+    info.udata |= data[2];
+    info.udata <<= 7;
+    info.udata |= data[3];
+    info.udata <<= 7;
+    info.udata |= data[4];
+}
+
+// 解析函数
+// void VSMDParser::parse(uint8_t* data, int size) {
+//     // if (size < 10) {
+//     //     debugSerial.println("data too short");
+//     //     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
+//                     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
+//                     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
+//                     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
+//                     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("未知命令: ");
+//                     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();
+//     }
+// }
+
+/**************************** 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

@@ -0,0 +1,34 @@
+#ifndef VSMD_PARSER_H
+#define VSMD_PARSER_H
+
+#include <Arduino.h>
+#include "ble.h"
+
+
+// 联合体定义：用于数据转换
+typedef union {
+    float fdata;
+    int32_t idata;
+    uint32_t udata;
+} value_info;
+
+// 解析类声明
+class VSMDParser {
+public:
+    VSMDParser();
+
+    void parse(uint8_t* data, int size);
+
+private:
+    // BCC校验函数
+    static uint8_t bcc_checksum(uint8_t* data, int size);
+
+    // 将4字节数据转换为联合体中的值
+    void convert(uint8_t* data, value_info& info);
+};
+
+extern VSMDParser vsmdParser;
+
+extern BLECharacteristic* pCharacteristic;  // 外部 BLE 特征值指针声明
+
+#endif