diff --git a/casic-ship/src/main/java/com/casic/missiles/modular/system/constants/CheckCode.java b/casic-ship/src/main/java/com/casic/missiles/modular/system/constants/CheckCode.java
new file mode 100644
index 0000000..b39b29c
--- /dev/null
+++ b/casic-ship/src/main/java/com/casic/missiles/modular/system/constants/CheckCode.java
@@ -0,0 +1,50 @@
+package com.casic.missiles.modular.system.constants;
+
+/**
+ * CRC16校验码
+ *
+ * @author a203
+ */
+public class CheckCode {
+ public static int[] CRC_HIGH = {
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
+ 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+ 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
+ 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81,
+ 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
+ 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
+ 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
+ 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+ 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
+ 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
+ 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
+ 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+ 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
+ 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
+ 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
+ 0x40
+ };
+
+ public static int[] CRC_LOW = {
+ 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4,
+ 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
+ 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD,
+ 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
+ 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7,
+ 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
+ 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE,
+ 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
+ 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2,
+ 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
+ 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB,
+ 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
+ 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91,
+ 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
+ 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88,
+ 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
+ 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80,
+ 0x40
+ };
+}
diff --git a/casic-ship/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java b/casic-ship/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java
index f5b674c..2e787a7 100644
--- a/casic-ship/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java
+++ b/casic-ship/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java
@@ -1,18 +1,14 @@
package com.casic.missiles.modular.system.utils;
+import com.alibaba.fastjson.JSON;
+import com.casic.missiles.modular.system.constants.CheckCode;
import com.casic.missiles.modular.system.model.RobotInfo;
+import com.casic.missiles.modular.system.model.RobotRegion;
+
+import java.util.*;
/**
* 西工大数据解析协议
- *
- * 帧头（1byte） 0x2A
- * 帧长度（1byte） 0x23
- * 帧类型（1byte） 0x01
- * 子类型（1byte） 0x00
- * 机器人ID（1byte） 0x03
- * 帧数据 4D BD 80 CF 0C 5E 28 C2 01 EB EF 5F 5D 13 5D 40 3D 30 B2 25 4E 1B 4E 00 00 9C 42 BD 07 D0 C7
- * CRC16校验码（2byte） 0xC7 0x16
- * 帧尾（1byte） 0x0A
*
* @author a203
*/
@@ -21,12 +17,19 @@
private static final int BITS_OF_HEAD = 0x2A;
private static final int BITS_OF_END = 0x0A;
- public static RobotInfo translate(int[] bytes) {
- RobotInfo robot = new RobotInfo();
+ private static boolean isCorrectData(int[] bytes) {
if (bytes[0] != BITS_OF_HEAD) {
- return robot;
+ return false;
}
- if (bytes[bytes.length - 1] != BITS_OF_END) {
+ return bytes[bytes.length - 1] == BITS_OF_END;
+ }
+
+ /**
+ * 水下机器人信息
+ */
+ public static RobotInfo parseRobotInfo(int[] bytes) {
+ RobotInfo robot = new RobotInfo();
+ if (!isCorrectData(bytes)) {
return robot;
}
robot.setRobotId(bytes[4]);
@@ -89,6 +92,273 @@
}
/**
+ * 区域覆盖
+ */
+ public static RobotRegion parseRobotRegion(int[] bytes) {
+ RobotRegion region = new RobotRegion();
+ if (!isCorrectData(bytes)) {
+ return region;
+ }
+ int count = bytes[4];
+ region.setRobotCount(count);
+
+ int[] idsBytes = new int[count];
+ System.arraycopy(bytes, 5, idsBytes, 0, count);
+ StringBuilder builder = new StringBuilder();
+ for (int i = 0; i < idsBytes.length; i++) {
+ if (i != idsBytes.length - 1) {
+ builder.append(idsBytes[i]).append(",");
+ } else {
+ builder.append(idsBytes[i]);
+ }
+ }
+ region.setRobotIds(builder.toString());
+
+ //经度和纬度，所以需要*2
+ int targetSize = count * 2 * 4;
+ int[] targetBytes = new int[targetSize];
+ //起始4个字节，机器人个数1个字节
+ System.arraycopy(bytes, 5 + idsBytes.length, targetBytes, 0, targetSize);
+ region.setRobotTargets(formatLanLat(targetBytes));
+
+ region.setPointCount(bytes[5 + idsBytes.length + targetBytes.length]);
+
+ int[] radiusBytes = new int[2];
+ System.arraycopy(bytes, 5 + idsBytes.length + targetBytes.length + 1, radiusBytes, 0, 2);
+ region.setRegionRadius(covertTargetDistance(radiusBytes));
+
+ int aByte = bytes[5 + idsBytes.length + targetBytes.length + 1 + radiusBytes.length];
+ if (aByte == 0) {
+ region.setContour("轮廓标志位:多边形，" + count + "个轮廓点");
+ } else {
+ region.setContour("轮廓标志位:圆形，" + count + "个轮廓点");
+ }
+
+ //经度和纬度，所以需要*2
+ int pointSize = count * 2 * 4;
+ int[] pointBytes = new int[pointSize];
+ System.arraycopy(bytes,
+ 5 + idsBytes.length + targetBytes.length + 1 + radiusBytes.length + 1,
+ pointBytes, 0, pointSize);
+ region.setRobotPoints(formatLanLat(pointBytes));
+ return region;
+ }
+
+ /**
+ * 路径规划
+ */
+ public static String parseRobotRoute(int[] bytes) {
+ if (!isCorrectData(bytes)) {
+ return "";
+ }
+ Map routeMap = new HashMap<>(2);
+
+ int count = bytes[4];
+ routeMap.put("routeCount", String.valueOf(count));
+
+ int routeSize = count * 2 * 4;
+ int[] routeBytes = new int[routeSize];
+ System.arraycopy(bytes, 5, routeBytes, 0, routeSize);
+ routeMap.put("robotRoutes", formatLanLat(routeBytes));
+ return JSON.toJSONString(routeMap);
+ }
+
+ /**
+ * 定向任务
+ */
+ public static String parseDirectedTask(int[] bytes) {
+ if (!isCorrectData(bytes)) {
+ return "";
+ }
+ Map taskMap = new HashMap<>(2);
+ int[] depthBytes = new int[2];
+ System.arraycopy(bytes, 4, depthBytes, 0, 2);
+ taskMap.put("taskDepth", covertDepth(depthBytes) + "m");
+
+ int[] angleBytes = new int[2];
+ System.arraycopy(bytes, 6, angleBytes, 0, 2);
+ taskMap.put("headingAngle", covertHeadingAngle(angleBytes) + "°");
+ return JSON.toJSONString(taskMap);
+ }
+
+ /**
+ * 航路点任务
+ */
+ public static String parseTaskRoute(int[] bytes) {
+ if (!isCorrectData(bytes)) {
+ return "";
+ }
+ Map taskMap = new HashMap<>(3);
+
+ int[] depthBytes = new int[2];
+ System.arraycopy(bytes, 4, depthBytes, 0, 2);
+ taskMap.put("taskDepth", covertDepth(depthBytes) + "m");
+
+ int count = bytes[6];
+ taskMap.put("routeCount", String.valueOf(count));
+
+ int routeSize = count * 2 * 4;
+ int[] routeBytes = new int[routeSize];
+ System.arraycopy(bytes, 7, routeBytes, 0, routeSize);
+ taskMap.put("robotRoutes", formatLanLat(routeBytes));
+ return JSON.toJSONString(taskMap);
+ }
+
+ /**
+ * 定深直航任务
+ */
+ public static String parseDirectFlightTask(int[] bytes) {
+ if (!isCorrectData(bytes)) {
+ return "";
+ }
+ Map taskMap = new HashMap<>(3);
+ int[] depthBytes = new int[2];
+ System.arraycopy(bytes, 4, depthBytes, 0, 2);
+ taskMap.put("taskDepth", covertDepth(depthBytes) + "m");
+
+ int[] angleBytes = new int[2];
+ System.arraycopy(bytes, 6, angleBytes, 0, 2);
+ taskMap.put("headingAngle", covertHeadingAngle(angleBytes) + "°");
+
+ int[] timesBytes = new int[2];
+ System.arraycopy(bytes, 8, timesBytes, 0, 2);
+ taskMap.put("time", covertToDec(timesBytes) + "s");
+ return JSON.toJSONString(taskMap);
+ }
+
+ /**
+ * 驻留任务
+ */
+ public static String parseResideTask(int[] bytes) {
+ if (!isCorrectData(bytes)) {
+ return "";
+ }
+ Map taskMap = new HashMap<>(4);
+ int[] lngBytes = new int[4];
+ System.arraycopy(bytes, 4, lngBytes, 0, 4);
+ taskMap.put("lng", String.valueOf(covertLngLat(lngBytes)));
+
+ int[] latBytes = new int[4];
+ System.arraycopy(bytes, 8, latBytes, 0, 4);
+ taskMap.put("lat", String.valueOf(covertLngLat(latBytes)));
+
+ int[] angleBytes = new int[2];
+ System.arraycopy(bytes, 12, angleBytes, 0, 2);
+ taskMap.put("depth", covertDepth(angleBytes) + "m");
+
+ int[] timesBytes = new int[2];
+ System.arraycopy(bytes, 14, timesBytes, 0, 2);
+ taskMap.put("time", covertToDec(timesBytes) + "min");
+ return JSON.toJSONString(taskMap);
+ }
+
+ /**
+ * 投放任务
+ */
+ public static String parsePutTask(int[] bytes) {
+ if (!isCorrectData(bytes)) {
+ return "";
+ }
+ Map taskMap = new HashMap<>(4);
+ int[] lngBytes = new int[4];
+ System.arraycopy(bytes, 4, lngBytes, 0, 4);
+ taskMap.put("lng", String.valueOf(covertLngLat(lngBytes)));
+
+ int[] latBytes = new int[4];
+ System.arraycopy(bytes, 8, latBytes, 0, 4);
+ taskMap.put("lat", String.valueOf(covertLngLat(latBytes)));
+
+ int count = bytes[12];
+ taskMap.put("count", String.valueOf(count));
+
+ int[] idsBytes = new int[count];
+ System.arraycopy(bytes, 13, idsBytes, 0, count);
+ StringBuilder builder = new StringBuilder();
+ for (int i = 0; i < idsBytes.length; i++) {
+ if (i != idsBytes.length - 1) {
+ builder.append(idsBytes[i]).append(",");
+ } else {
+ builder.append(idsBytes[i]);
+ }
+ }
+ taskMap.put("ids", builder.toString());
+ return JSON.toJSONString(taskMap);
+ }
+
+ /**
+ * AIS数据请求
+ */
+ public static String parseAISRequest(int[] bytes) {
+ if (!isCorrectData(bytes)) {
+ return "";
+ }
+ Map taskMap = new HashMap<>(2);
+ int[] depthBytes = new int[2];
+ System.arraycopy(bytes, 4, depthBytes, 0, 2);
+ taskMap.put("radius", covertToDec(depthBytes) + "km");
+
+ taskMap.put("shipCount", String.valueOf(bytes[6]));
+ return JSON.toJSONString(taskMap);
+ }
+
+ /**
+ * 方舱位置
+ */
+ public static String parseShelterPosition(int[] bytes) {
+ if (!isCorrectData(bytes)) {
+ return "";
+ }
+ Map taskMap = new HashMap<>(2);
+ int[] lngBytes = new int[4];
+ System.arraycopy(bytes, 4, lngBytes, 0, 4);
+ taskMap.put("lng", String.valueOf(covertLngLat(lngBytes)));
+
+ int[] latBytes = new int[4];
+ System.arraycopy(bytes, 8, latBytes, 0, 4);
+ taskMap.put("lat", String.valueOf(covertLngLat(latBytes)));
+ return JSON.toJSONString(taskMap);
+ }
+
+ public static String formatLanLat(int[] targetBytes) {
+ List lnglat = new ArrayList<>();
+ for (int i = 0; i < targetBytes.length; i += 4) {
+ int[] target = new int[4];
+ System.arraycopy(targetBytes, i, target, 0, 4);
+
+ //每4个字节转换一次经纬度
+ lnglat.add(covertLngLat(target));
+ }
+ List> pointList = new ArrayList<>();
+ for (int i = 0; i < lnglat.size(); i += 2) {
+ Map lnglatMap = new HashMap<>(2);
+ for (int j = 0; j < 2; j++) {
+ lnglatMap.put("lng", lnglat.get(i));
+ lnglatMap.put("lat", lnglat.get(i + 1));
+ }
+ //每2个元素作为一个对象
+ pointList.add(lnglatMap);
+ }
+ return JSON.toJSONString(pointList);
+ }
+
+ /**
+ * 十进制转换算法
+ */
+ private static double covertToDec(int[] bytes) {
+ return ((bytes[0] & 0xFF) << 8) +
+ (bytes[1] & 0xFF);
+ }
+
+ /**
+ * 任务深度转换算法
+ */
+ private static double covertDepth(int[] bytes) {
+ float a = ((bytes[0] & 0xFF) << 8) +
+ (bytes[1] & 0xFF);
+ return (a * 0.1);
+ }
+
+ /**
* 目标频率转换算法
*/
private static String covertTargetHertz(int[] bytes) {
@@ -177,4 +447,268 @@
) / (Math.pow(2, 31) - 1);
return 180 * per;
}
-}
+
+ /*****数据转换为16进制*********************************************/
+
+ /**
+ * 环境数据
+ */
+ public static String parseEnvironmentalData(String data) {
+ //包含除校验位之前的所有数据
+ List dataLinkedList = createDataHead("01");
+
+ /**
+ * 组合帧数据
+ * */
+ //台风中心经度
+ double lng = 109.322222;
+ int[] lngBytes = lnglatToHex(lng);
+ for (int lngByte : lngBytes) {
+ dataLinkedList.add(hex(lngByte));
+ }
+
+ //台风中心纬度
+ double lat = 17.392232;
+ int[] latBytes = lnglatToHex(lat);
+ for (int latByte : latBytes) {
+ dataLinkedList.add(hex(latByte));
+ }
+
+ //台风风力
+ int power = 14;
+ dataLinkedList.add(hex(power));
+
+ //台风半径
+ double radius = 155.56;
+ int[] radiusBytes = radiusToHex(radius);
+ for (int radiusByte : radiusBytes) {
+ dataLinkedList.add(hex(radiusByte));
+ }
+
+ //机器人与台风中心距离
+ double distance = 355.56;
+ int[] distanceBytes = radiusToHex(distance);
+ for (int distanceByte : distanceBytes) {
+ dataLinkedList.add(hex(distanceByte));
+ }
+
+ //机器人与台风的方位角
+ double angle = 170.85;
+ int[] angleBytes = angleToHex(angle);
+ for (int angleByte : angleBytes) {
+ dataLinkedList.add(hex(angleByte));
+ }
+
+ //洋流大小
+ double oceanValue = 1.85;
+ dataLinkedList.add(hex((int) (oceanValue * 100)));
+
+ //洋流方向
+ double oceanAngle = 170.85;
+ int[] oceanAngleBytes = angleToHex(oceanAngle);
+ for (int oceanAngleByte : oceanAngleBytes) {
+ dataLinkedList.add(hex(oceanAngleByte));
+ }
+
+ //风向
+ double windDirection = 170.85;
+ int[] windDirectionBytes = angleToHex(windDirection);
+ for (int windDirectionByte : windDirectionBytes) {
+ dataLinkedList.add(hex(windDirectionByte));
+ }
+
+ //风速
+ double windSpeed = 170.85;
+ int[] windSpeedBytes = radiusToHex(windSpeed);
+ for (int windSpeedByte : windSpeedBytes) {
+ dataLinkedList.add(hex(windSpeedByte));
+ }
+
+ //浪高
+ double waveHigh = 12.5;
+ dataLinkedList.add(hex((int) (waveHigh * 10)));
+
+ //温度，TODO 可能需要调整
+ double temperature = 5.5;
+ dataLinkedList.add(hex((int) (temperature * 10)));
+
+ //水深
+ double depth = 2125.8;
+ int[] depthBytes = depthToHex(depth);
+ for (int depthByte : depthBytes) {
+ dataLinkedList.add(hex(depthByte));
+ }
+
+ //重新设置数据长度，不包括CRC16数据长度
+ int length = dataLinkedList.size();
+ dataLinkedList.set(1, hex(length));
+
+ //计算CRC16校验码
+ int crc = CRC16(dataLinkedList);
+ int[] crcBytes = crc16ToHex(crc);
+ for (int crcByte : crcBytes) {
+ dataLinkedList.add(hex(crcByte));
+ }
+
+ //帧尾
+ dataLinkedList.add("OA");
+ return formatData(dataLinkedList);
+ }
+
+ /**
+ * 环境数据
+ */
+ public static String parseAISData(String data) {
+ List dataLinkedList = createDataHead("02");
+
+ //船只数量
+ int shipCount = 2;
+ dataLinkedList.add(hex(shipCount));
+
+ for (int i = 0; i < shipCount; i++) {
+ //船只经度
+ double lng = 109.322222;
+ int[] lngBytes = lnglatToHex(lng);
+ for (int lngByte : lngBytes) {
+ dataLinkedList.add(hex(lngByte));
+ }
+
+ //船只纬度
+ double lat = 17.392232;
+ int[] latBytes = lnglatToHex(lat);
+ for (int latByte : latBytes) {
+ dataLinkedList.add(hex(latByte));
+ }
+ }
+
+ //重新设置数据长度，不包括CRC16数据长度
+ int length = dataLinkedList.size();
+ dataLinkedList.set(1, hex(length));
+
+ //计算CRC16校验码
+ int crc = CRC16(dataLinkedList);
+ int[] crcBytes = crc16ToHex(crc);
+ for (int crcByte : crcBytes) {
+ dataLinkedList.add(hex(crcByte));
+ }
+
+ //帧尾
+ dataLinkedList.add("OA");
+ return formatData(dataLinkedList);
+ }
+
+ /**
+ * 创建帧数据头
+ */
+ private static List createDataHead(String type) {
+ List head = new LinkedList<>();
+ head.add("2B");
+ //先设定数据长度为256
+ head.add("FF");
+ head.add(type);
+ //TODO 可能是缺省项
+ head.add("00");
+ //机器人ID固定是0x00
+ head.add("00");
+ return head;
+ }
+
+ /**
+ * 格式化帧数据为String
+ */
+ private static String formatData(List value) {
+ StringBuilder builder = new StringBuilder();
+ for (int i = 0; i < value.size(); i++) {
+ if (i != value.size() - 1) {
+ builder.append(value.get(i)).append(" ");
+ } else {
+ builder.append(value.get(i));
+ }
+ }
+ return builder.toString();
+ }
+
+ /**
+ * 计算CRC16校验码
+ */
+ private static int CRC16(List value) {
+ int crcHigh = 0xFF;
+ int crcLow = 0xFF;
+ int uIndex;
+ for (String s : value) {
+ uIndex = (crcLow ^ Integer.parseInt(s, 16));
+ crcLow = crcHigh ^ CheckCode.CRC_HIGH[uIndex];
+ crcHigh = CheckCode.CRC_LOW[uIndex];
+ }
+ return (crcHigh << 8 | crcLow);
+ }
+
+ /**
+ * 首位补零并转换为大写
+ */
+ private static String hex(int value) {
+ String hexString;
+ if (value <= 15) {
+ hexString = "0" + Integer.toHexString(value);
+ } else {
+ hexString = Integer.toHexString(value);
+ }
+ return hexString.toUpperCase(Locale.ROOT);
+ }
+
+ /**
+ * CRC编码值转16进制数组
+ */
+ private static int[] crc16ToHex(int value) {
+ int[] tempBytes = new int[2];
+ tempBytes[0] = value / 256;
+ tempBytes[1] = value % 256;
+ return tempBytes;
+ }
+
+ /**
+ * 深度转16进制数组
+ */
+ private static int[] depthToHex(double depth) {
+ int[] tempBytes = new int[2];
+ int value = (int) (depth * 10);
+ tempBytes[0] = value / 256;
+ tempBytes[1] = value % 256;
+ return tempBytes;
+ }
+
+ /**
+ * 角度转16进制数组
+ */
+ private static int[] angleToHex(double angle) {
+ int[] tempBytes = new int[2];
+ int value = (int) (angle * 100);
+ tempBytes[0] = value / 256;
+ tempBytes[1] = value % 256;
+ return tempBytes;
+ }
+
+ /**
+ * 距离半径转16进制数组
+ */
+ private static int[] radiusToHex(double radius) {
+ int[] tempBytes = new int[2];
+ int value = (int) (radius * 100);
+ tempBytes[0] = value / 256;
+ tempBytes[1] = value % 256;
+ return tempBytes;
+ }
+
+ /**
+ * 经纬度转16进制数组
+ */
+ private static int[] lnglatToHex(double lnglat) {
+ int per = (int) (lnglat / 180.0 * (Math.pow(2, 31) - 1));
+ int[] tempBytes = new int[4];
+ tempBytes[0] = (per >> 24) & 0xFF;
+ tempBytes[1] = (per >> 16) & 0xFF;
+ tempBytes[2] = (per >> 8) & 0xFF;
+ tempBytes[3] = (per) & 0xFF;
+ return tempBytes;
+ }
+}
\ No newline at end of file