package yaokong;
|
|
import java.nio.ByteBuffer;
|
import java.nio.ByteOrder;
|
|
public class Control80 {
|
|
/**
|
* 解析应答指令（指令类型0x80）
|
*
|
* @param responseHex 接收到的应答指令HEX字符串
|
* @return 解析后的应答信息对象
|
*/
|
public static AckResponse parseAckResponseHex(String responseHex) {
|
// 将HEX字符串转换为字节数组
|
byte[] responseBytes = hexStringToByteArray(responseHex);
|
return parseAckResponseBytes(responseBytes);
|
}
|
|
/**
|
* 解析应答指令字节数组
|
*/
|
public static AckResponse parseAckResponseBytes(byte[] responseBytes) {
|
if (responseBytes == null || responseBytes.length < 12) {
|
throw new IllegalArgumentException("应答数据长度不足");
|
}
|
|
ByteBuffer buffer = ByteBuffer.wrap(responseBytes).order(ByteOrder.LITTLE_ENDIAN);
|
|
// 检查帧头
|
byte header1 = buffer.get();
|
byte header2 = buffer.get();
|
if (header1 != BluetoothProtocol.FRAME_HEADER[0] || header2 != BluetoothProtocol.FRAME_HEADER[1]) {
|
throw new IllegalArgumentException("无效的帧头");
|
}
|
|
// 检查指令类型
|
byte commandType = buffer.get();
|
if (commandType != (byte) 0x80) {
|
throw new IllegalArgumentException("非应答指令");
|
}
|
|
// 数据长度
|
short dataLength = buffer.getShort();
|
if (dataLength != 5) {
|
throw new IllegalArgumentException("应答数据长度应为5，实际为" + dataLength);
|
}
|
|
// 序列号
|
short sequence = buffer.getShort();
|
|
// 原指令类型
|
byte originalCommand = buffer.get();
|
|
// 执行结果
|
byte result = buffer.get();
|
|
// 错误代码
|
byte errorCode = buffer.get();
|
|
// CRC16
|
short receivedCRC = buffer.getShort();
|
|
// 帧尾
|
byte footer = buffer.get();
|
if (footer != 0x0D) {
|
throw new IllegalArgumentException("无效的帧尾");
|
}
|
|
// 验证CRC
|
byte[] dataForCRC = new byte[1 + 2 + 2 + dataLength];
|
System.arraycopy(responseBytes, 2, dataForCRC, 0, dataForCRC.length);
|
int calculatedCRC = CRC16.calculateCRC16(dataForCRC, 0, dataForCRC.length);
|
|
boolean crcValid = (calculatedCRC == (receivedCRC & 0xFFFF));
|
|
return new AckResponse(sequence, originalCommand, result, errorCode, crcValid);
|
}
|
|
/**
|
* 应答信息类
|
*/
|
public static class AckResponse {
|
private short sequence; // 序列号
|
private byte originalCommand; // 原指令类型
|
private byte result; // 执行结果
|
private byte errorCode; // 错误代码
|
private boolean crcValid; // CRC校验是否有效
|
|
public AckResponse(short sequence, byte originalCommand, byte result,
|
byte errorCode, boolean crcValid) {
|
this.sequence = sequence;
|
this.originalCommand = originalCommand;
|
this.result = result;
|
this.errorCode = errorCode;
|
this.crcValid = crcValid;
|
}
|
|
public short getSequence() {
|
return sequence;
|
}
|
|
public byte getOriginalCommand() {
|
return originalCommand;
|
}
|
|
public byte getResult() {
|
return result;
|
}
|
|
public byte getErrorCode() {
|
return errorCode;
|
}
|
|
public boolean isCrcValid() {
|
return crcValid;
|
}
|
|
public boolean isSuccess() {
|
return result == 0x00;
|
}
|
|
public String getOriginalCommandName() {
|
switch (originalCommand & 0xFF) {
|
case 0x01: return "路径坐标指令";
|
case 0x02: return "基准站坐标指令";
|
case 0x03: return "控制转向指令";
|
case 0x04: return "控制指令";
|
case 0x05: return "路径分片指令";
|
default: return "未知指令(0x" + String.format("%02X", originalCommand) + ")";
|
}
|
}
|
|
public String getResultDescription() {
|
switch (result) {
|
case 0x00: return "成功";
|
case 0x01: return "失败";
|
case 0x02: return "忙";
|
default: return "未知(" + result + ")";
|
}
|
}
|
|
public String getErrorDescription() {
|
switch (errorCode) {
|
case 0x00: return "SUCCESS";
|
case 0x01: return "CRC_ERROR";
|
case 0x02: return "DATA_LENGTH_ERROR";
|
case 0x03: return "MEMORY_FULL";
|
case 0x04: return "INVALID_COORDINATE";
|
case 0x05: return "SYSTEM_BUSY";
|
case 0x06: return "EMERGENCY_STOP";
|
default: return "未知错误码(0x" + String.format("%02X", errorCode) + ")";
|
}
|
}
|
|
@Override
|
public String toString() {
|
return String.format("应答信息: [序列号=%d, 原指令=%s, 执行结果=%s, 错误码=%s, CRC校验=%s]",
|
sequence & 0xFFFF,
|
getOriginalCommandName(),
|
getResultDescription(),
|
getErrorDescription(),
|
crcValid ? "有效" : "无效");
|
}
|
}
|
|
/**
|
* 模拟接收到的应答指令用于测试
|
*/
|
public static String createMockResponse(short sequence, byte originalCommand,
|
byte result, byte errorCode) {
|
int dataLength = 5; // 固定长度
|
|
ByteBuffer buffer = ByteBuffer.allocate(2 + 1 + 2 + 2 + dataLength + 2 + 1);
|
buffer.order(ByteOrder.LITTLE_ENDIAN);
|
|
// 帧头
|
buffer.put(BluetoothProtocol.FRAME_HEADER);
|
|
// 指令类型
|
buffer.put((byte) 0x80);
|
|
// 数据长度
|
buffer.putShort((short) dataLength);
|
|
// 序列号
|
buffer.putShort(sequence);
|
|
// 原指令类型
|
buffer.put(originalCommand);
|
|
// 执行结果
|
buffer.put(result);
|
|
// 错误代码
|
buffer.put(errorCode);
|
|
// 计算CRC16
|
byte[] dataForCRC = new byte[1 + 2 + 2 + dataLength];
|
System.arraycopy(buffer.array(), 2, dataForCRC, 0, dataForCRC.length);
|
int crc = CRC16.calculateCRC16(dataForCRC, 0, dataForCRC.length);
|
buffer.putShort((short) crc);
|
|
// 帧尾
|
buffer.put((byte) 0x0D);
|
|
return bytesToHex(buffer.array());
|
}
|
|
/**
|
* HEX字符串转换为字节数组
|
*/
|
private static byte[] hexStringToByteArray(String hexString) {
|
if (hexString == null || hexString.trim().isEmpty()) {
|
throw new IllegalArgumentException("HEX字符串不能为空");
|
}
|
|
// 移除空格
|
String cleanHex = hexString.replaceAll("\\s+", "");
|
|
if (cleanHex.length() % 2 != 0) {
|
throw new IllegalArgumentException("HEX字符串长度必须是偶数");
|
}
|
|
byte[] data = new byte[cleanHex.length() / 2];
|
for (int i = 0; i < cleanHex.length(); i += 2) {
|
String hex = cleanHex.substring(i, i + 2);
|
data[i / 2] = (byte) Integer.parseInt(hex, 16);
|
}
|
|
return data;
|
}
|
|
private static String bytesToHex(byte[] bytes) {
|
StringBuilder hexString = new StringBuilder();
|
for (int i = 0; i < bytes.length; i++) {
|
String hex = Integer.toHexString(bytes[i] & 0xFF);
|
if (hex.length() == 1) {
|
hexString.append('0');
|
}
|
hexString.append(hex);
|
if (i < bytes.length - 1) {
|
hexString.append(' ');
|
}
|
}
|
return hexString.toString().toUpperCase();
|
}
|
|
}
|
