chenxin/VSMD
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

加入vsmd test

Browse Source
This commit is contained in:
chenxin
2025-06-30 13:27:25 +08:00
parent 89a0366629
commit c9383ce175
3 changed files with 390 additions and 366 deletions
Download Patch File Download Diff File Expand all files Collapse all files

320
src/main.cpp
View File

@ -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 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 handle_cfg_cmd();
void handle_ena_cmd();
void handle_off_cmd();
void handle_mov_cmd();
void handle_pos_cmd(int value);
void handle_rmv_cmd(int value);
void handle_pps_cmd(int value = -1);
void handle_org_cmd();
void handle_stp_cmd();
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"];
if (cmd == nullptr) {
sendJsonResponse("error", "NO_CMD_FIELD");
// 使用 command 字段替代 cmd
const char* command = doc["command"];
if (command == nullptr) {
sendJsonResponse("error", "NO_COMMAND_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);
// 获取 PA 参数对象（可选）
JsonObject params = doc["PA"];
// 根据command执行相应操作
if (strcmp(command, "cfg") == 0) {
handle_cfg_cmd();
} else if (strcmp(command, "ena") == 0) {
handle_ena_cmd();
} else if (strcmp(command, "off") == 0) {
handle_off_cmd();
} else if (strcmp(command, "mov") == 0) {
handle_mov_cmd();
} else if (strcmp(command, "pos") == 0) {
// 修正：正确的类型转换
int value = params.containsKey("value") ? params["value"].as<int>() : 0;
handle_pos_cmd(value);
} else if (strcmp(command, "rmv") == 0) {
int value = params.containsKey("value") ? params["value"].as<int>() : 0;
handle_rmv_cmd(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) {
handleOriginCommand();
} else if (strcmp(cmd, "stp") == 0) {
handleStopCommand();
} else if (strcmp(cmd, "zero") == 0) {
const char* value = doc["value"] | "";
handleZeroCommand(value);
} else if (strcmp(command, "org") == 0) {
handle_org_cmd();
} else if (strcmp(command, "stp") == 0) {
handle_stp_cmd();
} else if (strcmp(command, "zero") == 0) {
// 修正：正确的字符串类型转换
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,31 +221,34 @@ 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["position"] = currentPosition;
responseDoc["moving"] = isMoving;
// 可选：添加其他信息
responseDoc["Other"] = "";
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.println();
DebugSerial.print("Sent JSON Response: ");
DebugSerial.println(jsonResponse);
// DebugSerial.print("Sent JSON Response: ");
// DebugSerial.println(jsonResponse);
}
void sendStringResponse(const char* response) {
@ -231,134 +260,45 @@ void sendStringResponse(const char* response) {
DebugSerial.println(response);
}
void handleConfigCommand(JsonObject& doc) {
DebugSerial.println("Processing configuration command...");
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");
// 创建响应JSON对象
// 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["cmd"] = "cfg";
responseDoc["status"] = "OK";
responseDoc["command"] = "dev";
responseDoc["ST"] = "OK";
responseDoc["Other"] = deviceInfo;
// 添加设备状态信息
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));
@ -366,18 +306,20 @@ void handleConfigCommand(JsonObject& doc) {
packedLength = IRIS_Protocol_Pack(0x00, strlen(jsonResponse), (uint8_t*)jsonResponse, sendBuffer);
DebugSerial.write(sendBuffer, packedLength);
DebugSerial.print("Sent Config Response: ");
DebugSerial.println();
DebugSerial.print("Sent JSON Response: ");
DebugSerial.println(jsonResponse);
}
void handleEnableCommand() {
void handle_ena_cmd() {
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;

432
src/vsmd_parser.cpp
View File

@ -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];
}
// 解析函数
// void VSMDParser::parse(uint8_t* data, int size) {
// // if (size < 10) {
// // debugSerial.println("data too short");
// // return;
// // }
// VSMD 解析函数
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 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;
// }
@ -71,202 +259,96 @@ void VSMDParser::convert(uint8_t* data, value_info& info) {
// 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);
// 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
// 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("未知命令: ");
// debugSerial.println(data[2]);
// String response = "未知命令: ";
// response += data[2];
// response += "\n";
// // 发送通过蓝牙
// if (pCharacteristic) {
// pCharacteristic->setValue(response.c_str());
// pCharacteristic->notify();
// }
// break;
// }
// } else {
// return;
// debugSerial.println("BCC ERROR");
// debugSerial.print("CHECK BCC: ");
// debugSerial.println(checksum);
// debugSerial.print("RES BCC: ");
// debugSerial.println(data[size - 2], HEX);
// // 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 {
// debugSerial.println("Head or Tail ERROR");
// debugSerial.print("Receive Data: ");
// String response = "Head or Tail ERROR\nReceive Data: ";
// for (int i = 0; i < size; i++) {
// debugSerial.print(data[i], HEX);
// debugSerial.print(" ");
// response += data[i], HEX;
// 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();
}
}
}

4
src/vsmd_parser.h
View File

@ -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校验函数