diff --git a/casic-shelter/pom.xml b/casic-shelter/pom.xml index c1dc630..3c096f1 100644 --- a/casic-shelter/pom.xml +++ b/casic-shelter/pom.xml @@ -57,6 +57,10 @@ spatial4j 0.8 + + io.netty + netty-all + diff --git a/casic-shelter/pom.xml b/casic-shelter/pom.xml index c1dc630..3c096f1 100644 --- a/casic-shelter/pom.xml +++ b/casic-shelter/pom.xml @@ -57,6 +57,10 @@ spatial4j 0.8 + + io.netty + netty-all + diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java index 4af70db..ea31852 100644 --- a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java @@ -6,31 +6,9 @@ * @author a203 */ public class Constant { - public static final int[] DATA = new int[]{ - 0x2A,//帧头 - 0x23,//帧长度 - 0x01,//帧类型 - 0x00,//子类型 - 0x03,//机器人ID - 0x4D, 0xBD, 0x80, 0xCF,//位置经度 - 0x0C, 0x5E, 0x28, 0xC2,//位置纬度 - 0x01, //航行状态 - 0xEB, //仪表电压 - 0xEF, //动力电压 - 0x5F, //仪表电余量 - 0x5D, //动力电余量 - 0x13, 0x5D, //俯仰角 - 0x40, 0x3D, //横滚角 - 0x30, 0xB2, //航向角 - 0x25, 0x4E, //浮力值 - 0x1B, 0x4E, //滑块位置 - 0x00, //探测状态 - 0x00, 0x9C, //目标距离 - 0x42, 0xBD, //目标方位 - 0x07, 0xD0, //目标频率 - 0xC7, 0x16, //CRC16校验码 - 0x0A //帧尾 - }; + + public static final int BITS_OF_HEAD = 0x2A; + public static final int BITS_OF_END = 0x0A; /** * 洋流相关常量 diff --git a/casic-shelter/pom.xml b/casic-shelter/pom.xml index c1dc630..3c096f1 100644 --- a/casic-shelter/pom.xml +++ b/casic-shelter/pom.xml @@ -57,6 +57,10 @@ spatial4j 0.8 + + io.netty + netty-all + diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java index 4af70db..ea31852 100644 --- a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java @@ -6,31 +6,9 @@ * @author a203 */ public class Constant { - public static final int[] DATA = new int[]{ - 0x2A,//帧头 - 0x23,//帧长度 - 0x01,//帧类型 - 0x00,//子类型 - 0x03,//机器人ID - 0x4D, 0xBD, 0x80, 0xCF,//位置经度 - 0x0C, 0x5E, 0x28, 0xC2,//位置纬度 - 0x01, //航行状态 - 0xEB, //仪表电压 - 0xEF, //动力电压 - 0x5F, //仪表电余量 - 0x5D, //动力电余量 - 0x13, 0x5D, //俯仰角 - 0x40, 0x3D, //横滚角 - 0x30, 0xB2, //航向角 - 0x25, 0x4E, //浮力值 - 0x1B, 0x4E, //滑块位置 - 0x00, //探测状态 - 0x00, 0x9C, //目标距离 - 0x42, 0xBD, //目标方位 - 0x07, 0xD0, //目标频率 - 0xC7, 0x16, //CRC16校验码 - 0x0A //帧尾 - }; + + public static final int BITS_OF_HEAD = 0x2A; + public static final int BITS_OF_END = 0x0A; /** * 洋流相关常量 diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java deleted file mode 100644 index dc6ab54..0000000 --- a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java +++ /dev/null @@ -1,609 +0,0 @@ -package com.casic.missiles.modular.system.utils; - -import com.alibaba.fastjson.JSON; -import com.alibaba.fastjson.TypeReference; -import com.casic.missiles.modular.system.dto.AisDTO; -import com.casic.missiles.modular.system.dto.EnvironmentDTO; -import com.casic.missiles.modular.system.dto.OceanDetailDTO; -import com.casic.missiles.modular.system.dto.remote.OceanDetailRemoteData; -import com.casic.missiles.modular.system.model.RobotInfo; -import org.locationtech.spatial4j.distance.DistanceUtils; - -import java.text.DecimalFormat; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Map; - -/** - * 西工大数据解析协议 - * - * @author a203 - */ -public class DataParser { - - private static final int BITS_OF_HEAD = 0x2A; - private static final int BITS_OF_END = 0x0A; - //赤道半径(单位km) - private static final double EARTH_RADIUS = 6371.393; - - private static boolean isCorrectData(int[] bytes) { - if (bytes[0] != BITS_OF_HEAD) { - return false; - } - return bytes[bytes.length - 1] == BITS_OF_END; - } - - /** - * 水下机器人信息 - */ - public static RobotInfo decodeRobotInfo(int[] bytes) { - RobotInfo robot = new RobotInfo(); - if (!isCorrectData(bytes)) { - return robot; - } - robot.setRobotId(bytes[4]); - - int[] lngBytes = new int[4]; - System.arraycopy(bytes, 5, lngBytes, 0, 4); - robot.setLng(covertLngLat(lngBytes)); - - int[] latBytes = new int[4]; - System.arraycopy(bytes, 9, latBytes, 0, 4); - robot.setLat(covertLngLat(latBytes)); - - robot.setNavigateState(covertState(bytes[13])); - robot.setInstrumentV((float) (bytes[14] * 0.1)); - robot.setPowerV((float) bytes[15]); - robot.setInstrumentE((float) (bytes[16])); - robot.setPowerE((float) (bytes[17])); - - int[] pitchAngleBytes = new int[2]; - System.arraycopy(bytes, 18, pitchAngleBytes, 0, 2); - robot.setPitchAngle(covertPitchAngle(pitchAngleBytes)); - - int[] rollAngleBytes = new int[2]; - System.arraycopy(bytes, 20, rollAngleBytes, 0, 2); - robot.setRollAngle(covertRollAngle(rollAngleBytes)); - - int[] headingAngleBytes = new int[2]; - System.arraycopy(bytes, 22, headingAngleBytes, 0, 2); - robot.setHeadingAngle(covertHeadingAngle(headingAngleBytes)); - - int[] buoyancyBytes = new int[2]; - System.arraycopy(bytes, 24, buoyancyBytes, 0, 2); - robot.setBuoyancy(covertBuoyancy(buoyancyBytes)); - - int[] positionBytes = new int[2]; - System.arraycopy(bytes, 26, positionBytes, 0, 2); - //滑块位置算法和浮力算法一样 - robot.setPosition(covertBuoyancy(positionBytes)); - - int status = bytes[28]; - if (status == 0) { - robot.setDetectStatus("没有目标"); - } else { - robot.setDetectStatus("发现目标"); - } - - int[] targetDisBytes = new int[2]; - System.arraycopy(bytes, 29, targetDisBytes, 0, 2); - robot.setTargetDistance(covertTargetDistance(targetDisBytes)); - - int[] targetDirBytes = new int[2]; - System.arraycopy(bytes, 31, targetDirBytes, 0, 2); - //目标方位算法和航向角算法一样 - robot.setTargetDirection(covertHeadingAngle(targetDirBytes)); - - int[] targetHzBytes = new int[2]; - System.arraycopy(bytes, 33, targetHzBytes, 0, 2); - robot.setTargetHertz(covertTargetHertz(targetHzBytes)); - return robot; - } - - /** - * 区域覆盖 - */ - public static String decodeRobotRegion(int[] bytes) { - if (!isCorrectData(bytes)) { - return ""; - } - Map regionMap = new HashMap<>(); - int count = bytes[4]; - regionMap.put("count", String.valueOf(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]); - } - } - regionMap.put("robotIds", builder.toString()); - - //经度和纬度，所以需要*2 - int targetSize = count * 2 * 4; - int[] targetBytes = new int[targetSize]; - //起始4个字节，机器人个数1个字节 - System.arraycopy(bytes, 5 + idsBytes.length, targetBytes, 0, targetSize); - regionMap.put("robotTargets", formatLanLat(targetBytes)); - - regionMap.put("pointCount", String.valueOf(bytes[5 + idsBytes.length + targetBytes.length])); - - int[] radiusBytes = new int[2]; - System.arraycopy(bytes, 5 + idsBytes.length + targetBytes.length + 1, radiusBytes, 0, 2); - regionMap.put("regionRadius", covertTargetDistance(radiusBytes)); - - int aByte = bytes[5 + idsBytes.length + targetBytes.length + 1 + radiusBytes.length]; - if (aByte == 0) { - regionMap.put("contour", "轮廓标志位:多边形，" + count + "个轮廓点"); - } else { - regionMap.put("contour", "轮廓标志位:圆形，" + 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); - regionMap.put("robotPoints", formatLanLat(pointBytes)); - return JSON.toJSONString(regionMap); - } - - /** - * 路径规划 - */ - public static String decodeRobotRoute(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 decodeDirectedTask(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 decodeTaskRoute(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 decodeDirectFlightTask(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 decodeResideTask(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 decodePutTask(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); - } - - /** - * 短信内容 - */ - public static String decodeSMS(int[] bytes) { - //帧头、帧长度、帧类型、子类型、机器人ID，CRC16校验码、帧尾总长度=8 - int dataLen = bytes.length - 8; - - int[] dataBytes = new int[dataLen]; - System.arraycopy(bytes, 5, dataBytes, 0, dataLen); - StringBuilder builder = new StringBuilder(); - for (int dataByte : dataBytes) { - builder.append((char) dataByte); - } - return builder.toString(); - } - - /** - * AIS数据请求 - */ - public static String decodeAISRequest(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); - } - - /** - * 方舱位置 - * 编码encode - * 解码decode - */ - public static String decodeShelterPosition(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) { - return ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF) + "hz"; - } - - /** - * 目标距离转换算法 - */ - private static String covertTargetDistance(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01) + "km"; - } - - /** - * 横滚角转换算法 - */ - private static float covertBuoyancy(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 100); - } - - /** - * 横滚角转换算法 - */ - private static float covertHeadingAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01); - } - - /** - * 横滚角转换算法 - */ - private static float covertRollAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 180); - } - - /** - * 俯仰角转换算法 - */ - private static float covertPitchAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 90); - } - - /** - * 航行状态转换 - */ - private static String covertState(int b) { - String state = ""; - switch (b) { - case 0: - state = "任务中"; - break; - case 1: - state = "待机"; - break; - case 2: - state = "故障"; - break; - case 3: - state = "未启用"; - break; - default: - break; - } - return state; - } - - /** - * 经纬度转换算法 - */ - private static double covertLngLat(int[] bytes) { - double per = ( - ((bytes[0] & 0xFF) << 24) + - ((bytes[1] & 0xFF) << 16) + - ((bytes[2] & 0xFF) << 8) + - (bytes[3] & 0xFF) - ) / (Math.pow(2, 31) - 1); - return 180 * per; - } - - /** - * 环境数据 - */ - public static Map encodeEnvironment(String data) { - Map resultMap = createDataHead("AIS"); - resultMap.put("type", "Env"); - - EnvironmentDTO dto = new EnvironmentDTO(); - //台风中心经度 - double lng = 109.322222; - dto.setTyphoonLng(lng); - - //台风中心纬度 - double lat = 17.392232; - dto.setTyphoonLat(lat); - - //台风风力 - int power = 14; - dto.setTyphoonPower(power); - - //台风半径 - double radius = 155.56; - dto.setTyphoonRadius(radius); - - //机器人与台风中心距离 - double distance = distance(115.6314213, 12.9691386, 112.0623457, 19.9936680); - dto.setDistance(distance); - - //机器人与台风的方位角 - double angle = azimuth(115.6314213, 12.9691386, 112.0623457, 19.9936680); - dto.setAzimuth(angle); - - //洋流数据 - String oceanData = HttpRequestHelper.obtainOceanData(115.6314213, 12.9691386, "2021-10-22", "0", "10"); - OceanDetailDTO oceanDetailDTO = null; - if ("0".equals(HttpRequestHelper.getOceanCode(oceanData))) { - OceanDetailRemoteData remoteData = JSON.parseObject(oceanData, new TypeReference() { - }); - oceanDetailDTO = new OceanDetailDTO(remoteData); - } - dto.setOceanDetail(oceanDetailDTO); - - resultMap.put("data", dto); - return resultMap; - } - - /** - * AIS数据 - */ - public static Map encodeAIS(String data) { - Map resultMap = createDataHead("AIS"); - resultMap.put("type", "AIS"); - //船只数量 - int shipCount = 2; - List positionBeans = new ArrayList<>(); - for (int i = 0; i < shipCount; i++) { - AisDTO ais = new AisDTO(); - ais.setCourse(187.2); - ais.setHeading(189.0); - //船只经度 - double lng = 109.322222; - ais.setLng(lng); - //船只纬度 - double lat = 17.392232; - ais.setLat(lat); - ais.setPositionTime("2021-09-29 16:12:25"); - ais.setSpeed(16.3); - - positionBeans.add(ais); - } - resultMap.put("data", positionBeans); - return resultMap; - } - - /** - * 短信数据 - */ - public static Map encodeSMS(String data) { - Map resultMap = createDataHead("SMS"); - resultMap.put("data", data); - return resultMap; - } - - private static Map createDataHead(String type) { - Map resultMap = new HashMap<>(8); - resultMap.put("code", 200); - resultMap.put("type", type); - resultMap.put("message", "请求成功"); - resultMap.put("success", true); - return resultMap; - } - - /** - * 经纬度转弧度 - */ - private static double lnglatToRadian(double d) { - return d * Math.PI / 180.0; - } - - /** - * 经纬度距离计算 - */ - private static double distance(double lonA, double latA, double lonB, double latB) { - double aLng = lnglatToRadian(lonA); - double aLat = lnglatToRadian(latA); - double bLng = lnglatToRadian(lonB); - double bLat = lnglatToRadian(latB); - - double dist = DistanceUtils.distHaversineRAD(aLat, aLng, bLat, bLng) * EARTH_RADIUS; - DecimalFormat df = new DecimalFormat("#.00"); - return Double.parseDouble(df.format(dist)); - } - - /** - * 方位角 - *

- * https://blog.csdn.net/dulingwen/article/details/96868530 - */ - private static int azimuth(double lonA, double latA, double lonB, double latB) { - double deltaLng = lonB - lonA; - - double y = Math.sin(deltaLng) * Math.cos(latB); - double x = Math.cos(latA) * Math.sin(latB) - Math.sin(latA) * Math.cos(latB) * Math.cos(deltaLng); - - double bearing = Math.atan2(y, x); - bearing = Math.toDegrees(bearing); - bearing = (bearing + 360) % 360; - - return (int) bearing; - } -} \ No newline at end of file diff --git a/casic-shelter/pom.xml b/casic-shelter/pom.xml index c1dc630..3c096f1 100644 --- a/casic-shelter/pom.xml +++ b/casic-shelter/pom.xml @@ -57,6 +57,10 @@ spatial4j 0.8 + + io.netty + netty-all + diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java index 4af70db..ea31852 100644 --- a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java @@ -6,31 +6,9 @@ * @author a203 */ public class Constant { - public static final int[] DATA = new int[]{ - 0x2A,//帧头 - 0x23,//帧长度 - 0x01,//帧类型 - 0x00,//子类型 - 0x03,//机器人ID - 0x4D, 0xBD, 0x80, 0xCF,//位置经度 - 0x0C, 0x5E, 0x28, 0xC2,//位置纬度 - 0x01, //航行状态 - 0xEB, //仪表电压 - 0xEF, //动力电压 - 0x5F, //仪表电余量 - 0x5D, //动力电余量 - 0x13, 0x5D, //俯仰角 - 0x40, 0x3D, //横滚角 - 0x30, 0xB2, //航向角 - 0x25, 0x4E, //浮力值 - 0x1B, 0x4E, //滑块位置 - 0x00, //探测状态 - 0x00, 0x9C, //目标距离 - 0x42, 0xBD, //目标方位 - 0x07, 0xD0, //目标频率 - 0xC7, 0x16, //CRC16校验码 - 0x0A //帧尾 - }; + + public static final int BITS_OF_HEAD = 0x2A; + public static final int BITS_OF_END = 0x0A; /** * 洋流相关常量 diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java deleted file mode 100644 index dc6ab54..0000000 --- a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java +++ /dev/null @@ -1,609 +0,0 @@ -package com.casic.missiles.modular.system.utils; - -import com.alibaba.fastjson.JSON; -import com.alibaba.fastjson.TypeReference; -import com.casic.missiles.modular.system.dto.AisDTO; -import com.casic.missiles.modular.system.dto.EnvironmentDTO; -import com.casic.missiles.modular.system.dto.OceanDetailDTO; -import com.casic.missiles.modular.system.dto.remote.OceanDetailRemoteData; -import com.casic.missiles.modular.system.model.RobotInfo; -import org.locationtech.spatial4j.distance.DistanceUtils; - -import java.text.DecimalFormat; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Map; - -/** - * 西工大数据解析协议 - * - * @author a203 - */ -public class DataParser { - - private static final int BITS_OF_HEAD = 0x2A; - private static final int BITS_OF_END = 0x0A; - //赤道半径(单位km) - private static final double EARTH_RADIUS = 6371.393; - - private static boolean isCorrectData(int[] bytes) { - if (bytes[0] != BITS_OF_HEAD) { - return false; - } - return bytes[bytes.length - 1] == BITS_OF_END; - } - - /** - * 水下机器人信息 - */ - public static RobotInfo decodeRobotInfo(int[] bytes) { - RobotInfo robot = new RobotInfo(); - if (!isCorrectData(bytes)) { - return robot; - } - robot.setRobotId(bytes[4]); - - int[] lngBytes = new int[4]; - System.arraycopy(bytes, 5, lngBytes, 0, 4); - robot.setLng(covertLngLat(lngBytes)); - - int[] latBytes = new int[4]; - System.arraycopy(bytes, 9, latBytes, 0, 4); - robot.setLat(covertLngLat(latBytes)); - - robot.setNavigateState(covertState(bytes[13])); - robot.setInstrumentV((float) (bytes[14] * 0.1)); - robot.setPowerV((float) bytes[15]); - robot.setInstrumentE((float) (bytes[16])); - robot.setPowerE((float) (bytes[17])); - - int[] pitchAngleBytes = new int[2]; - System.arraycopy(bytes, 18, pitchAngleBytes, 0, 2); - robot.setPitchAngle(covertPitchAngle(pitchAngleBytes)); - - int[] rollAngleBytes = new int[2]; - System.arraycopy(bytes, 20, rollAngleBytes, 0, 2); - robot.setRollAngle(covertRollAngle(rollAngleBytes)); - - int[] headingAngleBytes = new int[2]; - System.arraycopy(bytes, 22, headingAngleBytes, 0, 2); - robot.setHeadingAngle(covertHeadingAngle(headingAngleBytes)); - - int[] buoyancyBytes = new int[2]; - System.arraycopy(bytes, 24, buoyancyBytes, 0, 2); - robot.setBuoyancy(covertBuoyancy(buoyancyBytes)); - - int[] positionBytes = new int[2]; - System.arraycopy(bytes, 26, positionBytes, 0, 2); - //滑块位置算法和浮力算法一样 - robot.setPosition(covertBuoyancy(positionBytes)); - - int status = bytes[28]; - if (status == 0) { - robot.setDetectStatus("没有目标"); - } else { - robot.setDetectStatus("发现目标"); - } - - int[] targetDisBytes = new int[2]; - System.arraycopy(bytes, 29, targetDisBytes, 0, 2); - robot.setTargetDistance(covertTargetDistance(targetDisBytes)); - - int[] targetDirBytes = new int[2]; - System.arraycopy(bytes, 31, targetDirBytes, 0, 2); - //目标方位算法和航向角算法一样 - robot.setTargetDirection(covertHeadingAngle(targetDirBytes)); - - int[] targetHzBytes = new int[2]; - System.arraycopy(bytes, 33, targetHzBytes, 0, 2); - robot.setTargetHertz(covertTargetHertz(targetHzBytes)); - return robot; - } - - /** - * 区域覆盖 - */ - public static String decodeRobotRegion(int[] bytes) { - if (!isCorrectData(bytes)) { - return ""; - } - Map regionMap = new HashMap<>(); - int count = bytes[4]; - regionMap.put("count", String.valueOf(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]); - } - } - regionMap.put("robotIds", builder.toString()); - - //经度和纬度，所以需要*2 - int targetSize = count * 2 * 4; - int[] targetBytes = new int[targetSize]; - //起始4个字节，机器人个数1个字节 - System.arraycopy(bytes, 5 + idsBytes.length, targetBytes, 0, targetSize); - regionMap.put("robotTargets", formatLanLat(targetBytes)); - - regionMap.put("pointCount", String.valueOf(bytes[5 + idsBytes.length + targetBytes.length])); - - int[] radiusBytes = new int[2]; - System.arraycopy(bytes, 5 + idsBytes.length + targetBytes.length + 1, radiusBytes, 0, 2); - regionMap.put("regionRadius", covertTargetDistance(radiusBytes)); - - int aByte = bytes[5 + idsBytes.length + targetBytes.length + 1 + radiusBytes.length]; - if (aByte == 0) { - regionMap.put("contour", "轮廓标志位:多边形，" + count + "个轮廓点"); - } else { - regionMap.put("contour", "轮廓标志位:圆形，" + 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); - regionMap.put("robotPoints", formatLanLat(pointBytes)); - return JSON.toJSONString(regionMap); - } - - /** - * 路径规划 - */ - public static String decodeRobotRoute(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 decodeDirectedTask(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 decodeTaskRoute(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 decodeDirectFlightTask(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 decodeResideTask(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 decodePutTask(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); - } - - /** - * 短信内容 - */ - public static String decodeSMS(int[] bytes) { - //帧头、帧长度、帧类型、子类型、机器人ID，CRC16校验码、帧尾总长度=8 - int dataLen = bytes.length - 8; - - int[] dataBytes = new int[dataLen]; - System.arraycopy(bytes, 5, dataBytes, 0, dataLen); - StringBuilder builder = new StringBuilder(); - for (int dataByte : dataBytes) { - builder.append((char) dataByte); - } - return builder.toString(); - } - - /** - * AIS数据请求 - */ - public static String decodeAISRequest(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); - } - - /** - * 方舱位置 - * 编码encode - * 解码decode - */ - public static String decodeShelterPosition(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) { - return ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF) + "hz"; - } - - /** - * 目标距离转换算法 - */ - private static String covertTargetDistance(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01) + "km"; - } - - /** - * 横滚角转换算法 - */ - private static float covertBuoyancy(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 100); - } - - /** - * 横滚角转换算法 - */ - private static float covertHeadingAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01); - } - - /** - * 横滚角转换算法 - */ - private static float covertRollAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 180); - } - - /** - * 俯仰角转换算法 - */ - private static float covertPitchAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 90); - } - - /** - * 航行状态转换 - */ - private static String covertState(int b) { - String state = ""; - switch (b) { - case 0: - state = "任务中"; - break; - case 1: - state = "待机"; - break; - case 2: - state = "故障"; - break; - case 3: - state = "未启用"; - break; - default: - break; - } - return state; - } - - /** - * 经纬度转换算法 - */ - private static double covertLngLat(int[] bytes) { - double per = ( - ((bytes[0] & 0xFF) << 24) + - ((bytes[1] & 0xFF) << 16) + - ((bytes[2] & 0xFF) << 8) + - (bytes[3] & 0xFF) - ) / (Math.pow(2, 31) - 1); - return 180 * per; - } - - /** - * 环境数据 - */ - public static Map encodeEnvironment(String data) { - Map resultMap = createDataHead("AIS"); - resultMap.put("type", "Env"); - - EnvironmentDTO dto = new EnvironmentDTO(); - //台风中心经度 - double lng = 109.322222; - dto.setTyphoonLng(lng); - - //台风中心纬度 - double lat = 17.392232; - dto.setTyphoonLat(lat); - - //台风风力 - int power = 14; - dto.setTyphoonPower(power); - - //台风半径 - double radius = 155.56; - dto.setTyphoonRadius(radius); - - //机器人与台风中心距离 - double distance = distance(115.6314213, 12.9691386, 112.0623457, 19.9936680); - dto.setDistance(distance); - - //机器人与台风的方位角 - double angle = azimuth(115.6314213, 12.9691386, 112.0623457, 19.9936680); - dto.setAzimuth(angle); - - //洋流数据 - String oceanData = HttpRequestHelper.obtainOceanData(115.6314213, 12.9691386, "2021-10-22", "0", "10"); - OceanDetailDTO oceanDetailDTO = null; - if ("0".equals(HttpRequestHelper.getOceanCode(oceanData))) { - OceanDetailRemoteData remoteData = JSON.parseObject(oceanData, new TypeReference() { - }); - oceanDetailDTO = new OceanDetailDTO(remoteData); - } - dto.setOceanDetail(oceanDetailDTO); - - resultMap.put("data", dto); - return resultMap; - } - - /** - * AIS数据 - */ - public static Map encodeAIS(String data) { - Map resultMap = createDataHead("AIS"); - resultMap.put("type", "AIS"); - //船只数量 - int shipCount = 2; - List positionBeans = new ArrayList<>(); - for (int i = 0; i < shipCount; i++) { - AisDTO ais = new AisDTO(); - ais.setCourse(187.2); - ais.setHeading(189.0); - //船只经度 - double lng = 109.322222; - ais.setLng(lng); - //船只纬度 - double lat = 17.392232; - ais.setLat(lat); - ais.setPositionTime("2021-09-29 16:12:25"); - ais.setSpeed(16.3); - - positionBeans.add(ais); - } - resultMap.put("data", positionBeans); - return resultMap; - } - - /** - * 短信数据 - */ - public static Map encodeSMS(String data) { - Map resultMap = createDataHead("SMS"); - resultMap.put("data", data); - return resultMap; - } - - private static Map createDataHead(String type) { - Map resultMap = new HashMap<>(8); - resultMap.put("code", 200); - resultMap.put("type", type); - resultMap.put("message", "请求成功"); - resultMap.put("success", true); - return resultMap; - } - - /** - * 经纬度转弧度 - */ - private static double lnglatToRadian(double d) { - return d * Math.PI / 180.0; - } - - /** - * 经纬度距离计算 - */ - private static double distance(double lonA, double latA, double lonB, double latB) { - double aLng = lnglatToRadian(lonA); - double aLat = lnglatToRadian(latA); - double bLng = lnglatToRadian(lonB); - double bLat = lnglatToRadian(latB); - - double dist = DistanceUtils.distHaversineRAD(aLat, aLng, bLat, bLng) * EARTH_RADIUS; - DecimalFormat df = new DecimalFormat("#.00"); - return Double.parseDouble(df.format(dist)); - } - - /** - * 方位角 - *

- * https://blog.csdn.net/dulingwen/article/details/96868530 - */ - private static int azimuth(double lonA, double latA, double lonB, double latB) { - double deltaLng = lonB - lonA; - - double y = Math.sin(deltaLng) * Math.cos(latB); - double x = Math.cos(latA) * Math.sin(latB) - Math.sin(latA) * Math.cos(latB) * Math.cos(deltaLng); - - double bearing = Math.atan2(y, x); - bearing = Math.toDegrees(bearing); - bearing = (bearing + 360) % 360; - - return (int) bearing; - } -} \ No newline at end of file diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java new file mode 100644 index 0000000..9cb0e99 --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java @@ -0,0 +1,429 @@ +package com.casic.missiles.modular.system.utils; + +import com.alibaba.fastjson.JSON; +import com.casic.missiles.modular.system.model.RobotInfo; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * 西工大数据解析 + * + * @author a203 + */ +public class DecodeData { + /** + * 水下机器人信息 + */ + public static RobotInfo decodeRobotInfo(int[] bytes) { + RobotInfo robot = new RobotInfo(); + + robot.setRobotId(bytes[4]); + + int[] lngBytes = new int[4]; + System.arraycopy(bytes, 5, lngBytes, 0, 4); + robot.setLng(covertLngLat(lngBytes)); + + int[] latBytes = new int[4]; + System.arraycopy(bytes, 9, latBytes, 0, 4); + robot.setLat(covertLngLat(latBytes)); + + robot.setNavigateState(covertState(bytes[13])); + robot.setInstrumentV((float) (bytes[14] * 0.1)); + robot.setPowerV((float) bytes[15]); + robot.setInstrumentE((float) (bytes[16])); + robot.setPowerE((float) (bytes[17])); + + int[] pitchAngleBytes = new int[2]; + System.arraycopy(bytes, 18, pitchAngleBytes, 0, 2); + robot.setPitchAngle(covertPitchAngle(pitchAngleBytes)); + + int[] rollAngleBytes = new int[2]; + System.arraycopy(bytes, 20, rollAngleBytes, 0, 2); + robot.setRollAngle(covertRollAngle(rollAngleBytes)); + + int[] headingAngleBytes = new int[2]; + System.arraycopy(bytes, 22, headingAngleBytes, 0, 2); + robot.setHeadingAngle(covertHeadingAngle(headingAngleBytes)); + + int[] buoyancyBytes = new int[2]; + System.arraycopy(bytes, 24, buoyancyBytes, 0, 2); + robot.setBuoyancy(covertBuoyancy(buoyancyBytes)); + + int[] positionBytes = new int[2]; + System.arraycopy(bytes, 26, positionBytes, 0, 2); + //滑块位置算法和浮力算法一样 + robot.setPosition(covertBuoyancy(positionBytes)); + + int status = bytes[28]; + if (status == 0) { + robot.setDetectStatus("没有目标"); + } else { + robot.setDetectStatus("发现目标"); + } + + int[] targetDisBytes = new int[2]; + System.arraycopy(bytes, 29, targetDisBytes, 0, 2); + robot.setTargetDistance(covertTargetDistance(targetDisBytes)); + + int[] targetDirBytes = new int[2]; + System.arraycopy(bytes, 31, targetDirBytes, 0, 2); + //目标方位算法和航向角算法一样 + robot.setTargetDirection(covertHeadingAngle(targetDirBytes)); + + int[] targetHzBytes = new int[2]; + System.arraycopy(bytes, 33, targetHzBytes, 0, 2); + robot.setTargetHertz(covertTargetHertz(targetHzBytes)); + return robot; + } + + /** + * 区域覆盖 + */ + public static String decodeRobotRegion(int[] bytes) { + Map regionMap = new HashMap<>(); + int count = bytes[4]; + regionMap.put("count", String.valueOf(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]); + } + } + regionMap.put("robotIds", builder.toString()); + + //经度和纬度，所以需要*2 + int targetSize = count * 2 * 4; + int[] targetBytes = new int[targetSize]; + //起始4个字节，机器人个数1个字节 + System.arraycopy(bytes, 5 + idsBytes.length, targetBytes, 0, targetSize); + regionMap.put("robotTargets", formatLanLat(targetBytes)); + + regionMap.put("pointCount", String.valueOf(bytes[5 + idsBytes.length + targetBytes.length])); + + int[] radiusBytes = new int[2]; + System.arraycopy(bytes, 5 + idsBytes.length + targetBytes.length + 1, radiusBytes, 0, 2); + regionMap.put("regionRadius", covertTargetDistance(radiusBytes)); + + int aByte = bytes[5 + idsBytes.length + targetBytes.length + 1 + radiusBytes.length]; + if (aByte == 0) { + regionMap.put("contour", "轮廓标志位:多边形，" + count + "个轮廓点"); + } else { + regionMap.put("contour", "轮廓标志位:圆形，" + 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); + regionMap.put("robotPoints", formatLanLat(pointBytes)); + return JSON.toJSONString(regionMap); + } + + /** + * 路径规划 + */ + public static String decodeRobotRoute(int[] bytes) { + 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 decodeDirectedTask(int[] bytes) { + 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 decodeTaskRoute(int[] bytes) { + 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 decodeDirectFlightTask(int[] bytes) { + 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 decodeResideTask(int[] bytes) { + 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 decodePutTask(int[] bytes) { + 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); + } + + /** + * 短信内容 + */ + public static String decodeSMS(int[] bytes) { + //帧头、帧长度、帧类型、子类型、机器人ID，CRC16校验码、帧尾总长度=8 + int dataLen = bytes.length - 8; + + int[] dataBytes = new int[dataLen]; + System.arraycopy(bytes, 5, dataBytes, 0, dataLen); + StringBuilder builder = new StringBuilder(); + for (int dataByte : dataBytes) { + builder.append((char) dataByte); + } + return builder.toString(); + } + + /** + * AIS数据请求 + */ + public static String decodeAISRequest(int[] bytes) { + 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); + } + + /** + * 方舱位置 + * 编码encode + * 解码decode + */ + public static String decodeShelterPosition(int[] bytes) { + 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) { + return ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF) + "hz"; + } + + /** + * 目标距离转换算法 + */ + private static String covertTargetDistance(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01) + "km"; + } + + /** + * 横滚角转换算法 + */ + private static float covertBuoyancy(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 100); + } + + /** + * 横滚角转换算法 + */ + private static float covertHeadingAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01); + } + + /** + * 横滚角转换算法 + */ + private static float covertRollAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 180); + } + + /** + * 俯仰角转换算法 + */ + private static float covertPitchAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 90); + } + + /** + * 航行状态转换 + */ + private static String covertState(int b) { + String state = ""; + switch (b) { + case 0: + state = "任务中"; + break; + case 1: + state = "待机"; + break; + case 2: + state = "故障"; + break; + case 3: + state = "未启用"; + break; + default: + break; + } + return state; + } + + /** + * 经纬度转换算法 + */ + private static double covertLngLat(int[] bytes) { + double per = ( + ((bytes[0] & 0xFF) << 24) + + ((bytes[1] & 0xFF) << 16) + + ((bytes[2] & 0xFF) << 8) + + (bytes[3] & 0xFF) + ) / (Math.pow(2, 31) - 1); + return 180 * per; + } +} diff --git a/casic-shelter/pom.xml b/casic-shelter/pom.xml index c1dc630..3c096f1 100644 --- a/casic-shelter/pom.xml +++ b/casic-shelter/pom.xml @@ -57,6 +57,10 @@ spatial4j 0.8 + + io.netty + netty-all + diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java index 4af70db..ea31852 100644 --- a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java @@ -6,31 +6,9 @@ * @author a203 */ public class Constant { - public static final int[] DATA = new int[]{ - 0x2A,//帧头 - 0x23,//帧长度 - 0x01,//帧类型 - 0x00,//子类型 - 0x03,//机器人ID - 0x4D, 0xBD, 0x80, 0xCF,//位置经度 - 0x0C, 0x5E, 0x28, 0xC2,//位置纬度 - 0x01, //航行状态 - 0xEB, //仪表电压 - 0xEF, //动力电压 - 0x5F, //仪表电余量 - 0x5D, //动力电余量 - 0x13, 0x5D, //俯仰角 - 0x40, 0x3D, //横滚角 - 0x30, 0xB2, //航向角 - 0x25, 0x4E, //浮力值 - 0x1B, 0x4E, //滑块位置 - 0x00, //探测状态 - 0x00, 0x9C, //目标距离 - 0x42, 0xBD, //目标方位 - 0x07, 0xD0, //目标频率 - 0xC7, 0x16, //CRC16校验码 - 0x0A //帧尾 - }; + + public static final int BITS_OF_HEAD = 0x2A; + public static final int BITS_OF_END = 0x0A; /** * 洋流相关常量 diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java deleted file mode 100644 index dc6ab54..0000000 --- a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java +++ /dev/null @@ -1,609 +0,0 @@ -package com.casic.missiles.modular.system.utils; - -import com.alibaba.fastjson.JSON; -import com.alibaba.fastjson.TypeReference; -import com.casic.missiles.modular.system.dto.AisDTO; -import com.casic.missiles.modular.system.dto.EnvironmentDTO; -import com.casic.missiles.modular.system.dto.OceanDetailDTO; -import com.casic.missiles.modular.system.dto.remote.OceanDetailRemoteData; -import com.casic.missiles.modular.system.model.RobotInfo; -import org.locationtech.spatial4j.distance.DistanceUtils; - -import java.text.DecimalFormat; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Map; - -/** - * 西工大数据解析协议 - * - * @author a203 - */ -public class DataParser { - - private static final int BITS_OF_HEAD = 0x2A; - private static final int BITS_OF_END = 0x0A; - //赤道半径(单位km) - private static final double EARTH_RADIUS = 6371.393; - - private static boolean isCorrectData(int[] bytes) { - if (bytes[0] != BITS_OF_HEAD) { - return false; - } - return bytes[bytes.length - 1] == BITS_OF_END; - } - - /** - * 水下机器人信息 - */ - public static RobotInfo decodeRobotInfo(int[] bytes) { - RobotInfo robot = new RobotInfo(); - if (!isCorrectData(bytes)) { - return robot; - } - robot.setRobotId(bytes[4]); - - int[] lngBytes = new int[4]; - System.arraycopy(bytes, 5, lngBytes, 0, 4); - robot.setLng(covertLngLat(lngBytes)); - - int[] latBytes = new int[4]; - System.arraycopy(bytes, 9, latBytes, 0, 4); - robot.setLat(covertLngLat(latBytes)); - - robot.setNavigateState(covertState(bytes[13])); - robot.setInstrumentV((float) (bytes[14] * 0.1)); - robot.setPowerV((float) bytes[15]); - robot.setInstrumentE((float) (bytes[16])); - robot.setPowerE((float) (bytes[17])); - - int[] pitchAngleBytes = new int[2]; - System.arraycopy(bytes, 18, pitchAngleBytes, 0, 2); - robot.setPitchAngle(covertPitchAngle(pitchAngleBytes)); - - int[] rollAngleBytes = new int[2]; - System.arraycopy(bytes, 20, rollAngleBytes, 0, 2); - robot.setRollAngle(covertRollAngle(rollAngleBytes)); - - int[] headingAngleBytes = new int[2]; - System.arraycopy(bytes, 22, headingAngleBytes, 0, 2); - robot.setHeadingAngle(covertHeadingAngle(headingAngleBytes)); - - int[] buoyancyBytes = new int[2]; - System.arraycopy(bytes, 24, buoyancyBytes, 0, 2); - robot.setBuoyancy(covertBuoyancy(buoyancyBytes)); - - int[] positionBytes = new int[2]; - System.arraycopy(bytes, 26, positionBytes, 0, 2); - //滑块位置算法和浮力算法一样 - robot.setPosition(covertBuoyancy(positionBytes)); - - int status = bytes[28]; - if (status == 0) { - robot.setDetectStatus("没有目标"); - } else { - robot.setDetectStatus("发现目标"); - } - - int[] targetDisBytes = new int[2]; - System.arraycopy(bytes, 29, targetDisBytes, 0, 2); - robot.setTargetDistance(covertTargetDistance(targetDisBytes)); - - int[] targetDirBytes = new int[2]; - System.arraycopy(bytes, 31, targetDirBytes, 0, 2); - //目标方位算法和航向角算法一样 - robot.setTargetDirection(covertHeadingAngle(targetDirBytes)); - - int[] targetHzBytes = new int[2]; - System.arraycopy(bytes, 33, targetHzBytes, 0, 2); - robot.setTargetHertz(covertTargetHertz(targetHzBytes)); - return robot; - } - - /** - * 区域覆盖 - */ - public static String decodeRobotRegion(int[] bytes) { - if (!isCorrectData(bytes)) { - return ""; - } - Map regionMap = new HashMap<>(); - int count = bytes[4]; - regionMap.put("count", String.valueOf(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]); - } - } - regionMap.put("robotIds", builder.toString()); - - //经度和纬度，所以需要*2 - int targetSize = count * 2 * 4; - int[] targetBytes = new int[targetSize]; - //起始4个字节，机器人个数1个字节 - System.arraycopy(bytes, 5 + idsBytes.length, targetBytes, 0, targetSize); - regionMap.put("robotTargets", formatLanLat(targetBytes)); - - regionMap.put("pointCount", String.valueOf(bytes[5 + idsBytes.length + targetBytes.length])); - - int[] radiusBytes = new int[2]; - System.arraycopy(bytes, 5 + idsBytes.length + targetBytes.length + 1, radiusBytes, 0, 2); - regionMap.put("regionRadius", covertTargetDistance(radiusBytes)); - - int aByte = bytes[5 + idsBytes.length + targetBytes.length + 1 + radiusBytes.length]; - if (aByte == 0) { - regionMap.put("contour", "轮廓标志位:多边形，" + count + "个轮廓点"); - } else { - regionMap.put("contour", "轮廓标志位:圆形，" + 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); - regionMap.put("robotPoints", formatLanLat(pointBytes)); - return JSON.toJSONString(regionMap); - } - - /** - * 路径规划 - */ - public static String decodeRobotRoute(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 decodeDirectedTask(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 decodeTaskRoute(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 decodeDirectFlightTask(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 decodeResideTask(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 decodePutTask(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); - } - - /** - * 短信内容 - */ - public static String decodeSMS(int[] bytes) { - //帧头、帧长度、帧类型、子类型、机器人ID，CRC16校验码、帧尾总长度=8 - int dataLen = bytes.length - 8; - - int[] dataBytes = new int[dataLen]; - System.arraycopy(bytes, 5, dataBytes, 0, dataLen); - StringBuilder builder = new StringBuilder(); - for (int dataByte : dataBytes) { - builder.append((char) dataByte); - } - return builder.toString(); - } - - /** - * AIS数据请求 - */ - public static String decodeAISRequest(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); - } - - /** - * 方舱位置 - * 编码encode - * 解码decode - */ - public static String decodeShelterPosition(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) { - return ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF) + "hz"; - } - - /** - * 目标距离转换算法 - */ - private static String covertTargetDistance(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01) + "km"; - } - - /** - * 横滚角转换算法 - */ - private static float covertBuoyancy(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 100); - } - - /** - * 横滚角转换算法 - */ - private static float covertHeadingAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01); - } - - /** - * 横滚角转换算法 - */ - private static float covertRollAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 180); - } - - /** - * 俯仰角转换算法 - */ - private static float covertPitchAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 90); - } - - /** - * 航行状态转换 - */ - private static String covertState(int b) { - String state = ""; - switch (b) { - case 0: - state = "任务中"; - break; - case 1: - state = "待机"; - break; - case 2: - state = "故障"; - break; - case 3: - state = "未启用"; - break; - default: - break; - } - return state; - } - - /** - * 经纬度转换算法 - */ - private static double covertLngLat(int[] bytes) { - double per = ( - ((bytes[0] & 0xFF) << 24) + - ((bytes[1] & 0xFF) << 16) + - ((bytes[2] & 0xFF) << 8) + - (bytes[3] & 0xFF) - ) / (Math.pow(2, 31) - 1); - return 180 * per; - } - - /** - * 环境数据 - */ - public static Map encodeEnvironment(String data) { - Map resultMap = createDataHead("AIS"); - resultMap.put("type", "Env"); - - EnvironmentDTO dto = new EnvironmentDTO(); - //台风中心经度 - double lng = 109.322222; - dto.setTyphoonLng(lng); - - //台风中心纬度 - double lat = 17.392232; - dto.setTyphoonLat(lat); - - //台风风力 - int power = 14; - dto.setTyphoonPower(power); - - //台风半径 - double radius = 155.56; - dto.setTyphoonRadius(radius); - - //机器人与台风中心距离 - double distance = distance(115.6314213, 12.9691386, 112.0623457, 19.9936680); - dto.setDistance(distance); - - //机器人与台风的方位角 - double angle = azimuth(115.6314213, 12.9691386, 112.0623457, 19.9936680); - dto.setAzimuth(angle); - - //洋流数据 - String oceanData = HttpRequestHelper.obtainOceanData(115.6314213, 12.9691386, "2021-10-22", "0", "10"); - OceanDetailDTO oceanDetailDTO = null; - if ("0".equals(HttpRequestHelper.getOceanCode(oceanData))) { - OceanDetailRemoteData remoteData = JSON.parseObject(oceanData, new TypeReference() { - }); - oceanDetailDTO = new OceanDetailDTO(remoteData); - } - dto.setOceanDetail(oceanDetailDTO); - - resultMap.put("data", dto); - return resultMap; - } - - /** - * AIS数据 - */ - public static Map encodeAIS(String data) { - Map resultMap = createDataHead("AIS"); - resultMap.put("type", "AIS"); - //船只数量 - int shipCount = 2; - List positionBeans = new ArrayList<>(); - for (int i = 0; i < shipCount; i++) { - AisDTO ais = new AisDTO(); - ais.setCourse(187.2); - ais.setHeading(189.0); - //船只经度 - double lng = 109.322222; - ais.setLng(lng); - //船只纬度 - double lat = 17.392232; - ais.setLat(lat); - ais.setPositionTime("2021-09-29 16:12:25"); - ais.setSpeed(16.3); - - positionBeans.add(ais); - } - resultMap.put("data", positionBeans); - return resultMap; - } - - /** - * 短信数据 - */ - public static Map encodeSMS(String data) { - Map resultMap = createDataHead("SMS"); - resultMap.put("data", data); - return resultMap; - } - - private static Map createDataHead(String type) { - Map resultMap = new HashMap<>(8); - resultMap.put("code", 200); - resultMap.put("type", type); - resultMap.put("message", "请求成功"); - resultMap.put("success", true); - return resultMap; - } - - /** - * 经纬度转弧度 - */ - private static double lnglatToRadian(double d) { - return d * Math.PI / 180.0; - } - - /** - * 经纬度距离计算 - */ - private static double distance(double lonA, double latA, double lonB, double latB) { - double aLng = lnglatToRadian(lonA); - double aLat = lnglatToRadian(latA); - double bLng = lnglatToRadian(lonB); - double bLat = lnglatToRadian(latB); - - double dist = DistanceUtils.distHaversineRAD(aLat, aLng, bLat, bLng) * EARTH_RADIUS; - DecimalFormat df = new DecimalFormat("#.00"); - return Double.parseDouble(df.format(dist)); - } - - /** - * 方位角 - *

- * https://blog.csdn.net/dulingwen/article/details/96868530 - */ - private static int azimuth(double lonA, double latA, double lonB, double latB) { - double deltaLng = lonB - lonA; - - double y = Math.sin(deltaLng) * Math.cos(latB); - double x = Math.cos(latA) * Math.sin(latB) - Math.sin(latA) * Math.cos(latB) * Math.cos(deltaLng); - - double bearing = Math.atan2(y, x); - bearing = Math.toDegrees(bearing); - bearing = (bearing + 360) % 360; - - return (int) bearing; - } -} \ No newline at end of file diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java new file mode 100644 index 0000000..9cb0e99 --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java @@ -0,0 +1,429 @@ +package com.casic.missiles.modular.system.utils; + +import com.alibaba.fastjson.JSON; +import com.casic.missiles.modular.system.model.RobotInfo; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * 西工大数据解析 + * + * @author a203 + */ +public class DecodeData { + /** + * 水下机器人信息 + */ + public static RobotInfo decodeRobotInfo(int[] bytes) { + RobotInfo robot = new RobotInfo(); + + robot.setRobotId(bytes[4]); + + int[] lngBytes = new int[4]; + System.arraycopy(bytes, 5, lngBytes, 0, 4); + robot.setLng(covertLngLat(lngBytes)); + + int[] latBytes = new int[4]; + System.arraycopy(bytes, 9, latBytes, 0, 4); + robot.setLat(covertLngLat(latBytes)); + + robot.setNavigateState(covertState(bytes[13])); + robot.setInstrumentV((float) (bytes[14] * 0.1)); + robot.setPowerV((float) bytes[15]); + robot.setInstrumentE((float) (bytes[16])); + robot.setPowerE((float) (bytes[17])); + + int[] pitchAngleBytes = new int[2]; + System.arraycopy(bytes, 18, pitchAngleBytes, 0, 2); + robot.setPitchAngle(covertPitchAngle(pitchAngleBytes)); + + int[] rollAngleBytes = new int[2]; + System.arraycopy(bytes, 20, rollAngleBytes, 0, 2); + robot.setRollAngle(covertRollAngle(rollAngleBytes)); + + int[] headingAngleBytes = new int[2]; + System.arraycopy(bytes, 22, headingAngleBytes, 0, 2); + robot.setHeadingAngle(covertHeadingAngle(headingAngleBytes)); + + int[] buoyancyBytes = new int[2]; + System.arraycopy(bytes, 24, buoyancyBytes, 0, 2); + robot.setBuoyancy(covertBuoyancy(buoyancyBytes)); + + int[] positionBytes = new int[2]; + System.arraycopy(bytes, 26, positionBytes, 0, 2); + //滑块位置算法和浮力算法一样 + robot.setPosition(covertBuoyancy(positionBytes)); + + int status = bytes[28]; + if (status == 0) { + robot.setDetectStatus("没有目标"); + } else { + robot.setDetectStatus("发现目标"); + } + + int[] targetDisBytes = new int[2]; + System.arraycopy(bytes, 29, targetDisBytes, 0, 2); + robot.setTargetDistance(covertTargetDistance(targetDisBytes)); + + int[] targetDirBytes = new int[2]; + System.arraycopy(bytes, 31, targetDirBytes, 0, 2); + //目标方位算法和航向角算法一样 + robot.setTargetDirection(covertHeadingAngle(targetDirBytes)); + + int[] targetHzBytes = new int[2]; + System.arraycopy(bytes, 33, targetHzBytes, 0, 2); + robot.setTargetHertz(covertTargetHertz(targetHzBytes)); + return robot; + } + + /** + * 区域覆盖 + */ + public static String decodeRobotRegion(int[] bytes) { + Map regionMap = new HashMap<>(); + int count = bytes[4]; + regionMap.put("count", String.valueOf(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]); + } + } + regionMap.put("robotIds", builder.toString()); + + //经度和纬度，所以需要*2 + int targetSize = count * 2 * 4; + int[] targetBytes = new int[targetSize]; + //起始4个字节，机器人个数1个字节 + System.arraycopy(bytes, 5 + idsBytes.length, targetBytes, 0, targetSize); + regionMap.put("robotTargets", formatLanLat(targetBytes)); + + regionMap.put("pointCount", String.valueOf(bytes[5 + idsBytes.length + targetBytes.length])); + + int[] radiusBytes = new int[2]; + System.arraycopy(bytes, 5 + idsBytes.length + targetBytes.length + 1, radiusBytes, 0, 2); + regionMap.put("regionRadius", covertTargetDistance(radiusBytes)); + + int aByte = bytes[5 + idsBytes.length + targetBytes.length + 1 + radiusBytes.length]; + if (aByte == 0) { + regionMap.put("contour", "轮廓标志位:多边形，" + count + "个轮廓点"); + } else { + regionMap.put("contour", "轮廓标志位:圆形，" + 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); + regionMap.put("robotPoints", formatLanLat(pointBytes)); + return JSON.toJSONString(regionMap); + } + + /** + * 路径规划 + */ + public static String decodeRobotRoute(int[] bytes) { + 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 decodeDirectedTask(int[] bytes) { + 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 decodeTaskRoute(int[] bytes) { + 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 decodeDirectFlightTask(int[] bytes) { + 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 decodeResideTask(int[] bytes) { + 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 decodePutTask(int[] bytes) { + 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); + } + + /** + * 短信内容 + */ + public static String decodeSMS(int[] bytes) { + //帧头、帧长度、帧类型、子类型、机器人ID，CRC16校验码、帧尾总长度=8 + int dataLen = bytes.length - 8; + + int[] dataBytes = new int[dataLen]; + System.arraycopy(bytes, 5, dataBytes, 0, dataLen); + StringBuilder builder = new StringBuilder(); + for (int dataByte : dataBytes) { + builder.append((char) dataByte); + } + return builder.toString(); + } + + /** + * AIS数据请求 + */ + public static String decodeAISRequest(int[] bytes) { + 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); + } + + /** + * 方舱位置 + * 编码encode + * 解码decode + */ + public static String decodeShelterPosition(int[] bytes) { + 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) { + return ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF) + "hz"; + } + + /** + * 目标距离转换算法 + */ + private static String covertTargetDistance(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01) + "km"; + } + + /** + * 横滚角转换算法 + */ + private static float covertBuoyancy(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 100); + } + + /** + * 横滚角转换算法 + */ + private static float covertHeadingAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01); + } + + /** + * 横滚角转换算法 + */ + private static float covertRollAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 180); + } + + /** + * 俯仰角转换算法 + */ + private static float covertPitchAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 90); + } + + /** + * 航行状态转换 + */ + private static String covertState(int b) { + String state = ""; + switch (b) { + case 0: + state = "任务中"; + break; + case 1: + state = "待机"; + break; + case 2: + state = "故障"; + break; + case 3: + state = "未启用"; + break; + default: + break; + } + return state; + } + + /** + * 经纬度转换算法 + */ + private static double covertLngLat(int[] bytes) { + double per = ( + ((bytes[0] & 0xFF) << 24) + + ((bytes[1] & 0xFF) << 16) + + ((bytes[2] & 0xFF) << 8) + + (bytes[3] & 0xFF) + ) / (Math.pow(2, 31) - 1); + return 180 * per; + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java new file mode 100644 index 0000000..bc0e41e --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java @@ -0,0 +1,160 @@ +package com.casic.missiles.modular.system.utils; + +import com.alibaba.fastjson.JSON; +import com.alibaba.fastjson.TypeReference; +import com.casic.missiles.modular.system.dto.AisDTO; +import com.casic.missiles.modular.system.dto.EnvironmentDTO; +import com.casic.missiles.modular.system.dto.OceanDetailDTO; +import com.casic.missiles.modular.system.dto.remote.OceanDetailRemoteData; +import org.locationtech.spatial4j.distance.DistanceUtils; + +import java.text.DecimalFormat; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * 203所数据编码 + * + * @author a203 + */ +public class EncodeData { + /** + * 赤道半径(单位km) + */ + private static final double EARTH_RADIUS = 6371.393; + + /** + * 环境数据 + */ + public static Map encodeEnvironment(String data) { + Map resultMap = createDataHead("AIS"); + resultMap.put("type", "Env"); + resultMap.put("robotID", 0); + + EnvironmentDTO dto = new EnvironmentDTO(); + //台风中心经度 + double lng = 109.322222; + dto.setTyphoonLng(lng); + + //台风中心纬度 + double lat = 17.392232; + dto.setTyphoonLat(lat); + + //台风风力 + int power = 14; + dto.setTyphoonPower(power); + + //台风半径 + double radius = 155.56; + dto.setTyphoonRadius(radius); + + //机器人与台风中心距离 + double distance = distance(115.6314213, 12.9691386, 112.0623457, 19.9936680); + dto.setDistance(distance); + + //机器人与台风的方位角 + double angle = azimuth(115.6314213, 12.9691386, 112.0623457, 19.9936680); + dto.setAzimuth(angle); + + //洋流数据 + String oceanData = HttpRequestHelper.obtainOceanData(115.6314213, 12.9691386, "2021-10-22", "0", "10"); + OceanDetailDTO oceanDetailDTO = null; + if ("0".equals(HttpRequestHelper.getOceanCode(oceanData))) { + OceanDetailRemoteData remoteData = JSON.parseObject(oceanData, new TypeReference() { + }); + oceanDetailDTO = new OceanDetailDTO(remoteData); + } + dto.setOceanDetail(oceanDetailDTO); + + resultMap.put("data", dto); + return resultMap; + } + + /** + * AIS数据 + */ + public static Map encodeAIS(String data) { + Map resultMap = createDataHead("AIS"); + resultMap.put("type", "AIS"); + resultMap.put("robotID", 0); + //船只数量 + int shipCount = 2; + List positionBeans = new ArrayList<>(); + for (int i = 0; i < shipCount; i++) { + AisDTO ais = new AisDTO(); + ais.setCourse(187.2); + ais.setHeading(189.0); + //船只经度 + double lng = 109.322222; + ais.setLng(lng); + //船只纬度 + double lat = 17.392232; + ais.setLat(lat); + ais.setPositionTime("2021-09-29 16:12:25"); + ais.setSpeed(16.3); + + positionBeans.add(ais); + } + resultMap.put("data", positionBeans); + return resultMap; + } + + /** + * 短信数据 + */ + public static Map encodeSMS(String data) { + Map resultMap = createDataHead("SMS"); + resultMap.put("data", data); + return resultMap; + } + + private static Map createDataHead(String type) { + Map resultMap = new HashMap<>(8); + resultMap.put("code", 200); + resultMap.put("type", type); + resultMap.put("message", "请求成功"); + resultMap.put("success", true); + return resultMap; + } + + /** + * 经纬度转弧度 + */ + private static double lnglatToRadian(double d) { + return d * Math.PI / 180.0; + } + + /** + * 经纬度距离计算 + */ + private static double distance(double lonA, double latA, double lonB, double latB) { + double aLng = lnglatToRadian(lonA); + double aLat = lnglatToRadian(latA); + double bLng = lnglatToRadian(lonB); + double bLat = lnglatToRadian(latB); + + double dist = DistanceUtils.distHaversineRAD(aLat, aLng, bLat, bLng) * EARTH_RADIUS; + DecimalFormat df = new DecimalFormat("#.00"); + return Double.parseDouble(df.format(dist)); + } + + /** + * 方位角 + *

+ * https://blog.csdn.net/dulingwen/article/details/96868530 + */ + private static int azimuth(double lonA, double latA, double lonB, double latB) { + double deltaLng = lonB - lonA; + + double y = Math.sin(deltaLng) * Math.cos(latB); + double x = Math.cos(latA) * Math.sin(latB) - Math.sin(latA) * Math.cos(latB) * Math.cos(deltaLng); + + double bearing = Math.atan2(y, x); + bearing = Math.toDegrees(bearing); + bearing = (bearing + 360) % 360; + + return (int) bearing; + } +} diff --git a/casic-shelter/pom.xml b/casic-shelter/pom.xml index c1dc630..3c096f1 100644 --- a/casic-shelter/pom.xml +++ b/casic-shelter/pom.xml @@ -57,6 +57,10 @@ spatial4j 0.8 + + io.netty + netty-all + diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java index 4af70db..ea31852 100644 --- a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java @@ -6,31 +6,9 @@ * @author a203 */ public class Constant { - public static final int[] DATA = new int[]{ - 0x2A,//帧头 - 0x23,//帧长度 - 0x01,//帧类型 - 0x00,//子类型 - 0x03,//机器人ID - 0x4D, 0xBD, 0x80, 0xCF,//位置经度 - 0x0C, 0x5E, 0x28, 0xC2,//位置纬度 - 0x01, //航行状态 - 0xEB, //仪表电压 - 0xEF, //动力电压 - 0x5F, //仪表电余量 - 0x5D, //动力电余量 - 0x13, 0x5D, //俯仰角 - 0x40, 0x3D, //横滚角 - 0x30, 0xB2, //航向角 - 0x25, 0x4E, //浮力值 - 0x1B, 0x4E, //滑块位置 - 0x00, //探测状态 - 0x00, 0x9C, //目标距离 - 0x42, 0xBD, //目标方位 - 0x07, 0xD0, //目标频率 - 0xC7, 0x16, //CRC16校验码 - 0x0A //帧尾 - }; + + public static final int BITS_OF_HEAD = 0x2A; + public static final int BITS_OF_END = 0x0A; /** * 洋流相关常量 diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java deleted file mode 100644 index dc6ab54..0000000 --- a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java +++ /dev/null @@ -1,609 +0,0 @@ -package com.casic.missiles.modular.system.utils; - -import com.alibaba.fastjson.JSON; -import com.alibaba.fastjson.TypeReference; -import com.casic.missiles.modular.system.dto.AisDTO; -import com.casic.missiles.modular.system.dto.EnvironmentDTO; -import com.casic.missiles.modular.system.dto.OceanDetailDTO; -import com.casic.missiles.modular.system.dto.remote.OceanDetailRemoteData; -import com.casic.missiles.modular.system.model.RobotInfo; -import org.locationtech.spatial4j.distance.DistanceUtils; - -import java.text.DecimalFormat; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Map; - -/** - * 西工大数据解析协议 - * - * @author a203 - */ -public class DataParser { - - private static final int BITS_OF_HEAD = 0x2A; - private static final int BITS_OF_END = 0x0A; - //赤道半径(单位km) - private static final double EARTH_RADIUS = 6371.393; - - private static boolean isCorrectData(int[] bytes) { - if (bytes[0] != BITS_OF_HEAD) { - return false; - } - return bytes[bytes.length - 1] == BITS_OF_END; - } - - /** - * 水下机器人信息 - */ - public static RobotInfo decodeRobotInfo(int[] bytes) { - RobotInfo robot = new RobotInfo(); - if (!isCorrectData(bytes)) { - return robot; - } - robot.setRobotId(bytes[4]); - - int[] lngBytes = new int[4]; - System.arraycopy(bytes, 5, lngBytes, 0, 4); - robot.setLng(covertLngLat(lngBytes)); - - int[] latBytes = new int[4]; - System.arraycopy(bytes, 9, latBytes, 0, 4); - robot.setLat(covertLngLat(latBytes)); - - robot.setNavigateState(covertState(bytes[13])); - robot.setInstrumentV((float) (bytes[14] * 0.1)); - robot.setPowerV((float) bytes[15]); - robot.setInstrumentE((float) (bytes[16])); - robot.setPowerE((float) (bytes[17])); - - int[] pitchAngleBytes = new int[2]; - System.arraycopy(bytes, 18, pitchAngleBytes, 0, 2); - robot.setPitchAngle(covertPitchAngle(pitchAngleBytes)); - - int[] rollAngleBytes = new int[2]; - System.arraycopy(bytes, 20, rollAngleBytes, 0, 2); - robot.setRollAngle(covertRollAngle(rollAngleBytes)); - - int[] headingAngleBytes = new int[2]; - System.arraycopy(bytes, 22, headingAngleBytes, 0, 2); - robot.setHeadingAngle(covertHeadingAngle(headingAngleBytes)); - - int[] buoyancyBytes = new int[2]; - System.arraycopy(bytes, 24, buoyancyBytes, 0, 2); - robot.setBuoyancy(covertBuoyancy(buoyancyBytes)); - - int[] positionBytes = new int[2]; - System.arraycopy(bytes, 26, positionBytes, 0, 2); - //滑块位置算法和浮力算法一样 - robot.setPosition(covertBuoyancy(positionBytes)); - - int status = bytes[28]; - if (status == 0) { - robot.setDetectStatus("没有目标"); - } else { - robot.setDetectStatus("发现目标"); - } - - int[] targetDisBytes = new int[2]; - System.arraycopy(bytes, 29, targetDisBytes, 0, 2); - robot.setTargetDistance(covertTargetDistance(targetDisBytes)); - - int[] targetDirBytes = new int[2]; - System.arraycopy(bytes, 31, targetDirBytes, 0, 2); - //目标方位算法和航向角算法一样 - robot.setTargetDirection(covertHeadingAngle(targetDirBytes)); - - int[] targetHzBytes = new int[2]; - System.arraycopy(bytes, 33, targetHzBytes, 0, 2); - robot.setTargetHertz(covertTargetHertz(targetHzBytes)); - return robot; - } - - /** - * 区域覆盖 - */ - public static String decodeRobotRegion(int[] bytes) { - if (!isCorrectData(bytes)) { - return ""; - } - Map regionMap = new HashMap<>(); - int count = bytes[4]; - regionMap.put("count", String.valueOf(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]); - } - } - regionMap.put("robotIds", builder.toString()); - - //经度和纬度，所以需要*2 - int targetSize = count * 2 * 4; - int[] targetBytes = new int[targetSize]; - //起始4个字节，机器人个数1个字节 - System.arraycopy(bytes, 5 + idsBytes.length, targetBytes, 0, targetSize); - regionMap.put("robotTargets", formatLanLat(targetBytes)); - - regionMap.put("pointCount", String.valueOf(bytes[5 + idsBytes.length + targetBytes.length])); - - int[] radiusBytes = new int[2]; - System.arraycopy(bytes, 5 + idsBytes.length + targetBytes.length + 1, radiusBytes, 0, 2); - regionMap.put("regionRadius", covertTargetDistance(radiusBytes)); - - int aByte = bytes[5 + idsBytes.length + targetBytes.length + 1 + radiusBytes.length]; - if (aByte == 0) { - regionMap.put("contour", "轮廓标志位:多边形，" + count + "个轮廓点"); - } else { - regionMap.put("contour", "轮廓标志位:圆形，" + 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); - regionMap.put("robotPoints", formatLanLat(pointBytes)); - return JSON.toJSONString(regionMap); - } - - /** - * 路径规划 - */ - public static String decodeRobotRoute(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 decodeDirectedTask(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 decodeTaskRoute(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 decodeDirectFlightTask(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 decodeResideTask(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 decodePutTask(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); - } - - /** - * 短信内容 - */ - public static String decodeSMS(int[] bytes) { - //帧头、帧长度、帧类型、子类型、机器人ID，CRC16校验码、帧尾总长度=8 - int dataLen = bytes.length - 8; - - int[] dataBytes = new int[dataLen]; - System.arraycopy(bytes, 5, dataBytes, 0, dataLen); - StringBuilder builder = new StringBuilder(); - for (int dataByte : dataBytes) { - builder.append((char) dataByte); - } - return builder.toString(); - } - - /** - * AIS数据请求 - */ - public static String decodeAISRequest(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); - } - - /** - * 方舱位置 - * 编码encode - * 解码decode - */ - public static String decodeShelterPosition(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) { - return ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF) + "hz"; - } - - /** - * 目标距离转换算法 - */ - private static String covertTargetDistance(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01) + "km"; - } - - /** - * 横滚角转换算法 - */ - private static float covertBuoyancy(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 100); - } - - /** - * 横滚角转换算法 - */ - private static float covertHeadingAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01); - } - - /** - * 横滚角转换算法 - */ - private static float covertRollAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 180); - } - - /** - * 俯仰角转换算法 - */ - private static float covertPitchAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 90); - } - - /** - * 航行状态转换 - */ - private static String covertState(int b) { - String state = ""; - switch (b) { - case 0: - state = "任务中"; - break; - case 1: - state = "待机"; - break; - case 2: - state = "故障"; - break; - case 3: - state = "未启用"; - break; - default: - break; - } - return state; - } - - /** - * 经纬度转换算法 - */ - private static double covertLngLat(int[] bytes) { - double per = ( - ((bytes[0] & 0xFF) << 24) + - ((bytes[1] & 0xFF) << 16) + - ((bytes[2] & 0xFF) << 8) + - (bytes[3] & 0xFF) - ) / (Math.pow(2, 31) - 1); - return 180 * per; - } - - /** - * 环境数据 - */ - public static Map encodeEnvironment(String data) { - Map resultMap = createDataHead("AIS"); - resultMap.put("type", "Env"); - - EnvironmentDTO dto = new EnvironmentDTO(); - //台风中心经度 - double lng = 109.322222; - dto.setTyphoonLng(lng); - - //台风中心纬度 - double lat = 17.392232; - dto.setTyphoonLat(lat); - - //台风风力 - int power = 14; - dto.setTyphoonPower(power); - - //台风半径 - double radius = 155.56; - dto.setTyphoonRadius(radius); - - //机器人与台风中心距离 - double distance = distance(115.6314213, 12.9691386, 112.0623457, 19.9936680); - dto.setDistance(distance); - - //机器人与台风的方位角 - double angle = azimuth(115.6314213, 12.9691386, 112.0623457, 19.9936680); - dto.setAzimuth(angle); - - //洋流数据 - String oceanData = HttpRequestHelper.obtainOceanData(115.6314213, 12.9691386, "2021-10-22", "0", "10"); - OceanDetailDTO oceanDetailDTO = null; - if ("0".equals(HttpRequestHelper.getOceanCode(oceanData))) { - OceanDetailRemoteData remoteData = JSON.parseObject(oceanData, new TypeReference() { - }); - oceanDetailDTO = new OceanDetailDTO(remoteData); - } - dto.setOceanDetail(oceanDetailDTO); - - resultMap.put("data", dto); - return resultMap; - } - - /** - * AIS数据 - */ - public static Map encodeAIS(String data) { - Map resultMap = createDataHead("AIS"); - resultMap.put("type", "AIS"); - //船只数量 - int shipCount = 2; - List positionBeans = new ArrayList<>(); - for (int i = 0; i < shipCount; i++) { - AisDTO ais = new AisDTO(); - ais.setCourse(187.2); - ais.setHeading(189.0); - //船只经度 - double lng = 109.322222; - ais.setLng(lng); - //船只纬度 - double lat = 17.392232; - ais.setLat(lat); - ais.setPositionTime("2021-09-29 16:12:25"); - ais.setSpeed(16.3); - - positionBeans.add(ais); - } - resultMap.put("data", positionBeans); - return resultMap; - } - - /** - * 短信数据 - */ - public static Map encodeSMS(String data) { - Map resultMap = createDataHead("SMS"); - resultMap.put("data", data); - return resultMap; - } - - private static Map createDataHead(String type) { - Map resultMap = new HashMap<>(8); - resultMap.put("code", 200); - resultMap.put("type", type); - resultMap.put("message", "请求成功"); - resultMap.put("success", true); - return resultMap; - } - - /** - * 经纬度转弧度 - */ - private static double lnglatToRadian(double d) { - return d * Math.PI / 180.0; - } - - /** - * 经纬度距离计算 - */ - private static double distance(double lonA, double latA, double lonB, double latB) { - double aLng = lnglatToRadian(lonA); - double aLat = lnglatToRadian(latA); - double bLng = lnglatToRadian(lonB); - double bLat = lnglatToRadian(latB); - - double dist = DistanceUtils.distHaversineRAD(aLat, aLng, bLat, bLng) * EARTH_RADIUS; - DecimalFormat df = new DecimalFormat("#.00"); - return Double.parseDouble(df.format(dist)); - } - - /** - * 方位角 - *

- * https://blog.csdn.net/dulingwen/article/details/96868530 - */ - private static int azimuth(double lonA, double latA, double lonB, double latB) { - double deltaLng = lonB - lonA; - - double y = Math.sin(deltaLng) * Math.cos(latB); - double x = Math.cos(latA) * Math.sin(latB) - Math.sin(latA) * Math.cos(latB) * Math.cos(deltaLng); - - double bearing = Math.atan2(y, x); - bearing = Math.toDegrees(bearing); - bearing = (bearing + 360) % 360; - - return (int) bearing; - } -} \ No newline at end of file diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java new file mode 100644 index 0000000..9cb0e99 --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java @@ -0,0 +1,429 @@ +package com.casic.missiles.modular.system.utils; + +import com.alibaba.fastjson.JSON; +import com.casic.missiles.modular.system.model.RobotInfo; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * 西工大数据解析 + * + * @author a203 + */ +public class DecodeData { + /** + * 水下机器人信息 + */ + public static RobotInfo decodeRobotInfo(int[] bytes) { + RobotInfo robot = new RobotInfo(); + + robot.setRobotId(bytes[4]); + + int[] lngBytes = new int[4]; + System.arraycopy(bytes, 5, lngBytes, 0, 4); + robot.setLng(covertLngLat(lngBytes)); + + int[] latBytes = new int[4]; + System.arraycopy(bytes, 9, latBytes, 0, 4); + robot.setLat(covertLngLat(latBytes)); + + robot.setNavigateState(covertState(bytes[13])); + robot.setInstrumentV((float) (bytes[14] * 0.1)); + robot.setPowerV((float) bytes[15]); + robot.setInstrumentE((float) (bytes[16])); + robot.setPowerE((float) (bytes[17])); + + int[] pitchAngleBytes = new int[2]; + System.arraycopy(bytes, 18, pitchAngleBytes, 0, 2); + robot.setPitchAngle(covertPitchAngle(pitchAngleBytes)); + + int[] rollAngleBytes = new int[2]; + System.arraycopy(bytes, 20, rollAngleBytes, 0, 2); + robot.setRollAngle(covertRollAngle(rollAngleBytes)); + + int[] headingAngleBytes = new int[2]; + System.arraycopy(bytes, 22, headingAngleBytes, 0, 2); + robot.setHeadingAngle(covertHeadingAngle(headingAngleBytes)); + + int[] buoyancyBytes = new int[2]; + System.arraycopy(bytes, 24, buoyancyBytes, 0, 2); + robot.setBuoyancy(covertBuoyancy(buoyancyBytes)); + + int[] positionBytes = new int[2]; + System.arraycopy(bytes, 26, positionBytes, 0, 2); + //滑块位置算法和浮力算法一样 + robot.setPosition(covertBuoyancy(positionBytes)); + + int status = bytes[28]; + if (status == 0) { + robot.setDetectStatus("没有目标"); + } else { + robot.setDetectStatus("发现目标"); + } + + int[] targetDisBytes = new int[2]; + System.arraycopy(bytes, 29, targetDisBytes, 0, 2); + robot.setTargetDistance(covertTargetDistance(targetDisBytes)); + + int[] targetDirBytes = new int[2]; + System.arraycopy(bytes, 31, targetDirBytes, 0, 2); + //目标方位算法和航向角算法一样 + robot.setTargetDirection(covertHeadingAngle(targetDirBytes)); + + int[] targetHzBytes = new int[2]; + System.arraycopy(bytes, 33, targetHzBytes, 0, 2); + robot.setTargetHertz(covertTargetHertz(targetHzBytes)); + return robot; + } + + /** + * 区域覆盖 + */ + public static String decodeRobotRegion(int[] bytes) { + Map regionMap = new HashMap<>(); + int count = bytes[4]; + regionMap.put("count", String.valueOf(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]); + } + } + regionMap.put("robotIds", builder.toString()); + + //经度和纬度，所以需要*2 + int targetSize = count * 2 * 4; + int[] targetBytes = new int[targetSize]; + //起始4个字节，机器人个数1个字节 + System.arraycopy(bytes, 5 + idsBytes.length, targetBytes, 0, targetSize); + regionMap.put("robotTargets", formatLanLat(targetBytes)); + + regionMap.put("pointCount", String.valueOf(bytes[5 + idsBytes.length + targetBytes.length])); + + int[] radiusBytes = new int[2]; + System.arraycopy(bytes, 5 + idsBytes.length + targetBytes.length + 1, radiusBytes, 0, 2); + regionMap.put("regionRadius", covertTargetDistance(radiusBytes)); + + int aByte = bytes[5 + idsBytes.length + targetBytes.length + 1 + radiusBytes.length]; + if (aByte == 0) { + regionMap.put("contour", "轮廓标志位:多边形，" + count + "个轮廓点"); + } else { + regionMap.put("contour", "轮廓标志位:圆形，" + 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); + regionMap.put("robotPoints", formatLanLat(pointBytes)); + return JSON.toJSONString(regionMap); + } + + /** + * 路径规划 + */ + public static String decodeRobotRoute(int[] bytes) { + 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 decodeDirectedTask(int[] bytes) { + 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 decodeTaskRoute(int[] bytes) { + 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 decodeDirectFlightTask(int[] bytes) { + 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 decodeResideTask(int[] bytes) { + 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 decodePutTask(int[] bytes) { + 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); + } + + /** + * 短信内容 + */ + public static String decodeSMS(int[] bytes) { + //帧头、帧长度、帧类型、子类型、机器人ID，CRC16校验码、帧尾总长度=8 + int dataLen = bytes.length - 8; + + int[] dataBytes = new int[dataLen]; + System.arraycopy(bytes, 5, dataBytes, 0, dataLen); + StringBuilder builder = new StringBuilder(); + for (int dataByte : dataBytes) { + builder.append((char) dataByte); + } + return builder.toString(); + } + + /** + * AIS数据请求 + */ + public static String decodeAISRequest(int[] bytes) { + 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); + } + + /** + * 方舱位置 + * 编码encode + * 解码decode + */ + public static String decodeShelterPosition(int[] bytes) { + 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) { + return ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF) + "hz"; + } + + /** + * 目标距离转换算法 + */ + private static String covertTargetDistance(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01) + "km"; + } + + /** + * 横滚角转换算法 + */ + private static float covertBuoyancy(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 100); + } + + /** + * 横滚角转换算法 + */ + private static float covertHeadingAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01); + } + + /** + * 横滚角转换算法 + */ + private static float covertRollAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 180); + } + + /** + * 俯仰角转换算法 + */ + private static float covertPitchAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 90); + } + + /** + * 航行状态转换 + */ + private static String covertState(int b) { + String state = ""; + switch (b) { + case 0: + state = "任务中"; + break; + case 1: + state = "待机"; + break; + case 2: + state = "故障"; + break; + case 3: + state = "未启用"; + break; + default: + break; + } + return state; + } + + /** + * 经纬度转换算法 + */ + private static double covertLngLat(int[] bytes) { + double per = ( + ((bytes[0] & 0xFF) << 24) + + ((bytes[1] & 0xFF) << 16) + + ((bytes[2] & 0xFF) << 8) + + (bytes[3] & 0xFF) + ) / (Math.pow(2, 31) - 1); + return 180 * per; + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java new file mode 100644 index 0000000..bc0e41e --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java @@ -0,0 +1,160 @@ +package com.casic.missiles.modular.system.utils; + +import com.alibaba.fastjson.JSON; +import com.alibaba.fastjson.TypeReference; +import com.casic.missiles.modular.system.dto.AisDTO; +import com.casic.missiles.modular.system.dto.EnvironmentDTO; +import com.casic.missiles.modular.system.dto.OceanDetailDTO; +import com.casic.missiles.modular.system.dto.remote.OceanDetailRemoteData; +import org.locationtech.spatial4j.distance.DistanceUtils; + +import java.text.DecimalFormat; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * 203所数据编码 + * + * @author a203 + */ +public class EncodeData { + /** + * 赤道半径(单位km) + */ + private static final double EARTH_RADIUS = 6371.393; + + /** + * 环境数据 + */ + public static Map encodeEnvironment(String data) { + Map resultMap = createDataHead("AIS"); + resultMap.put("type", "Env"); + resultMap.put("robotID", 0); + + EnvironmentDTO dto = new EnvironmentDTO(); + //台风中心经度 + double lng = 109.322222; + dto.setTyphoonLng(lng); + + //台风中心纬度 + double lat = 17.392232; + dto.setTyphoonLat(lat); + + //台风风力 + int power = 14; + dto.setTyphoonPower(power); + + //台风半径 + double radius = 155.56; + dto.setTyphoonRadius(radius); + + //机器人与台风中心距离 + double distance = distance(115.6314213, 12.9691386, 112.0623457, 19.9936680); + dto.setDistance(distance); + + //机器人与台风的方位角 + double angle = azimuth(115.6314213, 12.9691386, 112.0623457, 19.9936680); + dto.setAzimuth(angle); + + //洋流数据 + String oceanData = HttpRequestHelper.obtainOceanData(115.6314213, 12.9691386, "2021-10-22", "0", "10"); + OceanDetailDTO oceanDetailDTO = null; + if ("0".equals(HttpRequestHelper.getOceanCode(oceanData))) { + OceanDetailRemoteData remoteData = JSON.parseObject(oceanData, new TypeReference() { + }); + oceanDetailDTO = new OceanDetailDTO(remoteData); + } + dto.setOceanDetail(oceanDetailDTO); + + resultMap.put("data", dto); + return resultMap; + } + + /** + * AIS数据 + */ + public static Map encodeAIS(String data) { + Map resultMap = createDataHead("AIS"); + resultMap.put("type", "AIS"); + resultMap.put("robotID", 0); + //船只数量 + int shipCount = 2; + List positionBeans = new ArrayList<>(); + for (int i = 0; i < shipCount; i++) { + AisDTO ais = new AisDTO(); + ais.setCourse(187.2); + ais.setHeading(189.0); + //船只经度 + double lng = 109.322222; + ais.setLng(lng); + //船只纬度 + double lat = 17.392232; + ais.setLat(lat); + ais.setPositionTime("2021-09-29 16:12:25"); + ais.setSpeed(16.3); + + positionBeans.add(ais); + } + resultMap.put("data", positionBeans); + return resultMap; + } + + /** + * 短信数据 + */ + public static Map encodeSMS(String data) { + Map resultMap = createDataHead("SMS"); + resultMap.put("data", data); + return resultMap; + } + + private static Map createDataHead(String type) { + Map resultMap = new HashMap<>(8); + resultMap.put("code", 200); + resultMap.put("type", type); + resultMap.put("message", "请求成功"); + resultMap.put("success", true); + return resultMap; + } + + /** + * 经纬度转弧度 + */ + private static double lnglatToRadian(double d) { + return d * Math.PI / 180.0; + } + + /** + * 经纬度距离计算 + */ + private static double distance(double lonA, double latA, double lonB, double latB) { + double aLng = lnglatToRadian(lonA); + double aLat = lnglatToRadian(latA); + double bLng = lnglatToRadian(lonB); + double bLat = lnglatToRadian(latB); + + double dist = DistanceUtils.distHaversineRAD(aLat, aLng, bLat, bLng) * EARTH_RADIUS; + DecimalFormat df = new DecimalFormat("#.00"); + return Double.parseDouble(df.format(dist)); + } + + /** + * 方位角 + *

+ * https://blog.csdn.net/dulingwen/article/details/96868530 + */ + private static int azimuth(double lonA, double latA, double lonB, double latB) { + double deltaLng = lonB - lonA; + + double y = Math.sin(deltaLng) * Math.cos(latB); + double x = Math.cos(latA) * Math.sin(latB) - Math.sin(latA) * Math.cos(latB) * Math.cos(deltaLng); + + double bearing = Math.atan2(y, x); + bearing = Math.toDegrees(bearing); + bearing = (bearing + 360) % 360; + + return (int) bearing; + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/socket/BootNettyServer.java b/casic-shelter/src/main/java/com/casic/missiles/socket/BootNettyServer.java new file mode 100644 index 0000000..02f5ba2 --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/socket/BootNettyServer.java @@ -0,0 +1,59 @@ +package com.casic.missiles.socket; + +import io.netty.bootstrap.ServerBootstrap; +import io.netty.channel.AdaptiveRecvByteBufAllocator; +import io.netty.channel.ChannelFuture; +import io.netty.channel.ChannelOption; +import io.netty.channel.EventLoopGroup; +import io.netty.channel.nio.NioEventLoopGroup; +import io.netty.channel.socket.SocketChannel; +import io.netty.channel.socket.nio.NioServerSocketChannel; +import lombok.extern.slf4j.Slf4j; + +/** + * @author a203 + */ +@Slf4j +public class BootNettyServer { + public void bind(int port) { + EventLoopGroup bossGroup = new NioEventLoopGroup(1); + EventLoopGroup workerGroup = new NioEventLoopGroup(); + try { + /** + * ServerBootstrap 是一个启动NIO服务的辅助启动类 + */ + ServerBootstrap serverBootstrap = new ServerBootstrap(); + /** + * 设置group，将bossGroup，workerGroup线程组传递到ServerBootstrap + */ + serverBootstrap = serverBootstrap.group(bossGroup, workerGroup); + /** + * ServerSocketChannel是以NIO的selector为基础进行实现的，用来接收新的连接，这里告诉Channel通过NioServerSocketChannel获取新的连接 + */ + serverBootstrap = serverBootstrap.channel(NioServerSocketChannel.class); + serverBootstrap = serverBootstrap.option(ChannelOption.RCVBUF_ALLOCATOR, new AdaptiveRecvByteBufAllocator(64, 10496, 1048576)); + serverBootstrap = serverBootstrap.childOption(ChannelOption.RCVBUF_ALLOCATOR, new AdaptiveRecvByteBufAllocator(64, 10496, 1048576)); + /** + * 设置 I/O处理类,主要用于网络I/O事件，记录日志，编码、解码消息 + */ + serverBootstrap = serverBootstrap.childHandler(new ChannelInitializer()); + log.info("端口已开启，占用" + port + "端口号...."); + /** + * 绑定端口，同步等待成功 + */ + ChannelFuture channelFuture = serverBootstrap.bind(port).sync(); + /** + * 等待服务器监听端口关闭 + */ + channelFuture.channel().closeFuture().sync(); + } catch (InterruptedException e) { + e.printStackTrace(); + } finally { + /** + * 退出，释放线程池资源 + */ + bossGroup.shutdownGracefully(); + workerGroup.shutdownGracefully(); + } + } +} diff --git a/casic-shelter/pom.xml b/casic-shelter/pom.xml index c1dc630..3c096f1 100644 --- a/casic-shelter/pom.xml +++ b/casic-shelter/pom.xml @@ -57,6 +57,10 @@ spatial4j 0.8 + + io.netty + netty-all + diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java index 4af70db..ea31852 100644 --- a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java @@ -6,31 +6,9 @@ * @author a203 */ public class Constant { - public static final int[] DATA = new int[]{ - 0x2A,//帧头 - 0x23,//帧长度 - 0x01,//帧类型 - 0x00,//子类型 - 0x03,//机器人ID - 0x4D, 0xBD, 0x80, 0xCF,//位置经度 - 0x0C, 0x5E, 0x28, 0xC2,//位置纬度 - 0x01, //航行状态 - 0xEB, //仪表电压 - 0xEF, //动力电压 - 0x5F, //仪表电余量 - 0x5D, //动力电余量 - 0x13, 0x5D, //俯仰角 - 0x40, 0x3D, //横滚角 - 0x30, 0xB2, //航向角 - 0x25, 0x4E, //浮力值 - 0x1B, 0x4E, //滑块位置 - 0x00, //探测状态 - 0x00, 0x9C, //目标距离 - 0x42, 0xBD, //目标方位 - 0x07, 0xD0, //目标频率 - 0xC7, 0x16, //CRC16校验码 - 0x0A //帧尾 - }; + + public static final int BITS_OF_HEAD = 0x2A; + public static final int BITS_OF_END = 0x0A; /** * 洋流相关常量 diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java deleted file mode 100644 index dc6ab54..0000000 --- a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java +++ /dev/null @@ -1,609 +0,0 @@ -package com.casic.missiles.modular.system.utils; - -import com.alibaba.fastjson.JSON; -import com.alibaba.fastjson.TypeReference; -import com.casic.missiles.modular.system.dto.AisDTO; -import com.casic.missiles.modular.system.dto.EnvironmentDTO; -import com.casic.missiles.modular.system.dto.OceanDetailDTO; -import com.casic.missiles.modular.system.dto.remote.OceanDetailRemoteData; -import com.casic.missiles.modular.system.model.RobotInfo; -import org.locationtech.spatial4j.distance.DistanceUtils; - -import java.text.DecimalFormat; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Map; - -/** - * 西工大数据解析协议 - * - * @author a203 - */ -public class DataParser { - - private static final int BITS_OF_HEAD = 0x2A; - private static final int BITS_OF_END = 0x0A; - //赤道半径(单位km) - private static final double EARTH_RADIUS = 6371.393; - - private static boolean isCorrectData(int[] bytes) { - if (bytes[0] != BITS_OF_HEAD) { - return false; - } - return bytes[bytes.length - 1] == BITS_OF_END; - } - - /** - * 水下机器人信息 - */ - public static RobotInfo decodeRobotInfo(int[] bytes) { - RobotInfo robot = new RobotInfo(); - if (!isCorrectData(bytes)) { - return robot; - } - robot.setRobotId(bytes[4]); - - int[] lngBytes = new int[4]; - System.arraycopy(bytes, 5, lngBytes, 0, 4); - robot.setLng(covertLngLat(lngBytes)); - - int[] latBytes = new int[4]; - System.arraycopy(bytes, 9, latBytes, 0, 4); - robot.setLat(covertLngLat(latBytes)); - - robot.setNavigateState(covertState(bytes[13])); - robot.setInstrumentV((float) (bytes[14] * 0.1)); - robot.setPowerV((float) bytes[15]); - robot.setInstrumentE((float) (bytes[16])); - robot.setPowerE((float) (bytes[17])); - - int[] pitchAngleBytes = new int[2]; - System.arraycopy(bytes, 18, pitchAngleBytes, 0, 2); - robot.setPitchAngle(covertPitchAngle(pitchAngleBytes)); - - int[] rollAngleBytes = new int[2]; - System.arraycopy(bytes, 20, rollAngleBytes, 0, 2); - robot.setRollAngle(covertRollAngle(rollAngleBytes)); - - int[] headingAngleBytes = new int[2]; - System.arraycopy(bytes, 22, headingAngleBytes, 0, 2); - robot.setHeadingAngle(covertHeadingAngle(headingAngleBytes)); - - int[] buoyancyBytes = new int[2]; - System.arraycopy(bytes, 24, buoyancyBytes, 0, 2); - robot.setBuoyancy(covertBuoyancy(buoyancyBytes)); - - int[] positionBytes = new int[2]; - System.arraycopy(bytes, 26, positionBytes, 0, 2); - //滑块位置算法和浮力算法一样 - robot.setPosition(covertBuoyancy(positionBytes)); - - int status = bytes[28]; - if (status == 0) { - robot.setDetectStatus("没有目标"); - } else { - robot.setDetectStatus("发现目标"); - } - - int[] targetDisBytes = new int[2]; - System.arraycopy(bytes, 29, targetDisBytes, 0, 2); - robot.setTargetDistance(covertTargetDistance(targetDisBytes)); - - int[] targetDirBytes = new int[2]; - System.arraycopy(bytes, 31, targetDirBytes, 0, 2); - //目标方位算法和航向角算法一样 - robot.setTargetDirection(covertHeadingAngle(targetDirBytes)); - - int[] targetHzBytes = new int[2]; - System.arraycopy(bytes, 33, targetHzBytes, 0, 2); - robot.setTargetHertz(covertTargetHertz(targetHzBytes)); - return robot; - } - - /** - * 区域覆盖 - */ - public static String decodeRobotRegion(int[] bytes) { - if (!isCorrectData(bytes)) { - return ""; - } - Map regionMap = new HashMap<>(); - int count = bytes[4]; - regionMap.put("count", String.valueOf(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]); - } - } - regionMap.put("robotIds", builder.toString()); - - //经度和纬度，所以需要*2 - int targetSize = count * 2 * 4; - int[] targetBytes = new int[targetSize]; - //起始4个字节，机器人个数1个字节 - System.arraycopy(bytes, 5 + idsBytes.length, targetBytes, 0, targetSize); - regionMap.put("robotTargets", formatLanLat(targetBytes)); - - regionMap.put("pointCount", String.valueOf(bytes[5 + idsBytes.length + targetBytes.length])); - - int[] radiusBytes = new int[2]; - System.arraycopy(bytes, 5 + idsBytes.length + targetBytes.length + 1, radiusBytes, 0, 2); - regionMap.put("regionRadius", covertTargetDistance(radiusBytes)); - - int aByte = bytes[5 + idsBytes.length + targetBytes.length + 1 + radiusBytes.length]; - if (aByte == 0) { - regionMap.put("contour", "轮廓标志位:多边形，" + count + "个轮廓点"); - } else { - regionMap.put("contour", "轮廓标志位:圆形，" + 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); - regionMap.put("robotPoints", formatLanLat(pointBytes)); - return JSON.toJSONString(regionMap); - } - - /** - * 路径规划 - */ - public static String decodeRobotRoute(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 decodeDirectedTask(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 decodeTaskRoute(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 decodeDirectFlightTask(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 decodeResideTask(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 decodePutTask(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); - } - - /** - * 短信内容 - */ - public static String decodeSMS(int[] bytes) { - //帧头、帧长度、帧类型、子类型、机器人ID，CRC16校验码、帧尾总长度=8 - int dataLen = bytes.length - 8; - - int[] dataBytes = new int[dataLen]; - System.arraycopy(bytes, 5, dataBytes, 0, dataLen); - StringBuilder builder = new StringBuilder(); - for (int dataByte : dataBytes) { - builder.append((char) dataByte); - } - return builder.toString(); - } - - /** - * AIS数据请求 - */ - public static String decodeAISRequest(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); - } - - /** - * 方舱位置 - * 编码encode - * 解码decode - */ - public static String decodeShelterPosition(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) { - return ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF) + "hz"; - } - - /** - * 目标距离转换算法 - */ - private static String covertTargetDistance(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01) + "km"; - } - - /** - * 横滚角转换算法 - */ - private static float covertBuoyancy(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 100); - } - - /** - * 横滚角转换算法 - */ - private static float covertHeadingAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01); - } - - /** - * 横滚角转换算法 - */ - private static float covertRollAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 180); - } - - /** - * 俯仰角转换算法 - */ - private static float covertPitchAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 90); - } - - /** - * 航行状态转换 - */ - private static String covertState(int b) { - String state = ""; - switch (b) { - case 0: - state = "任务中"; - break; - case 1: - state = "待机"; - break; - case 2: - state = "故障"; - break; - case 3: - state = "未启用"; - break; - default: - break; - } - return state; - } - - /** - * 经纬度转换算法 - */ - private static double covertLngLat(int[] bytes) { - double per = ( - ((bytes[0] & 0xFF) << 24) + - ((bytes[1] & 0xFF) << 16) + - ((bytes[2] & 0xFF) << 8) + - (bytes[3] & 0xFF) - ) / (Math.pow(2, 31) - 1); - return 180 * per; - } - - /** - * 环境数据 - */ - public static Map encodeEnvironment(String data) { - Map resultMap = createDataHead("AIS"); - resultMap.put("type", "Env"); - - EnvironmentDTO dto = new EnvironmentDTO(); - //台风中心经度 - double lng = 109.322222; - dto.setTyphoonLng(lng); - - //台风中心纬度 - double lat = 17.392232; - dto.setTyphoonLat(lat); - - //台风风力 - int power = 14; - dto.setTyphoonPower(power); - - //台风半径 - double radius = 155.56; - dto.setTyphoonRadius(radius); - - //机器人与台风中心距离 - double distance = distance(115.6314213, 12.9691386, 112.0623457, 19.9936680); - dto.setDistance(distance); - - //机器人与台风的方位角 - double angle = azimuth(115.6314213, 12.9691386, 112.0623457, 19.9936680); - dto.setAzimuth(angle); - - //洋流数据 - String oceanData = HttpRequestHelper.obtainOceanData(115.6314213, 12.9691386, "2021-10-22", "0", "10"); - OceanDetailDTO oceanDetailDTO = null; - if ("0".equals(HttpRequestHelper.getOceanCode(oceanData))) { - OceanDetailRemoteData remoteData = JSON.parseObject(oceanData, new TypeReference() { - }); - oceanDetailDTO = new OceanDetailDTO(remoteData); - } - dto.setOceanDetail(oceanDetailDTO); - - resultMap.put("data", dto); - return resultMap; - } - - /** - * AIS数据 - */ - public static Map encodeAIS(String data) { - Map resultMap = createDataHead("AIS"); - resultMap.put("type", "AIS"); - //船只数量 - int shipCount = 2; - List positionBeans = new ArrayList<>(); - for (int i = 0; i < shipCount; i++) { - AisDTO ais = new AisDTO(); - ais.setCourse(187.2); - ais.setHeading(189.0); - //船只经度 - double lng = 109.322222; - ais.setLng(lng); - //船只纬度 - double lat = 17.392232; - ais.setLat(lat); - ais.setPositionTime("2021-09-29 16:12:25"); - ais.setSpeed(16.3); - - positionBeans.add(ais); - } - resultMap.put("data", positionBeans); - return resultMap; - } - - /** - * 短信数据 - */ - public static Map encodeSMS(String data) { - Map resultMap = createDataHead("SMS"); - resultMap.put("data", data); - return resultMap; - } - - private static Map createDataHead(String type) { - Map resultMap = new HashMap<>(8); - resultMap.put("code", 200); - resultMap.put("type", type); - resultMap.put("message", "请求成功"); - resultMap.put("success", true); - return resultMap; - } - - /** - * 经纬度转弧度 - */ - private static double lnglatToRadian(double d) { - return d * Math.PI / 180.0; - } - - /** - * 经纬度距离计算 - */ - private static double distance(double lonA, double latA, double lonB, double latB) { - double aLng = lnglatToRadian(lonA); - double aLat = lnglatToRadian(latA); - double bLng = lnglatToRadian(lonB); - double bLat = lnglatToRadian(latB); - - double dist = DistanceUtils.distHaversineRAD(aLat, aLng, bLat, bLng) * EARTH_RADIUS; - DecimalFormat df = new DecimalFormat("#.00"); - return Double.parseDouble(df.format(dist)); - } - - /** - * 方位角 - *

- * https://blog.csdn.net/dulingwen/article/details/96868530 - */ - private static int azimuth(double lonA, double latA, double lonB, double latB) { - double deltaLng = lonB - lonA; - - double y = Math.sin(deltaLng) * Math.cos(latB); - double x = Math.cos(latA) * Math.sin(latB) - Math.sin(latA) * Math.cos(latB) * Math.cos(deltaLng); - - double bearing = Math.atan2(y, x); - bearing = Math.toDegrees(bearing); - bearing = (bearing + 360) % 360; - - return (int) bearing; - } -} \ No newline at end of file diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java new file mode 100644 index 0000000..9cb0e99 --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java @@ -0,0 +1,429 @@ +package com.casic.missiles.modular.system.utils; + +import com.alibaba.fastjson.JSON; +import com.casic.missiles.modular.system.model.RobotInfo; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * 西工大数据解析 + * + * @author a203 + */ +public class DecodeData { + /** + * 水下机器人信息 + */ + public static RobotInfo decodeRobotInfo(int[] bytes) { + RobotInfo robot = new RobotInfo(); + + robot.setRobotId(bytes[4]); + + int[] lngBytes = new int[4]; + System.arraycopy(bytes, 5, lngBytes, 0, 4); + robot.setLng(covertLngLat(lngBytes)); + + int[] latBytes = new int[4]; + System.arraycopy(bytes, 9, latBytes, 0, 4); + robot.setLat(covertLngLat(latBytes)); + + robot.setNavigateState(covertState(bytes[13])); + robot.setInstrumentV((float) (bytes[14] * 0.1)); + robot.setPowerV((float) bytes[15]); + robot.setInstrumentE((float) (bytes[16])); + robot.setPowerE((float) (bytes[17])); + + int[] pitchAngleBytes = new int[2]; + System.arraycopy(bytes, 18, pitchAngleBytes, 0, 2); + robot.setPitchAngle(covertPitchAngle(pitchAngleBytes)); + + int[] rollAngleBytes = new int[2]; + System.arraycopy(bytes, 20, rollAngleBytes, 0, 2); + robot.setRollAngle(covertRollAngle(rollAngleBytes)); + + int[] headingAngleBytes = new int[2]; + System.arraycopy(bytes, 22, headingAngleBytes, 0, 2); + robot.setHeadingAngle(covertHeadingAngle(headingAngleBytes)); + + int[] buoyancyBytes = new int[2]; + System.arraycopy(bytes, 24, buoyancyBytes, 0, 2); + robot.setBuoyancy(covertBuoyancy(buoyancyBytes)); + + int[] positionBytes = new int[2]; + System.arraycopy(bytes, 26, positionBytes, 0, 2); + //滑块位置算法和浮力算法一样 + robot.setPosition(covertBuoyancy(positionBytes)); + + int status = bytes[28]; + if (status == 0) { + robot.setDetectStatus("没有目标"); + } else { + robot.setDetectStatus("发现目标"); + } + + int[] targetDisBytes = new int[2]; + System.arraycopy(bytes, 29, targetDisBytes, 0, 2); + robot.setTargetDistance(covertTargetDistance(targetDisBytes)); + + int[] targetDirBytes = new int[2]; + System.arraycopy(bytes, 31, targetDirBytes, 0, 2); + //目标方位算法和航向角算法一样 + robot.setTargetDirection(covertHeadingAngle(targetDirBytes)); + + int[] targetHzBytes = new int[2]; + System.arraycopy(bytes, 33, targetHzBytes, 0, 2); + robot.setTargetHertz(covertTargetHertz(targetHzBytes)); + return robot; + } + + /** + * 区域覆盖 + */ + public static String decodeRobotRegion(int[] bytes) { + Map regionMap = new HashMap<>(); + int count = bytes[4]; + regionMap.put("count", String.valueOf(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]); + } + } + regionMap.put("robotIds", builder.toString()); + + //经度和纬度，所以需要*2 + int targetSize = count * 2 * 4; + int[] targetBytes = new int[targetSize]; + //起始4个字节，机器人个数1个字节 + System.arraycopy(bytes, 5 + idsBytes.length, targetBytes, 0, targetSize); + regionMap.put("robotTargets", formatLanLat(targetBytes)); + + regionMap.put("pointCount", String.valueOf(bytes[5 + idsBytes.length + targetBytes.length])); + + int[] radiusBytes = new int[2]; + System.arraycopy(bytes, 5 + idsBytes.length + targetBytes.length + 1, radiusBytes, 0, 2); + regionMap.put("regionRadius", covertTargetDistance(radiusBytes)); + + int aByte = bytes[5 + idsBytes.length + targetBytes.length + 1 + radiusBytes.length]; + if (aByte == 0) { + regionMap.put("contour", "轮廓标志位:多边形，" + count + "个轮廓点"); + } else { + regionMap.put("contour", "轮廓标志位:圆形，" + 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); + regionMap.put("robotPoints", formatLanLat(pointBytes)); + return JSON.toJSONString(regionMap); + } + + /** + * 路径规划 + */ + public static String decodeRobotRoute(int[] bytes) { + 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 decodeDirectedTask(int[] bytes) { + 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 decodeTaskRoute(int[] bytes) { + 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 decodeDirectFlightTask(int[] bytes) { + 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 decodeResideTask(int[] bytes) { + 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 decodePutTask(int[] bytes) { + 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); + } + + /** + * 短信内容 + */ + public static String decodeSMS(int[] bytes) { + //帧头、帧长度、帧类型、子类型、机器人ID，CRC16校验码、帧尾总长度=8 + int dataLen = bytes.length - 8; + + int[] dataBytes = new int[dataLen]; + System.arraycopy(bytes, 5, dataBytes, 0, dataLen); + StringBuilder builder = new StringBuilder(); + for (int dataByte : dataBytes) { + builder.append((char) dataByte); + } + return builder.toString(); + } + + /** + * AIS数据请求 + */ + public static String decodeAISRequest(int[] bytes) { + 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); + } + + /** + * 方舱位置 + * 编码encode + * 解码decode + */ + public static String decodeShelterPosition(int[] bytes) { + 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) { + return ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF) + "hz"; + } + + /** + * 目标距离转换算法 + */ + private static String covertTargetDistance(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01) + "km"; + } + + /** + * 横滚角转换算法 + */ + private static float covertBuoyancy(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 100); + } + + /** + * 横滚角转换算法 + */ + private static float covertHeadingAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01); + } + + /** + * 横滚角转换算法 + */ + private static float covertRollAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 180); + } + + /** + * 俯仰角转换算法 + */ + private static float covertPitchAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 90); + } + + /** + * 航行状态转换 + */ + private static String covertState(int b) { + String state = ""; + switch (b) { + case 0: + state = "任务中"; + break; + case 1: + state = "待机"; + break; + case 2: + state = "故障"; + break; + case 3: + state = "未启用"; + break; + default: + break; + } + return state; + } + + /** + * 经纬度转换算法 + */ + private static double covertLngLat(int[] bytes) { + double per = ( + ((bytes[0] & 0xFF) << 24) + + ((bytes[1] & 0xFF) << 16) + + ((bytes[2] & 0xFF) << 8) + + (bytes[3] & 0xFF) + ) / (Math.pow(2, 31) - 1); + return 180 * per; + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java new file mode 100644 index 0000000..bc0e41e --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java @@ -0,0 +1,160 @@ +package com.casic.missiles.modular.system.utils; + +import com.alibaba.fastjson.JSON; +import com.alibaba.fastjson.TypeReference; +import com.casic.missiles.modular.system.dto.AisDTO; +import com.casic.missiles.modular.system.dto.EnvironmentDTO; +import com.casic.missiles.modular.system.dto.OceanDetailDTO; +import com.casic.missiles.modular.system.dto.remote.OceanDetailRemoteData; +import org.locationtech.spatial4j.distance.DistanceUtils; + +import java.text.DecimalFormat; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * 203所数据编码 + * + * @author a203 + */ +public class EncodeData { + /** + * 赤道半径(单位km) + */ + private static final double EARTH_RADIUS = 6371.393; + + /** + * 环境数据 + */ + public static Map encodeEnvironment(String data) { + Map resultMap = createDataHead("AIS"); + resultMap.put("type", "Env"); + resultMap.put("robotID", 0); + + EnvironmentDTO dto = new EnvironmentDTO(); + //台风中心经度 + double lng = 109.322222; + dto.setTyphoonLng(lng); + + //台风中心纬度 + double lat = 17.392232; + dto.setTyphoonLat(lat); + + //台风风力 + int power = 14; + dto.setTyphoonPower(power); + + //台风半径 + double radius = 155.56; + dto.setTyphoonRadius(radius); + + //机器人与台风中心距离 + double distance = distance(115.6314213, 12.9691386, 112.0623457, 19.9936680); + dto.setDistance(distance); + + //机器人与台风的方位角 + double angle = azimuth(115.6314213, 12.9691386, 112.0623457, 19.9936680); + dto.setAzimuth(angle); + + //洋流数据 + String oceanData = HttpRequestHelper.obtainOceanData(115.6314213, 12.9691386, "2021-10-22", "0", "10"); + OceanDetailDTO oceanDetailDTO = null; + if ("0".equals(HttpRequestHelper.getOceanCode(oceanData))) { + OceanDetailRemoteData remoteData = JSON.parseObject(oceanData, new TypeReference() { + }); + oceanDetailDTO = new OceanDetailDTO(remoteData); + } + dto.setOceanDetail(oceanDetailDTO); + + resultMap.put("data", dto); + return resultMap; + } + + /** + * AIS数据 + */ + public static Map encodeAIS(String data) { + Map resultMap = createDataHead("AIS"); + resultMap.put("type", "AIS"); + resultMap.put("robotID", 0); + //船只数量 + int shipCount = 2; + List positionBeans = new ArrayList<>(); + for (int i = 0; i < shipCount; i++) { + AisDTO ais = new AisDTO(); + ais.setCourse(187.2); + ais.setHeading(189.0); + //船只经度 + double lng = 109.322222; + ais.setLng(lng); + //船只纬度 + double lat = 17.392232; + ais.setLat(lat); + ais.setPositionTime("2021-09-29 16:12:25"); + ais.setSpeed(16.3); + + positionBeans.add(ais); + } + resultMap.put("data", positionBeans); + return resultMap; + } + + /** + * 短信数据 + */ + public static Map encodeSMS(String data) { + Map resultMap = createDataHead("SMS"); + resultMap.put("data", data); + return resultMap; + } + + private static Map createDataHead(String type) { + Map resultMap = new HashMap<>(8); + resultMap.put("code", 200); + resultMap.put("type", type); + resultMap.put("message", "请求成功"); + resultMap.put("success", true); + return resultMap; + } + + /** + * 经纬度转弧度 + */ + private static double lnglatToRadian(double d) { + return d * Math.PI / 180.0; + } + + /** + * 经纬度距离计算 + */ + private static double distance(double lonA, double latA, double lonB, double latB) { + double aLng = lnglatToRadian(lonA); + double aLat = lnglatToRadian(latA); + double bLng = lnglatToRadian(lonB); + double bLat = lnglatToRadian(latB); + + double dist = DistanceUtils.distHaversineRAD(aLat, aLng, bLat, bLng) * EARTH_RADIUS; + DecimalFormat df = new DecimalFormat("#.00"); + return Double.parseDouble(df.format(dist)); + } + + /** + * 方位角 + *

+ * https://blog.csdn.net/dulingwen/article/details/96868530 + */ + private static int azimuth(double lonA, double latA, double lonB, double latB) { + double deltaLng = lonB - lonA; + + double y = Math.sin(deltaLng) * Math.cos(latB); + double x = Math.cos(latA) * Math.sin(latB) - Math.sin(latA) * Math.cos(latB) * Math.cos(deltaLng); + + double bearing = Math.atan2(y, x); + bearing = Math.toDegrees(bearing); + bearing = (bearing + 360) % 360; + + return (int) bearing; + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/socket/BootNettyServer.java b/casic-shelter/src/main/java/com/casic/missiles/socket/BootNettyServer.java new file mode 100644 index 0000000..02f5ba2 --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/socket/BootNettyServer.java @@ -0,0 +1,59 @@ +package com.casic.missiles.socket; + +import io.netty.bootstrap.ServerBootstrap; +import io.netty.channel.AdaptiveRecvByteBufAllocator; +import io.netty.channel.ChannelFuture; +import io.netty.channel.ChannelOption; +import io.netty.channel.EventLoopGroup; +import io.netty.channel.nio.NioEventLoopGroup; +import io.netty.channel.socket.SocketChannel; +import io.netty.channel.socket.nio.NioServerSocketChannel; +import lombok.extern.slf4j.Slf4j; + +/** + * @author a203 + */ +@Slf4j +public class BootNettyServer { + public void bind(int port) { + EventLoopGroup bossGroup = new NioEventLoopGroup(1); + EventLoopGroup workerGroup = new NioEventLoopGroup(); + try { + /** + * ServerBootstrap 是一个启动NIO服务的辅助启动类 + */ + ServerBootstrap serverBootstrap = new ServerBootstrap(); + /** + * 设置group，将bossGroup，workerGroup线程组传递到ServerBootstrap + */ + serverBootstrap = serverBootstrap.group(bossGroup, workerGroup); + /** + * ServerSocketChannel是以NIO的selector为基础进行实现的，用来接收新的连接，这里告诉Channel通过NioServerSocketChannel获取新的连接 + */ + serverBootstrap = serverBootstrap.channel(NioServerSocketChannel.class); + serverBootstrap = serverBootstrap.option(ChannelOption.RCVBUF_ALLOCATOR, new AdaptiveRecvByteBufAllocator(64, 10496, 1048576)); + serverBootstrap = serverBootstrap.childOption(ChannelOption.RCVBUF_ALLOCATOR, new AdaptiveRecvByteBufAllocator(64, 10496, 1048576)); + /** + * 设置 I/O处理类,主要用于网络I/O事件，记录日志，编码、解码消息 + */ + serverBootstrap = serverBootstrap.childHandler(new ChannelInitializer()); + log.info("端口已开启，占用" + port + "端口号...."); + /** + * 绑定端口，同步等待成功 + */ + ChannelFuture channelFuture = serverBootstrap.bind(port).sync(); + /** + * 等待服务器监听端口关闭 + */ + channelFuture.channel().closeFuture().sync(); + } catch (InterruptedException e) { + e.printStackTrace(); + } finally { + /** + * 退出，释放线程池资源 + */ + bossGroup.shutdownGracefully(); + workerGroup.shutdownGracefully(); + } + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelHandlerAdapter.java b/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelHandlerAdapter.java new file mode 100644 index 0000000..7aa4a6a --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelHandlerAdapter.java @@ -0,0 +1,119 @@ +package com.casic.missiles.socket; + +import com.casic.missiles.modular.system.utils.Constant; +import com.casic.missiles.modular.system.utils.DecodeData; +import com.casic.missiles.modular.system.utils.EncodeData; +import io.netty.channel.ChannelHandlerContext; +import io.netty.channel.ChannelInboundHandlerAdapter; +import lombok.extern.slf4j.Slf4j; + +import java.net.InetSocketAddress; + +/** + * @author a203 + */ +@Slf4j +public class ChannelHandlerAdapter extends ChannelInboundHandlerAdapter { + + public static boolean isCorrectData(int[] bytes) { + if (bytes[0] != Constant.BITS_OF_HEAD) { + return false; + } + return bytes[bytes.length - 1] == Constant.BITS_OF_END; + } + + @Override + public void channelRead(ChannelHandlerContext ctx, Object msg) { + String temp = msg.toString(); + log.info("channelRead message ===> " + temp); + /** + * 回应客户端 + * */ + String[] dataArray = temp.split(" "); + int[] data = new int[dataArray.length]; + for (int i = 0; i < dataArray.length; i++) { + data[i] = Integer.parseInt(dataArray[i], 16); + } + if (isCorrectData(data)) { + int dataType = data[2]; + /** + * 先解析再回应 + * */ + switch (dataType) { + case 0x01: + //水下机器人信息 + DecodeData.decodeRobotInfo(data); + break; + case 0x02: + //算法结果 + + break; + case 0x03: + //任务数据还需要细分 + + break; + case 0x04: + //短信内容 + DecodeData.decodeSMS(data); + + /** + * Socket返回数据给西工大 + * */ + ctx.write(EncodeData.encodeSMS("")); + break; + case 0x05: + //环境数据请求 + EncodeData.encodeEnvironment(""); + break; + case 0x06: + //AIS数据请求 + EncodeData.encodeAIS(""); + break; + case 0x07: + //方舱位置 + DecodeData.decodeShelterPosition(data); + break; + default: + break; + } + } + } + + @Override + public void channelReadComplete(ChannelHandlerContext ctx) { + ctx.flush(); + } + + @Override + public void exceptionCaught(ChannelHandlerContext ctx, Throwable cause) { + cause.printStackTrace(); + ctx.close();//抛出异常，断开与客户端的连接 + } + + @Override + public void channelActive(ChannelHandlerContext ctx) throws Exception { + super.channelActive(ctx); + ctx.channel().read(); + InetSocketAddress socketAddress = (InetSocketAddress) ctx.channel().remoteAddress(); + String ip = socketAddress.getAddress().getHostAddress(); + log.info("channelActive ===> " + ip); + } + + @Override + public void channelInactive(ChannelHandlerContext ctx) throws Exception { + super.channelInactive(ctx); + InetSocketAddress socketAddress = (InetSocketAddress) ctx.channel().remoteAddress(); + String ip = socketAddress.getAddress().getHostAddress(); + log.info("channelInactive ===> " + ip); + ctx.close(); //断开连接时，必须关闭，否则造成资源浪费，并发量很大情况下可能造成宕机 + } + + @Override + public void userEventTriggered(ChannelHandlerContext ctx, Object evt) throws Exception { + super.userEventTriggered(ctx, evt); + InetSocketAddress socketAddress = (InetSocketAddress) ctx.channel().remoteAddress(); + String ip = socketAddress.getAddress().getHostAddress(); + log.info("userEventTriggered ===> " + ip); + ctx.close();//超时时断开连接 + } +} diff --git a/casic-shelter/pom.xml b/casic-shelter/pom.xml index c1dc630..3c096f1 100644 --- a/casic-shelter/pom.xml +++ b/casic-shelter/pom.xml @@ -57,6 +57,10 @@ spatial4j 0.8 + + io.netty + netty-all + diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java index 4af70db..ea31852 100644 --- a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java @@ -6,31 +6,9 @@ * @author a203 */ public class Constant { - public static final int[] DATA = new int[]{ - 0x2A,//帧头 - 0x23,//帧长度 - 0x01,//帧类型 - 0x00,//子类型 - 0x03,//机器人ID - 0x4D, 0xBD, 0x80, 0xCF,//位置经度 - 0x0C, 0x5E, 0x28, 0xC2,//位置纬度 - 0x01, //航行状态 - 0xEB, //仪表电压 - 0xEF, //动力电压 - 0x5F, //仪表电余量 - 0x5D, //动力电余量 - 0x13, 0x5D, //俯仰角 - 0x40, 0x3D, //横滚角 - 0x30, 0xB2, //航向角 - 0x25, 0x4E, //浮力值 - 0x1B, 0x4E, //滑块位置 - 0x00, //探测状态 - 0x00, 0x9C, //目标距离 - 0x42, 0xBD, //目标方位 - 0x07, 0xD0, //目标频率 - 0xC7, 0x16, //CRC16校验码 - 0x0A //帧尾 - }; + + public static final int BITS_OF_HEAD = 0x2A; + public static final int BITS_OF_END = 0x0A; /** * 洋流相关常量 diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java deleted file mode 100644 index dc6ab54..0000000 --- a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java +++ /dev/null @@ -1,609 +0,0 @@ -package com.casic.missiles.modular.system.utils; - -import com.alibaba.fastjson.JSON; -import com.alibaba.fastjson.TypeReference; -import com.casic.missiles.modular.system.dto.AisDTO; -import com.casic.missiles.modular.system.dto.EnvironmentDTO; -import com.casic.missiles.modular.system.dto.OceanDetailDTO; -import com.casic.missiles.modular.system.dto.remote.OceanDetailRemoteData; -import com.casic.missiles.modular.system.model.RobotInfo; -import org.locationtech.spatial4j.distance.DistanceUtils; - -import java.text.DecimalFormat; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Map; - -/** - * 西工大数据解析协议 - * - * @author a203 - */ -public class DataParser { - - private static final int BITS_OF_HEAD = 0x2A; - private static final int BITS_OF_END = 0x0A; - //赤道半径(单位km) - private static final double EARTH_RADIUS = 6371.393; - - private static boolean isCorrectData(int[] bytes) { - if (bytes[0] != BITS_OF_HEAD) { - return false; - } - return bytes[bytes.length - 1] == BITS_OF_END; - } - - /** - * 水下机器人信息 - */ - public static RobotInfo decodeRobotInfo(int[] bytes) { - RobotInfo robot = new RobotInfo(); - if (!isCorrectData(bytes)) { - return robot; - } - robot.setRobotId(bytes[4]); - - int[] lngBytes = new int[4]; - System.arraycopy(bytes, 5, lngBytes, 0, 4); - robot.setLng(covertLngLat(lngBytes)); - - int[] latBytes = new int[4]; - System.arraycopy(bytes, 9, latBytes, 0, 4); - robot.setLat(covertLngLat(latBytes)); - - robot.setNavigateState(covertState(bytes[13])); - robot.setInstrumentV((float) (bytes[14] * 0.1)); - robot.setPowerV((float) bytes[15]); - robot.setInstrumentE((float) (bytes[16])); - robot.setPowerE((float) (bytes[17])); - - int[] pitchAngleBytes = new int[2]; - System.arraycopy(bytes, 18, pitchAngleBytes, 0, 2); - robot.setPitchAngle(covertPitchAngle(pitchAngleBytes)); - - int[] rollAngleBytes = new int[2]; - System.arraycopy(bytes, 20, rollAngleBytes, 0, 2); - robot.setRollAngle(covertRollAngle(rollAngleBytes)); - - int[] headingAngleBytes = new int[2]; - System.arraycopy(bytes, 22, headingAngleBytes, 0, 2); - robot.setHeadingAngle(covertHeadingAngle(headingAngleBytes)); - - int[] buoyancyBytes = new int[2]; - System.arraycopy(bytes, 24, buoyancyBytes, 0, 2); - robot.setBuoyancy(covertBuoyancy(buoyancyBytes)); - - int[] positionBytes = new int[2]; - System.arraycopy(bytes, 26, positionBytes, 0, 2); - //滑块位置算法和浮力算法一样 - robot.setPosition(covertBuoyancy(positionBytes)); - - int status = bytes[28]; - if (status == 0) { - robot.setDetectStatus("没有目标"); - } else { - robot.setDetectStatus("发现目标"); - } - - int[] targetDisBytes = new int[2]; - System.arraycopy(bytes, 29, targetDisBytes, 0, 2); - robot.setTargetDistance(covertTargetDistance(targetDisBytes)); - - int[] targetDirBytes = new int[2]; - System.arraycopy(bytes, 31, targetDirBytes, 0, 2); - //目标方位算法和航向角算法一样 - robot.setTargetDirection(covertHeadingAngle(targetDirBytes)); - - int[] targetHzBytes = new int[2]; - System.arraycopy(bytes, 33, targetHzBytes, 0, 2); - robot.setTargetHertz(covertTargetHertz(targetHzBytes)); - return robot; - } - - /** - * 区域覆盖 - */ - public static String decodeRobotRegion(int[] bytes) { - if (!isCorrectData(bytes)) { - return ""; - } - Map regionMap = new HashMap<>(); - int count = bytes[4]; - regionMap.put("count", String.valueOf(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]); - } - } - regionMap.put("robotIds", builder.toString()); - - //经度和纬度，所以需要*2 - int targetSize = count * 2 * 4; - int[] targetBytes = new int[targetSize]; - //起始4个字节，机器人个数1个字节 - System.arraycopy(bytes, 5 + idsBytes.length, targetBytes, 0, targetSize); - regionMap.put("robotTargets", formatLanLat(targetBytes)); - - regionMap.put("pointCount", String.valueOf(bytes[5 + idsBytes.length + targetBytes.length])); - - int[] radiusBytes = new int[2]; - System.arraycopy(bytes, 5 + idsBytes.length + targetBytes.length + 1, radiusBytes, 0, 2); - regionMap.put("regionRadius", covertTargetDistance(radiusBytes)); - - int aByte = bytes[5 + idsBytes.length + targetBytes.length + 1 + radiusBytes.length]; - if (aByte == 0) { - regionMap.put("contour", "轮廓标志位:多边形，" + count + "个轮廓点"); - } else { - regionMap.put("contour", "轮廓标志位:圆形，" + 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); - regionMap.put("robotPoints", formatLanLat(pointBytes)); - return JSON.toJSONString(regionMap); - } - - /** - * 路径规划 - */ - public static String decodeRobotRoute(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 decodeDirectedTask(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 decodeTaskRoute(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 decodeDirectFlightTask(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 decodeResideTask(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 decodePutTask(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); - } - - /** - * 短信内容 - */ - public static String decodeSMS(int[] bytes) { - //帧头、帧长度、帧类型、子类型、机器人ID，CRC16校验码、帧尾总长度=8 - int dataLen = bytes.length - 8; - - int[] dataBytes = new int[dataLen]; - System.arraycopy(bytes, 5, dataBytes, 0, dataLen); - StringBuilder builder = new StringBuilder(); - for (int dataByte : dataBytes) { - builder.append((char) dataByte); - } - return builder.toString(); - } - - /** - * AIS数据请求 - */ - public static String decodeAISRequest(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); - } - - /** - * 方舱位置 - * 编码encode - * 解码decode - */ - public static String decodeShelterPosition(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) { - return ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF) + "hz"; - } - - /** - * 目标距离转换算法 - */ - private static String covertTargetDistance(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01) + "km"; - } - - /** - * 横滚角转换算法 - */ - private static float covertBuoyancy(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 100); - } - - /** - * 横滚角转换算法 - */ - private static float covertHeadingAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01); - } - - /** - * 横滚角转换算法 - */ - private static float covertRollAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 180); - } - - /** - * 俯仰角转换算法 - */ - private static float covertPitchAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 90); - } - - /** - * 航行状态转换 - */ - private static String covertState(int b) { - String state = ""; - switch (b) { - case 0: - state = "任务中"; - break; - case 1: - state = "待机"; - break; - case 2: - state = "故障"; - break; - case 3: - state = "未启用"; - break; - default: - break; - } - return state; - } - - /** - * 经纬度转换算法 - */ - private static double covertLngLat(int[] bytes) { - double per = ( - ((bytes[0] & 0xFF) << 24) + - ((bytes[1] & 0xFF) << 16) + - ((bytes[2] & 0xFF) << 8) + - (bytes[3] & 0xFF) - ) / (Math.pow(2, 31) - 1); - return 180 * per; - } - - /** - * 环境数据 - */ - public static Map encodeEnvironment(String data) { - Map resultMap = createDataHead("AIS"); - resultMap.put("type", "Env"); - - EnvironmentDTO dto = new EnvironmentDTO(); - //台风中心经度 - double lng = 109.322222; - dto.setTyphoonLng(lng); - - //台风中心纬度 - double lat = 17.392232; - dto.setTyphoonLat(lat); - - //台风风力 - int power = 14; - dto.setTyphoonPower(power); - - //台风半径 - double radius = 155.56; - dto.setTyphoonRadius(radius); - - //机器人与台风中心距离 - double distance = distance(115.6314213, 12.9691386, 112.0623457, 19.9936680); - dto.setDistance(distance); - - //机器人与台风的方位角 - double angle = azimuth(115.6314213, 12.9691386, 112.0623457, 19.9936680); - dto.setAzimuth(angle); - - //洋流数据 - String oceanData = HttpRequestHelper.obtainOceanData(115.6314213, 12.9691386, "2021-10-22", "0", "10"); - OceanDetailDTO oceanDetailDTO = null; - if ("0".equals(HttpRequestHelper.getOceanCode(oceanData))) { - OceanDetailRemoteData remoteData = JSON.parseObject(oceanData, new TypeReference() { - }); - oceanDetailDTO = new OceanDetailDTO(remoteData); - } - dto.setOceanDetail(oceanDetailDTO); - - resultMap.put("data", dto); - return resultMap; - } - - /** - * AIS数据 - */ - public static Map encodeAIS(String data) { - Map resultMap = createDataHead("AIS"); - resultMap.put("type", "AIS"); - //船只数量 - int shipCount = 2; - List positionBeans = new ArrayList<>(); - for (int i = 0; i < shipCount; i++) { - AisDTO ais = new AisDTO(); - ais.setCourse(187.2); - ais.setHeading(189.0); - //船只经度 - double lng = 109.322222; - ais.setLng(lng); - //船只纬度 - double lat = 17.392232; - ais.setLat(lat); - ais.setPositionTime("2021-09-29 16:12:25"); - ais.setSpeed(16.3); - - positionBeans.add(ais); - } - resultMap.put("data", positionBeans); - return resultMap; - } - - /** - * 短信数据 - */ - public static Map encodeSMS(String data) { - Map resultMap = createDataHead("SMS"); - resultMap.put("data", data); - return resultMap; - } - - private static Map createDataHead(String type) { - Map resultMap = new HashMap<>(8); - resultMap.put("code", 200); - resultMap.put("type", type); - resultMap.put("message", "请求成功"); - resultMap.put("success", true); - return resultMap; - } - - /** - * 经纬度转弧度 - */ - private static double lnglatToRadian(double d) { - return d * Math.PI / 180.0; - } - - /** - * 经纬度距离计算 - */ - private static double distance(double lonA, double latA, double lonB, double latB) { - double aLng = lnglatToRadian(lonA); - double aLat = lnglatToRadian(latA); - double bLng = lnglatToRadian(lonB); - double bLat = lnglatToRadian(latB); - - double dist = DistanceUtils.distHaversineRAD(aLat, aLng, bLat, bLng) * EARTH_RADIUS; - DecimalFormat df = new DecimalFormat("#.00"); - return Double.parseDouble(df.format(dist)); - } - - /** - * 方位角 - *

- * https://blog.csdn.net/dulingwen/article/details/96868530 - */ - private static int azimuth(double lonA, double latA, double lonB, double latB) { - double deltaLng = lonB - lonA; - - double y = Math.sin(deltaLng) * Math.cos(latB); - double x = Math.cos(latA) * Math.sin(latB) - Math.sin(latA) * Math.cos(latB) * Math.cos(deltaLng); - - double bearing = Math.atan2(y, x); - bearing = Math.toDegrees(bearing); - bearing = (bearing + 360) % 360; - - return (int) bearing; - } -} \ No newline at end of file diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java new file mode 100644 index 0000000..9cb0e99 --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java @@ -0,0 +1,429 @@ +package com.casic.missiles.modular.system.utils; + +import com.alibaba.fastjson.JSON; +import com.casic.missiles.modular.system.model.RobotInfo; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * 西工大数据解析 + * + * @author a203 + */ +public class DecodeData { + /** + * 水下机器人信息 + */ + public static RobotInfo decodeRobotInfo(int[] bytes) { + RobotInfo robot = new RobotInfo(); + + robot.setRobotId(bytes[4]); + + int[] lngBytes = new int[4]; + System.arraycopy(bytes, 5, lngBytes, 0, 4); + robot.setLng(covertLngLat(lngBytes)); + + int[] latBytes = new int[4]; + System.arraycopy(bytes, 9, latBytes, 0, 4); + robot.setLat(covertLngLat(latBytes)); + + robot.setNavigateState(covertState(bytes[13])); + robot.setInstrumentV((float) (bytes[14] * 0.1)); + robot.setPowerV((float) bytes[15]); + robot.setInstrumentE((float) (bytes[16])); + robot.setPowerE((float) (bytes[17])); + + int[] pitchAngleBytes = new int[2]; + System.arraycopy(bytes, 18, pitchAngleBytes, 0, 2); + robot.setPitchAngle(covertPitchAngle(pitchAngleBytes)); + + int[] rollAngleBytes = new int[2]; + System.arraycopy(bytes, 20, rollAngleBytes, 0, 2); + robot.setRollAngle(covertRollAngle(rollAngleBytes)); + + int[] headingAngleBytes = new int[2]; + System.arraycopy(bytes, 22, headingAngleBytes, 0, 2); + robot.setHeadingAngle(covertHeadingAngle(headingAngleBytes)); + + int[] buoyancyBytes = new int[2]; + System.arraycopy(bytes, 24, buoyancyBytes, 0, 2); + robot.setBuoyancy(covertBuoyancy(buoyancyBytes)); + + int[] positionBytes = new int[2]; + System.arraycopy(bytes, 26, positionBytes, 0, 2); + //滑块位置算法和浮力算法一样 + robot.setPosition(covertBuoyancy(positionBytes)); + + int status = bytes[28]; + if (status == 0) { + robot.setDetectStatus("没有目标"); + } else { + robot.setDetectStatus("发现目标"); + } + + int[] targetDisBytes = new int[2]; + System.arraycopy(bytes, 29, targetDisBytes, 0, 2); + robot.setTargetDistance(covertTargetDistance(targetDisBytes)); + + int[] targetDirBytes = new int[2]; + System.arraycopy(bytes, 31, targetDirBytes, 0, 2); + //目标方位算法和航向角算法一样 + robot.setTargetDirection(covertHeadingAngle(targetDirBytes)); + + int[] targetHzBytes = new int[2]; + System.arraycopy(bytes, 33, targetHzBytes, 0, 2); + robot.setTargetHertz(covertTargetHertz(targetHzBytes)); + return robot; + } + + /** + * 区域覆盖 + */ + public static String decodeRobotRegion(int[] bytes) { + Map regionMap = new HashMap<>(); + int count = bytes[4]; + regionMap.put("count", String.valueOf(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]); + } + } + regionMap.put("robotIds", builder.toString()); + + //经度和纬度，所以需要*2 + int targetSize = count * 2 * 4; + int[] targetBytes = new int[targetSize]; + //起始4个字节，机器人个数1个字节 + System.arraycopy(bytes, 5 + idsBytes.length, targetBytes, 0, targetSize); + regionMap.put("robotTargets", formatLanLat(targetBytes)); + + regionMap.put("pointCount", String.valueOf(bytes[5 + idsBytes.length + targetBytes.length])); + + int[] radiusBytes = new int[2]; + System.arraycopy(bytes, 5 + idsBytes.length + targetBytes.length + 1, radiusBytes, 0, 2); + regionMap.put("regionRadius", covertTargetDistance(radiusBytes)); + + int aByte = bytes[5 + idsBytes.length + targetBytes.length + 1 + radiusBytes.length]; + if (aByte == 0) { + regionMap.put("contour", "轮廓标志位:多边形，" + count + "个轮廓点"); + } else { + regionMap.put("contour", "轮廓标志位:圆形，" + 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); + regionMap.put("robotPoints", formatLanLat(pointBytes)); + return JSON.toJSONString(regionMap); + } + + /** + * 路径规划 + */ + public static String decodeRobotRoute(int[] bytes) { + 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 decodeDirectedTask(int[] bytes) { + 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 decodeTaskRoute(int[] bytes) { + 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 decodeDirectFlightTask(int[] bytes) { + 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 decodeResideTask(int[] bytes) { + 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 decodePutTask(int[] bytes) { + 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); + } + + /** + * 短信内容 + */ + public static String decodeSMS(int[] bytes) { + //帧头、帧长度、帧类型、子类型、机器人ID，CRC16校验码、帧尾总长度=8 + int dataLen = bytes.length - 8; + + int[] dataBytes = new int[dataLen]; + System.arraycopy(bytes, 5, dataBytes, 0, dataLen); + StringBuilder builder = new StringBuilder(); + for (int dataByte : dataBytes) { + builder.append((char) dataByte); + } + return builder.toString(); + } + + /** + * AIS数据请求 + */ + public static String decodeAISRequest(int[] bytes) { + 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); + } + + /** + * 方舱位置 + * 编码encode + * 解码decode + */ + public static String decodeShelterPosition(int[] bytes) { + 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) { + return ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF) + "hz"; + } + + /** + * 目标距离转换算法 + */ + private static String covertTargetDistance(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01) + "km"; + } + + /** + * 横滚角转换算法 + */ + private static float covertBuoyancy(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 100); + } + + /** + * 横滚角转换算法 + */ + private static float covertHeadingAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01); + } + + /** + * 横滚角转换算法 + */ + private static float covertRollAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 180); + } + + /** + * 俯仰角转换算法 + */ + private static float covertPitchAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 90); + } + + /** + * 航行状态转换 + */ + private static String covertState(int b) { + String state = ""; + switch (b) { + case 0: + state = "任务中"; + break; + case 1: + state = "待机"; + break; + case 2: + state = "故障"; + break; + case 3: + state = "未启用"; + break; + default: + break; + } + return state; + } + + /** + * 经纬度转换算法 + */ + private static double covertLngLat(int[] bytes) { + double per = ( + ((bytes[0] & 0xFF) << 24) + + ((bytes[1] & 0xFF) << 16) + + ((bytes[2] & 0xFF) << 8) + + (bytes[3] & 0xFF) + ) / (Math.pow(2, 31) - 1); + return 180 * per; + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java new file mode 100644 index 0000000..bc0e41e --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java @@ -0,0 +1,160 @@ +package com.casic.missiles.modular.system.utils; + +import com.alibaba.fastjson.JSON; +import com.alibaba.fastjson.TypeReference; +import com.casic.missiles.modular.system.dto.AisDTO; +import com.casic.missiles.modular.system.dto.EnvironmentDTO; +import com.casic.missiles.modular.system.dto.OceanDetailDTO; +import com.casic.missiles.modular.system.dto.remote.OceanDetailRemoteData; +import org.locationtech.spatial4j.distance.DistanceUtils; + +import java.text.DecimalFormat; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * 203所数据编码 + * + * @author a203 + */ +public class EncodeData { + /** + * 赤道半径(单位km) + */ + private static final double EARTH_RADIUS = 6371.393; + + /** + * 环境数据 + */ + public static Map encodeEnvironment(String data) { + Map resultMap = createDataHead("AIS"); + resultMap.put("type", "Env"); + resultMap.put("robotID", 0); + + EnvironmentDTO dto = new EnvironmentDTO(); + //台风中心经度 + double lng = 109.322222; + dto.setTyphoonLng(lng); + + //台风中心纬度 + double lat = 17.392232; + dto.setTyphoonLat(lat); + + //台风风力 + int power = 14; + dto.setTyphoonPower(power); + + //台风半径 + double radius = 155.56; + dto.setTyphoonRadius(radius); + + //机器人与台风中心距离 + double distance = distance(115.6314213, 12.9691386, 112.0623457, 19.9936680); + dto.setDistance(distance); + + //机器人与台风的方位角 + double angle = azimuth(115.6314213, 12.9691386, 112.0623457, 19.9936680); + dto.setAzimuth(angle); + + //洋流数据 + String oceanData = HttpRequestHelper.obtainOceanData(115.6314213, 12.9691386, "2021-10-22", "0", "10"); + OceanDetailDTO oceanDetailDTO = null; + if ("0".equals(HttpRequestHelper.getOceanCode(oceanData))) { + OceanDetailRemoteData remoteData = JSON.parseObject(oceanData, new TypeReference() { + }); + oceanDetailDTO = new OceanDetailDTO(remoteData); + } + dto.setOceanDetail(oceanDetailDTO); + + resultMap.put("data", dto); + return resultMap; + } + + /** + * AIS数据 + */ + public static Map encodeAIS(String data) { + Map resultMap = createDataHead("AIS"); + resultMap.put("type", "AIS"); + resultMap.put("robotID", 0); + //船只数量 + int shipCount = 2; + List positionBeans = new ArrayList<>(); + for (int i = 0; i < shipCount; i++) { + AisDTO ais = new AisDTO(); + ais.setCourse(187.2); + ais.setHeading(189.0); + //船只经度 + double lng = 109.322222; + ais.setLng(lng); + //船只纬度 + double lat = 17.392232; + ais.setLat(lat); + ais.setPositionTime("2021-09-29 16:12:25"); + ais.setSpeed(16.3); + + positionBeans.add(ais); + } + resultMap.put("data", positionBeans); + return resultMap; + } + + /** + * 短信数据 + */ + public static Map encodeSMS(String data) { + Map resultMap = createDataHead("SMS"); + resultMap.put("data", data); + return resultMap; + } + + private static Map createDataHead(String type) { + Map resultMap = new HashMap<>(8); + resultMap.put("code", 200); + resultMap.put("type", type); + resultMap.put("message", "请求成功"); + resultMap.put("success", true); + return resultMap; + } + + /** + * 经纬度转弧度 + */ + private static double lnglatToRadian(double d) { + return d * Math.PI / 180.0; + } + + /** + * 经纬度距离计算 + */ + private static double distance(double lonA, double latA, double lonB, double latB) { + double aLng = lnglatToRadian(lonA); + double aLat = lnglatToRadian(latA); + double bLng = lnglatToRadian(lonB); + double bLat = lnglatToRadian(latB); + + double dist = DistanceUtils.distHaversineRAD(aLat, aLng, bLat, bLng) * EARTH_RADIUS; + DecimalFormat df = new DecimalFormat("#.00"); + return Double.parseDouble(df.format(dist)); + } + + /** + * 方位角 + *

+ * https://blog.csdn.net/dulingwen/article/details/96868530 + */ + private static int azimuth(double lonA, double latA, double lonB, double latB) { + double deltaLng = lonB - lonA; + + double y = Math.sin(deltaLng) * Math.cos(latB); + double x = Math.cos(latA) * Math.sin(latB) - Math.sin(latA) * Math.cos(latB) * Math.cos(deltaLng); + + double bearing = Math.atan2(y, x); + bearing = Math.toDegrees(bearing); + bearing = (bearing + 360) % 360; + + return (int) bearing; + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/socket/BootNettyServer.java b/casic-shelter/src/main/java/com/casic/missiles/socket/BootNettyServer.java new file mode 100644 index 0000000..02f5ba2 --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/socket/BootNettyServer.java @@ -0,0 +1,59 @@ +package com.casic.missiles.socket; + +import io.netty.bootstrap.ServerBootstrap; +import io.netty.channel.AdaptiveRecvByteBufAllocator; +import io.netty.channel.ChannelFuture; +import io.netty.channel.ChannelOption; +import io.netty.channel.EventLoopGroup; +import io.netty.channel.nio.NioEventLoopGroup; +import io.netty.channel.socket.SocketChannel; +import io.netty.channel.socket.nio.NioServerSocketChannel; +import lombok.extern.slf4j.Slf4j; + +/** + * @author a203 + */ +@Slf4j +public class BootNettyServer { + public void bind(int port) { + EventLoopGroup bossGroup = new NioEventLoopGroup(1); + EventLoopGroup workerGroup = new NioEventLoopGroup(); + try { + /** + * ServerBootstrap 是一个启动NIO服务的辅助启动类 + */ + ServerBootstrap serverBootstrap = new ServerBootstrap(); + /** + * 设置group，将bossGroup，workerGroup线程组传递到ServerBootstrap + */ + serverBootstrap = serverBootstrap.group(bossGroup, workerGroup); + /** + * ServerSocketChannel是以NIO的selector为基础进行实现的，用来接收新的连接，这里告诉Channel通过NioServerSocketChannel获取新的连接 + */ + serverBootstrap = serverBootstrap.channel(NioServerSocketChannel.class); + serverBootstrap = serverBootstrap.option(ChannelOption.RCVBUF_ALLOCATOR, new AdaptiveRecvByteBufAllocator(64, 10496, 1048576)); + serverBootstrap = serverBootstrap.childOption(ChannelOption.RCVBUF_ALLOCATOR, new AdaptiveRecvByteBufAllocator(64, 10496, 1048576)); + /** + * 设置 I/O处理类,主要用于网络I/O事件，记录日志，编码、解码消息 + */ + serverBootstrap = serverBootstrap.childHandler(new ChannelInitializer()); + log.info("端口已开启，占用" + port + "端口号...."); + /** + * 绑定端口，同步等待成功 + */ + ChannelFuture channelFuture = serverBootstrap.bind(port).sync(); + /** + * 等待服务器监听端口关闭 + */ + channelFuture.channel().closeFuture().sync(); + } catch (InterruptedException e) { + e.printStackTrace(); + } finally { + /** + * 退出，释放线程池资源 + */ + bossGroup.shutdownGracefully(); + workerGroup.shutdownGracefully(); + } + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelHandlerAdapter.java b/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelHandlerAdapter.java new file mode 100644 index 0000000..7aa4a6a --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelHandlerAdapter.java @@ -0,0 +1,119 @@ +package com.casic.missiles.socket; + +import com.casic.missiles.modular.system.utils.Constant; +import com.casic.missiles.modular.system.utils.DecodeData; +import com.casic.missiles.modular.system.utils.EncodeData; +import io.netty.channel.ChannelHandlerContext; +import io.netty.channel.ChannelInboundHandlerAdapter; +import lombok.extern.slf4j.Slf4j; + +import java.net.InetSocketAddress; + +/** + * @author a203 + */ +@Slf4j +public class ChannelHandlerAdapter extends ChannelInboundHandlerAdapter { + + public static boolean isCorrectData(int[] bytes) { + if (bytes[0] != Constant.BITS_OF_HEAD) { + return false; + } + return bytes[bytes.length - 1] == Constant.BITS_OF_END; + } + + @Override + public void channelRead(ChannelHandlerContext ctx, Object msg) { + String temp = msg.toString(); + log.info("channelRead message ===> " + temp); + /** + * 回应客户端 + * */ + String[] dataArray = temp.split(" "); + int[] data = new int[dataArray.length]; + for (int i = 0; i < dataArray.length; i++) { + data[i] = Integer.parseInt(dataArray[i], 16); + } + if (isCorrectData(data)) { + int dataType = data[2]; + /** + * 先解析再回应 + * */ + switch (dataType) { + case 0x01: + //水下机器人信息 + DecodeData.decodeRobotInfo(data); + break; + case 0x02: + //算法结果 + + break; + case 0x03: + //任务数据还需要细分 + + break; + case 0x04: + //短信内容 + DecodeData.decodeSMS(data); + + /** + * Socket返回数据给西工大 + * */ + ctx.write(EncodeData.encodeSMS("")); + break; + case 0x05: + //环境数据请求 + EncodeData.encodeEnvironment(""); + break; + case 0x06: + //AIS数据请求 + EncodeData.encodeAIS(""); + break; + case 0x07: + //方舱位置 + DecodeData.decodeShelterPosition(data); + break; + default: + break; + } + } + } + + @Override + public void channelReadComplete(ChannelHandlerContext ctx) { + ctx.flush(); + } + + @Override + public void exceptionCaught(ChannelHandlerContext ctx, Throwable cause) { + cause.printStackTrace(); + ctx.close();//抛出异常，断开与客户端的连接 + } + + @Override + public void channelActive(ChannelHandlerContext ctx) throws Exception { + super.channelActive(ctx); + ctx.channel().read(); + InetSocketAddress socketAddress = (InetSocketAddress) ctx.channel().remoteAddress(); + String ip = socketAddress.getAddress().getHostAddress(); + log.info("channelActive ===> " + ip); + } + + @Override + public void channelInactive(ChannelHandlerContext ctx) throws Exception { + super.channelInactive(ctx); + InetSocketAddress socketAddress = (InetSocketAddress) ctx.channel().remoteAddress(); + String ip = socketAddress.getAddress().getHostAddress(); + log.info("channelInactive ===> " + ip); + ctx.close(); //断开连接时，必须关闭，否则造成资源浪费，并发量很大情况下可能造成宕机 + } + + @Override + public void userEventTriggered(ChannelHandlerContext ctx, Object evt) throws Exception { + super.userEventTriggered(ctx, evt); + InetSocketAddress socketAddress = (InetSocketAddress) ctx.channel().remoteAddress(); + String ip = socketAddress.getAddress().getHostAddress(); + log.info("userEventTriggered ===> " + ip); + ctx.close();//超时时断开连接 + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelInitializer.java b/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelInitializer.java new file mode 100644 index 0000000..4c9513b --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelInitializer.java @@ -0,0 +1,24 @@ +package com.casic.missiles.socket; + +import io.netty.channel.Channel; +import io.netty.handler.codec.string.StringDecoder; +import io.netty.handler.codec.string.StringEncoder; + +/** + * @author a203 + */ +public class ChannelInitializer extends io.netty.channel.ChannelInitializer { + + @Override + protected void initChannel(Channel channel) { + // ChannelOutboundHandler，依照逆序执行 + channel.pipeline().addLast("encoder", new StringEncoder()); + + // 属于ChannelInboundHandler，依照顺序执行 + channel.pipeline().addLast("decoder", new StringDecoder()); + /** + * 自定义ChannelInboundHandlerAdapter + */ + channel.pipeline().addLast(new ChannelHandlerAdapter()); + } +} diff --git a/casic-shelter/pom.xml b/casic-shelter/pom.xml index c1dc630..3c096f1 100644 --- a/casic-shelter/pom.xml +++ b/casic-shelter/pom.xml @@ -57,6 +57,10 @@ spatial4j 0.8 + + io.netty + netty-all + diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java index 4af70db..ea31852 100644 --- a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java @@ -6,31 +6,9 @@ * @author a203 */ public class Constant { - public static final int[] DATA = new int[]{ - 0x2A,//帧头 - 0x23,//帧长度 - 0x01,//帧类型 - 0x00,//子类型 - 0x03,//机器人ID - 0x4D, 0xBD, 0x80, 0xCF,//位置经度 - 0x0C, 0x5E, 0x28, 0xC2,//位置纬度 - 0x01, //航行状态 - 0xEB, //仪表电压 - 0xEF, //动力电压 - 0x5F, //仪表电余量 - 0x5D, //动力电余量 - 0x13, 0x5D, //俯仰角 - 0x40, 0x3D, //横滚角 - 0x30, 0xB2, //航向角 - 0x25, 0x4E, //浮力值 - 0x1B, 0x4E, //滑块位置 - 0x00, //探测状态 - 0x00, 0x9C, //目标距离 - 0x42, 0xBD, //目标方位 - 0x07, 0xD0, //目标频率 - 0xC7, 0x16, //CRC16校验码 - 0x0A //帧尾 - }; + + public static final int BITS_OF_HEAD = 0x2A; + public static final int BITS_OF_END = 0x0A; /** * 洋流相关常量 diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java deleted file mode 100644 index dc6ab54..0000000 --- a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java +++ /dev/null @@ -1,609 +0,0 @@ -package com.casic.missiles.modular.system.utils; - -import com.alibaba.fastjson.JSON; -import com.alibaba.fastjson.TypeReference; -import com.casic.missiles.modular.system.dto.AisDTO; -import com.casic.missiles.modular.system.dto.EnvironmentDTO; -import com.casic.missiles.modular.system.dto.OceanDetailDTO; -import com.casic.missiles.modular.system.dto.remote.OceanDetailRemoteData; -import com.casic.missiles.modular.system.model.RobotInfo; -import org.locationtech.spatial4j.distance.DistanceUtils; - -import java.text.DecimalFormat; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Map; - -/** - * 西工大数据解析协议 - * - * @author a203 - */ -public class DataParser { - - private static final int BITS_OF_HEAD = 0x2A; - private static final int BITS_OF_END = 0x0A; - //赤道半径(单位km) - private static final double EARTH_RADIUS = 6371.393; - - private static boolean isCorrectData(int[] bytes) { - if (bytes[0] != BITS_OF_HEAD) { - return false; - } - return bytes[bytes.length - 1] == BITS_OF_END; - } - - /** - * 水下机器人信息 - */ - public static RobotInfo decodeRobotInfo(int[] bytes) { - RobotInfo robot = new RobotInfo(); - if (!isCorrectData(bytes)) { - return robot; - } - robot.setRobotId(bytes[4]); - - int[] lngBytes = new int[4]; - System.arraycopy(bytes, 5, lngBytes, 0, 4); - robot.setLng(covertLngLat(lngBytes)); - - int[] latBytes = new int[4]; - System.arraycopy(bytes, 9, latBytes, 0, 4); - robot.setLat(covertLngLat(latBytes)); - - robot.setNavigateState(covertState(bytes[13])); - robot.setInstrumentV((float) (bytes[14] * 0.1)); - robot.setPowerV((float) bytes[15]); - robot.setInstrumentE((float) (bytes[16])); - robot.setPowerE((float) (bytes[17])); - - int[] pitchAngleBytes = new int[2]; - System.arraycopy(bytes, 18, pitchAngleBytes, 0, 2); - robot.setPitchAngle(covertPitchAngle(pitchAngleBytes)); - - int[] rollAngleBytes = new int[2]; - System.arraycopy(bytes, 20, rollAngleBytes, 0, 2); - robot.setRollAngle(covertRollAngle(rollAngleBytes)); - - int[] headingAngleBytes = new int[2]; - System.arraycopy(bytes, 22, headingAngleBytes, 0, 2); - robot.setHeadingAngle(covertHeadingAngle(headingAngleBytes)); - - int[] buoyancyBytes = new int[2]; - System.arraycopy(bytes, 24, buoyancyBytes, 0, 2); - robot.setBuoyancy(covertBuoyancy(buoyancyBytes)); - - int[] positionBytes = new int[2]; - System.arraycopy(bytes, 26, positionBytes, 0, 2); - //滑块位置算法和浮力算法一样 - robot.setPosition(covertBuoyancy(positionBytes)); - - int status = bytes[28]; - if (status == 0) { - robot.setDetectStatus("没有目标"); - } else { - robot.setDetectStatus("发现目标"); - } - - int[] targetDisBytes = new int[2]; - System.arraycopy(bytes, 29, targetDisBytes, 0, 2); - robot.setTargetDistance(covertTargetDistance(targetDisBytes)); - - int[] targetDirBytes = new int[2]; - System.arraycopy(bytes, 31, targetDirBytes, 0, 2); - //目标方位算法和航向角算法一样 - robot.setTargetDirection(covertHeadingAngle(targetDirBytes)); - - int[] targetHzBytes = new int[2]; - System.arraycopy(bytes, 33, targetHzBytes, 0, 2); - robot.setTargetHertz(covertTargetHertz(targetHzBytes)); - return robot; - } - - /** - * 区域覆盖 - */ - public static String decodeRobotRegion(int[] bytes) { - if (!isCorrectData(bytes)) { - return ""; - } - Map regionMap = new HashMap<>(); - int count = bytes[4]; - regionMap.put("count", String.valueOf(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]); - } - } - regionMap.put("robotIds", builder.toString()); - - //经度和纬度，所以需要*2 - int targetSize = count * 2 * 4; - int[] targetBytes = new int[targetSize]; - //起始4个字节，机器人个数1个字节 - System.arraycopy(bytes, 5 + idsBytes.length, targetBytes, 0, targetSize); - regionMap.put("robotTargets", formatLanLat(targetBytes)); - - regionMap.put("pointCount", String.valueOf(bytes[5 + idsBytes.length + targetBytes.length])); - - int[] radiusBytes = new int[2]; - System.arraycopy(bytes, 5 + idsBytes.length + targetBytes.length + 1, radiusBytes, 0, 2); - regionMap.put("regionRadius", covertTargetDistance(radiusBytes)); - - int aByte = bytes[5 + idsBytes.length + targetBytes.length + 1 + radiusBytes.length]; - if (aByte == 0) { - regionMap.put("contour", "轮廓标志位:多边形，" + count + "个轮廓点"); - } else { - regionMap.put("contour", "轮廓标志位:圆形，" + 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); - regionMap.put("robotPoints", formatLanLat(pointBytes)); - return JSON.toJSONString(regionMap); - } - - /** - * 路径规划 - */ - public static String decodeRobotRoute(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 decodeDirectedTask(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 decodeTaskRoute(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 decodeDirectFlightTask(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 decodeResideTask(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 decodePutTask(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); - } - - /** - * 短信内容 - */ - public static String decodeSMS(int[] bytes) { - //帧头、帧长度、帧类型、子类型、机器人ID，CRC16校验码、帧尾总长度=8 - int dataLen = bytes.length - 8; - - int[] dataBytes = new int[dataLen]; - System.arraycopy(bytes, 5, dataBytes, 0, dataLen); - StringBuilder builder = new StringBuilder(); - for (int dataByte : dataBytes) { - builder.append((char) dataByte); - } - return builder.toString(); - } - - /** - * AIS数据请求 - */ - public static String decodeAISRequest(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); - } - - /** - * 方舱位置 - * 编码encode - * 解码decode - */ - public static String decodeShelterPosition(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) { - return ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF) + "hz"; - } - - /** - * 目标距离转换算法 - */ - private static String covertTargetDistance(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01) + "km"; - } - - /** - * 横滚角转换算法 - */ - private static float covertBuoyancy(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 100); - } - - /** - * 横滚角转换算法 - */ - private static float covertHeadingAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01); - } - - /** - * 横滚角转换算法 - */ - private static float covertRollAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 180); - } - - /** - * 俯仰角转换算法 - */ - private static float covertPitchAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 90); - } - - /** - * 航行状态转换 - */ - private static String covertState(int b) { - String state = ""; - switch (b) { - case 0: - state = "任务中"; - break; - case 1: - state = "待机"; - break; - case 2: - state = "故障"; - break; - case 3: - state = "未启用"; - break; - default: - break; - } - return state; - } - - /** - * 经纬度转换算法 - */ - private static double covertLngLat(int[] bytes) { - double per = ( - ((bytes[0] & 0xFF) << 24) + - ((bytes[1] & 0xFF) << 16) + - ((bytes[2] & 0xFF) << 8) + - (bytes[3] & 0xFF) - ) / (Math.pow(2, 31) - 1); - return 180 * per; - } - - /** - * 环境数据 - */ - public static Map encodeEnvironment(String data) { - Map resultMap = createDataHead("AIS"); - resultMap.put("type", "Env"); - - EnvironmentDTO dto = new EnvironmentDTO(); - //台风中心经度 - double lng = 109.322222; - dto.setTyphoonLng(lng); - - //台风中心纬度 - double lat = 17.392232; - dto.setTyphoonLat(lat); - - //台风风力 - int power = 14; - dto.setTyphoonPower(power); - - //台风半径 - double radius = 155.56; - dto.setTyphoonRadius(radius); - - //机器人与台风中心距离 - double distance = distance(115.6314213, 12.9691386, 112.0623457, 19.9936680); - dto.setDistance(distance); - - //机器人与台风的方位角 - double angle = azimuth(115.6314213, 12.9691386, 112.0623457, 19.9936680); - dto.setAzimuth(angle); - - //洋流数据 - String oceanData = HttpRequestHelper.obtainOceanData(115.6314213, 12.9691386, "2021-10-22", "0", "10"); - OceanDetailDTO oceanDetailDTO = null; - if ("0".equals(HttpRequestHelper.getOceanCode(oceanData))) { - OceanDetailRemoteData remoteData = JSON.parseObject(oceanData, new TypeReference() { - }); - oceanDetailDTO = new OceanDetailDTO(remoteData); - } - dto.setOceanDetail(oceanDetailDTO); - - resultMap.put("data", dto); - return resultMap; - } - - /** - * AIS数据 - */ - public static Map encodeAIS(String data) { - Map resultMap = createDataHead("AIS"); - resultMap.put("type", "AIS"); - //船只数量 - int shipCount = 2; - List positionBeans = new ArrayList<>(); - for (int i = 0; i < shipCount; i++) { - AisDTO ais = new AisDTO(); - ais.setCourse(187.2); - ais.setHeading(189.0); - //船只经度 - double lng = 109.322222; - ais.setLng(lng); - //船只纬度 - double lat = 17.392232; - ais.setLat(lat); - ais.setPositionTime("2021-09-29 16:12:25"); - ais.setSpeed(16.3); - - positionBeans.add(ais); - } - resultMap.put("data", positionBeans); - return resultMap; - } - - /** - * 短信数据 - */ - public static Map encodeSMS(String data) { - Map resultMap = createDataHead("SMS"); - resultMap.put("data", data); - return resultMap; - } - - private static Map createDataHead(String type) { - Map resultMap = new HashMap<>(8); - resultMap.put("code", 200); - resultMap.put("type", type); - resultMap.put("message", "请求成功"); - resultMap.put("success", true); - return resultMap; - } - - /** - * 经纬度转弧度 - */ - private static double lnglatToRadian(double d) { - return d * Math.PI / 180.0; - } - - /** - * 经纬度距离计算 - */ - private static double distance(double lonA, double latA, double lonB, double latB) { - double aLng = lnglatToRadian(lonA); - double aLat = lnglatToRadian(latA); - double bLng = lnglatToRadian(lonB); - double bLat = lnglatToRadian(latB); - - double dist = DistanceUtils.distHaversineRAD(aLat, aLng, bLat, bLng) * EARTH_RADIUS; - DecimalFormat df = new DecimalFormat("#.00"); - return Double.parseDouble(df.format(dist)); - } - - /** - * 方位角 - *

- * https://blog.csdn.net/dulingwen/article/details/96868530 - */ - private static int azimuth(double lonA, double latA, double lonB, double latB) { - double deltaLng = lonB - lonA; - - double y = Math.sin(deltaLng) * Math.cos(latB); - double x = Math.cos(latA) * Math.sin(latB) - Math.sin(latA) * Math.cos(latB) * Math.cos(deltaLng); - - double bearing = Math.atan2(y, x); - bearing = Math.toDegrees(bearing); - bearing = (bearing + 360) % 360; - - return (int) bearing; - } -} \ No newline at end of file diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java new file mode 100644 index 0000000..9cb0e99 --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java @@ -0,0 +1,429 @@ +package com.casic.missiles.modular.system.utils; + +import com.alibaba.fastjson.JSON; +import com.casic.missiles.modular.system.model.RobotInfo; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * 西工大数据解析 + * + * @author a203 + */ +public class DecodeData { + /** + * 水下机器人信息 + */ + public static RobotInfo decodeRobotInfo(int[] bytes) { + RobotInfo robot = new RobotInfo(); + + robot.setRobotId(bytes[4]); + + int[] lngBytes = new int[4]; + System.arraycopy(bytes, 5, lngBytes, 0, 4); + robot.setLng(covertLngLat(lngBytes)); + + int[] latBytes = new int[4]; + System.arraycopy(bytes, 9, latBytes, 0, 4); + robot.setLat(covertLngLat(latBytes)); + + robot.setNavigateState(covertState(bytes[13])); + robot.setInstrumentV((float) (bytes[14] * 0.1)); + robot.setPowerV((float) bytes[15]); + robot.setInstrumentE((float) (bytes[16])); + robot.setPowerE((float) (bytes[17])); + + int[] pitchAngleBytes = new int[2]; + System.arraycopy(bytes, 18, pitchAngleBytes, 0, 2); + robot.setPitchAngle(covertPitchAngle(pitchAngleBytes)); + + int[] rollAngleBytes = new int[2]; + System.arraycopy(bytes, 20, rollAngleBytes, 0, 2); + robot.setRollAngle(covertRollAngle(rollAngleBytes)); + + int[] headingAngleBytes = new int[2]; + System.arraycopy(bytes, 22, headingAngleBytes, 0, 2); + robot.setHeadingAngle(covertHeadingAngle(headingAngleBytes)); + + int[] buoyancyBytes = new int[2]; + System.arraycopy(bytes, 24, buoyancyBytes, 0, 2); + robot.setBuoyancy(covertBuoyancy(buoyancyBytes)); + + int[] positionBytes = new int[2]; + System.arraycopy(bytes, 26, positionBytes, 0, 2); + //滑块位置算法和浮力算法一样 + robot.setPosition(covertBuoyancy(positionBytes)); + + int status = bytes[28]; + if (status == 0) { + robot.setDetectStatus("没有目标"); + } else { + robot.setDetectStatus("发现目标"); + } + + int[] targetDisBytes = new int[2]; + System.arraycopy(bytes, 29, targetDisBytes, 0, 2); + robot.setTargetDistance(covertTargetDistance(targetDisBytes)); + + int[] targetDirBytes = new int[2]; + System.arraycopy(bytes, 31, targetDirBytes, 0, 2); + //目标方位算法和航向角算法一样 + robot.setTargetDirection(covertHeadingAngle(targetDirBytes)); + + int[] targetHzBytes = new int[2]; + System.arraycopy(bytes, 33, targetHzBytes, 0, 2); + robot.setTargetHertz(covertTargetHertz(targetHzBytes)); + return robot; + } + + /** + * 区域覆盖 + */ + public static String decodeRobotRegion(int[] bytes) { + Map regionMap = new HashMap<>(); + int count = bytes[4]; + regionMap.put("count", String.valueOf(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]); + } + } + regionMap.put("robotIds", builder.toString()); + + //经度和纬度，所以需要*2 + int targetSize = count * 2 * 4; + int[] targetBytes = new int[targetSize]; + //起始4个字节，机器人个数1个字节 + System.arraycopy(bytes, 5 + idsBytes.length, targetBytes, 0, targetSize); + regionMap.put("robotTargets", formatLanLat(targetBytes)); + + regionMap.put("pointCount", String.valueOf(bytes[5 + idsBytes.length + targetBytes.length])); + + int[] radiusBytes = new int[2]; + System.arraycopy(bytes, 5 + idsBytes.length + targetBytes.length + 1, radiusBytes, 0, 2); + regionMap.put("regionRadius", covertTargetDistance(radiusBytes)); + + int aByte = bytes[5 + idsBytes.length + targetBytes.length + 1 + radiusBytes.length]; + if (aByte == 0) { + regionMap.put("contour", "轮廓标志位:多边形，" + count + "个轮廓点"); + } else { + regionMap.put("contour", "轮廓标志位:圆形，" + 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); + regionMap.put("robotPoints", formatLanLat(pointBytes)); + return JSON.toJSONString(regionMap); + } + + /** + * 路径规划 + */ + public static String decodeRobotRoute(int[] bytes) { + 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 decodeDirectedTask(int[] bytes) { + 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 decodeTaskRoute(int[] bytes) { + 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 decodeDirectFlightTask(int[] bytes) { + 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 decodeResideTask(int[] bytes) { + 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 decodePutTask(int[] bytes) { + 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); + } + + /** + * 短信内容 + */ + public static String decodeSMS(int[] bytes) { + //帧头、帧长度、帧类型、子类型、机器人ID，CRC16校验码、帧尾总长度=8 + int dataLen = bytes.length - 8; + + int[] dataBytes = new int[dataLen]; + System.arraycopy(bytes, 5, dataBytes, 0, dataLen); + StringBuilder builder = new StringBuilder(); + for (int dataByte : dataBytes) { + builder.append((char) dataByte); + } + return builder.toString(); + } + + /** + * AIS数据请求 + */ + public static String decodeAISRequest(int[] bytes) { + 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); + } + + /** + * 方舱位置 + * 编码encode + * 解码decode + */ + public static String decodeShelterPosition(int[] bytes) { + 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) { + return ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF) + "hz"; + } + + /** + * 目标距离转换算法 + */ + private static String covertTargetDistance(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01) + "km"; + } + + /** + * 横滚角转换算法 + */ + private static float covertBuoyancy(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 100); + } + + /** + * 横滚角转换算法 + */ + private static float covertHeadingAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01); + } + + /** + * 横滚角转换算法 + */ + private static float covertRollAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 180); + } + + /** + * 俯仰角转换算法 + */ + private static float covertPitchAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 90); + } + + /** + * 航行状态转换 + */ + private static String covertState(int b) { + String state = ""; + switch (b) { + case 0: + state = "任务中"; + break; + case 1: + state = "待机"; + break; + case 2: + state = "故障"; + break; + case 3: + state = "未启用"; + break; + default: + break; + } + return state; + } + + /** + * 经纬度转换算法 + */ + private static double covertLngLat(int[] bytes) { + double per = ( + ((bytes[0] & 0xFF) << 24) + + ((bytes[1] & 0xFF) << 16) + + ((bytes[2] & 0xFF) << 8) + + (bytes[3] & 0xFF) + ) / (Math.pow(2, 31) - 1); + return 180 * per; + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java new file mode 100644 index 0000000..bc0e41e --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java @@ -0,0 +1,160 @@ +package com.casic.missiles.modular.system.utils; + +import com.alibaba.fastjson.JSON; +import com.alibaba.fastjson.TypeReference; +import com.casic.missiles.modular.system.dto.AisDTO; +import com.casic.missiles.modular.system.dto.EnvironmentDTO; +import com.casic.missiles.modular.system.dto.OceanDetailDTO; +import com.casic.missiles.modular.system.dto.remote.OceanDetailRemoteData; +import org.locationtech.spatial4j.distance.DistanceUtils; + +import java.text.DecimalFormat; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * 203所数据编码 + * + * @author a203 + */ +public class EncodeData { + /** + * 赤道半径(单位km) + */ + private static final double EARTH_RADIUS = 6371.393; + + /** + * 环境数据 + */ + public static Map encodeEnvironment(String data) { + Map resultMap = createDataHead("AIS"); + resultMap.put("type", "Env"); + resultMap.put("robotID", 0); + + EnvironmentDTO dto = new EnvironmentDTO(); + //台风中心经度 + double lng = 109.322222; + dto.setTyphoonLng(lng); + + //台风中心纬度 + double lat = 17.392232; + dto.setTyphoonLat(lat); + + //台风风力 + int power = 14; + dto.setTyphoonPower(power); + + //台风半径 + double radius = 155.56; + dto.setTyphoonRadius(radius); + + //机器人与台风中心距离 + double distance = distance(115.6314213, 12.9691386, 112.0623457, 19.9936680); + dto.setDistance(distance); + + //机器人与台风的方位角 + double angle = azimuth(115.6314213, 12.9691386, 112.0623457, 19.9936680); + dto.setAzimuth(angle); + + //洋流数据 + String oceanData = HttpRequestHelper.obtainOceanData(115.6314213, 12.9691386, "2021-10-22", "0", "10"); + OceanDetailDTO oceanDetailDTO = null; + if ("0".equals(HttpRequestHelper.getOceanCode(oceanData))) { + OceanDetailRemoteData remoteData = JSON.parseObject(oceanData, new TypeReference() { + }); + oceanDetailDTO = new OceanDetailDTO(remoteData); + } + dto.setOceanDetail(oceanDetailDTO); + + resultMap.put("data", dto); + return resultMap; + } + + /** + * AIS数据 + */ + public static Map encodeAIS(String data) { + Map resultMap = createDataHead("AIS"); + resultMap.put("type", "AIS"); + resultMap.put("robotID", 0); + //船只数量 + int shipCount = 2; + List positionBeans = new ArrayList<>(); + for (int i = 0; i < shipCount; i++) { + AisDTO ais = new AisDTO(); + ais.setCourse(187.2); + ais.setHeading(189.0); + //船只经度 + double lng = 109.322222; + ais.setLng(lng); + //船只纬度 + double lat = 17.392232; + ais.setLat(lat); + ais.setPositionTime("2021-09-29 16:12:25"); + ais.setSpeed(16.3); + + positionBeans.add(ais); + } + resultMap.put("data", positionBeans); + return resultMap; + } + + /** + * 短信数据 + */ + public static Map encodeSMS(String data) { + Map resultMap = createDataHead("SMS"); + resultMap.put("data", data); + return resultMap; + } + + private static Map createDataHead(String type) { + Map resultMap = new HashMap<>(8); + resultMap.put("code", 200); + resultMap.put("type", type); + resultMap.put("message", "请求成功"); + resultMap.put("success", true); + return resultMap; + } + + /** + * 经纬度转弧度 + */ + private static double lnglatToRadian(double d) { + return d * Math.PI / 180.0; + } + + /** + * 经纬度距离计算 + */ + private static double distance(double lonA, double latA, double lonB, double latB) { + double aLng = lnglatToRadian(lonA); + double aLat = lnglatToRadian(latA); + double bLng = lnglatToRadian(lonB); + double bLat = lnglatToRadian(latB); + + double dist = DistanceUtils.distHaversineRAD(aLat, aLng, bLat, bLng) * EARTH_RADIUS; + DecimalFormat df = new DecimalFormat("#.00"); + return Double.parseDouble(df.format(dist)); + } + + /** + * 方位角 + *

+ * https://blog.csdn.net/dulingwen/article/details/96868530 + */ + private static int azimuth(double lonA, double latA, double lonB, double latB) { + double deltaLng = lonB - lonA; + + double y = Math.sin(deltaLng) * Math.cos(latB); + double x = Math.cos(latA) * Math.sin(latB) - Math.sin(latA) * Math.cos(latB) * Math.cos(deltaLng); + + double bearing = Math.atan2(y, x); + bearing = Math.toDegrees(bearing); + bearing = (bearing + 360) % 360; + + return (int) bearing; + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/socket/BootNettyServer.java b/casic-shelter/src/main/java/com/casic/missiles/socket/BootNettyServer.java new file mode 100644 index 0000000..02f5ba2 --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/socket/BootNettyServer.java @@ -0,0 +1,59 @@ +package com.casic.missiles.socket; + +import io.netty.bootstrap.ServerBootstrap; +import io.netty.channel.AdaptiveRecvByteBufAllocator; +import io.netty.channel.ChannelFuture; +import io.netty.channel.ChannelOption; +import io.netty.channel.EventLoopGroup; +import io.netty.channel.nio.NioEventLoopGroup; +import io.netty.channel.socket.SocketChannel; +import io.netty.channel.socket.nio.NioServerSocketChannel; +import lombok.extern.slf4j.Slf4j; + +/** + * @author a203 + */ +@Slf4j +public class BootNettyServer { + public void bind(int port) { + EventLoopGroup bossGroup = new NioEventLoopGroup(1); + EventLoopGroup workerGroup = new NioEventLoopGroup(); + try { + /** + * ServerBootstrap 是一个启动NIO服务的辅助启动类 + */ + ServerBootstrap serverBootstrap = new ServerBootstrap(); + /** + * 设置group，将bossGroup，workerGroup线程组传递到ServerBootstrap + */ + serverBootstrap = serverBootstrap.group(bossGroup, workerGroup); + /** + * ServerSocketChannel是以NIO的selector为基础进行实现的，用来接收新的连接，这里告诉Channel通过NioServerSocketChannel获取新的连接 + */ + serverBootstrap = serverBootstrap.channel(NioServerSocketChannel.class); + serverBootstrap = serverBootstrap.option(ChannelOption.RCVBUF_ALLOCATOR, new AdaptiveRecvByteBufAllocator(64, 10496, 1048576)); + serverBootstrap = serverBootstrap.childOption(ChannelOption.RCVBUF_ALLOCATOR, new AdaptiveRecvByteBufAllocator(64, 10496, 1048576)); + /** + * 设置 I/O处理类,主要用于网络I/O事件，记录日志，编码、解码消息 + */ + serverBootstrap = serverBootstrap.childHandler(new ChannelInitializer()); + log.info("端口已开启，占用" + port + "端口号...."); + /** + * 绑定端口，同步等待成功 + */ + ChannelFuture channelFuture = serverBootstrap.bind(port).sync(); + /** + * 等待服务器监听端口关闭 + */ + channelFuture.channel().closeFuture().sync(); + } catch (InterruptedException e) { + e.printStackTrace(); + } finally { + /** + * 退出，释放线程池资源 + */ + bossGroup.shutdownGracefully(); + workerGroup.shutdownGracefully(); + } + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelHandlerAdapter.java b/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelHandlerAdapter.java new file mode 100644 index 0000000..7aa4a6a --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelHandlerAdapter.java @@ -0,0 +1,119 @@ +package com.casic.missiles.socket; + +import com.casic.missiles.modular.system.utils.Constant; +import com.casic.missiles.modular.system.utils.DecodeData; +import com.casic.missiles.modular.system.utils.EncodeData; +import io.netty.channel.ChannelHandlerContext; +import io.netty.channel.ChannelInboundHandlerAdapter; +import lombok.extern.slf4j.Slf4j; + +import java.net.InetSocketAddress; + +/** + * @author a203 + */ +@Slf4j +public class ChannelHandlerAdapter extends ChannelInboundHandlerAdapter { + + public static boolean isCorrectData(int[] bytes) { + if (bytes[0] != Constant.BITS_OF_HEAD) { + return false; + } + return bytes[bytes.length - 1] == Constant.BITS_OF_END; + } + + @Override + public void channelRead(ChannelHandlerContext ctx, Object msg) { + String temp = msg.toString(); + log.info("channelRead message ===> " + temp); + /** + * 回应客户端 + * */ + String[] dataArray = temp.split(" "); + int[] data = new int[dataArray.length]; + for (int i = 0; i < dataArray.length; i++) { + data[i] = Integer.parseInt(dataArray[i], 16); + } + if (isCorrectData(data)) { + int dataType = data[2]; + /** + * 先解析再回应 + * */ + switch (dataType) { + case 0x01: + //水下机器人信息 + DecodeData.decodeRobotInfo(data); + break; + case 0x02: + //算法结果 + + break; + case 0x03: + //任务数据还需要细分 + + break; + case 0x04: + //短信内容 + DecodeData.decodeSMS(data); + + /** + * Socket返回数据给西工大 + * */ + ctx.write(EncodeData.encodeSMS("")); + break; + case 0x05: + //环境数据请求 + EncodeData.encodeEnvironment(""); + break; + case 0x06: + //AIS数据请求 + EncodeData.encodeAIS(""); + break; + case 0x07: + //方舱位置 + DecodeData.decodeShelterPosition(data); + break; + default: + break; + } + } + } + + @Override + public void channelReadComplete(ChannelHandlerContext ctx) { + ctx.flush(); + } + + @Override + public void exceptionCaught(ChannelHandlerContext ctx, Throwable cause) { + cause.printStackTrace(); + ctx.close();//抛出异常，断开与客户端的连接 + } + + @Override + public void channelActive(ChannelHandlerContext ctx) throws Exception { + super.channelActive(ctx); + ctx.channel().read(); + InetSocketAddress socketAddress = (InetSocketAddress) ctx.channel().remoteAddress(); + String ip = socketAddress.getAddress().getHostAddress(); + log.info("channelActive ===> " + ip); + } + + @Override + public void channelInactive(ChannelHandlerContext ctx) throws Exception { + super.channelInactive(ctx); + InetSocketAddress socketAddress = (InetSocketAddress) ctx.channel().remoteAddress(); + String ip = socketAddress.getAddress().getHostAddress(); + log.info("channelInactive ===> " + ip); + ctx.close(); //断开连接时，必须关闭，否则造成资源浪费，并发量很大情况下可能造成宕机 + } + + @Override + public void userEventTriggered(ChannelHandlerContext ctx, Object evt) throws Exception { + super.userEventTriggered(ctx, evt); + InetSocketAddress socketAddress = (InetSocketAddress) ctx.channel().remoteAddress(); + String ip = socketAddress.getAddress().getHostAddress(); + log.info("userEventTriggered ===> " + ip); + ctx.close();//超时时断开连接 + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelInitializer.java b/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelInitializer.java new file mode 100644 index 0000000..4c9513b --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelInitializer.java @@ -0,0 +1,24 @@ +package com.casic.missiles.socket; + +import io.netty.channel.Channel; +import io.netty.handler.codec.string.StringDecoder; +import io.netty.handler.codec.string.StringEncoder; + +/** + * @author a203 + */ +public class ChannelInitializer extends io.netty.channel.ChannelInitializer { + + @Override + protected void initChannel(Channel channel) { + // ChannelOutboundHandler，依照逆序执行 + channel.pipeline().addLast("encoder", new StringEncoder()); + + // 属于ChannelInboundHandler，依照顺序执行 + channel.pipeline().addLast("decoder", new StringDecoder()); + /** + * 自定义ChannelInboundHandlerAdapter + */ + channel.pipeline().addLast(new ChannelHandlerAdapter()); + } +} diff --git a/casic-web/src/main/java/com/casic/missiles/CasicApplication.java b/casic-web/src/main/java/com/casic/missiles/CasicApplication.java index dd2413c..7ad77bb 100644 --- a/casic-web/src/main/java/com/casic/missiles/CasicApplication.java +++ b/casic-web/src/main/java/com/casic/missiles/CasicApplication.java @@ -1,9 +1,14 @@ package com.casic.missiles; +import com.casic.missiles.socket.BootNettyServer; import lombok.extern.slf4j.Slf4j; +import org.springframework.beans.factory.annotation.Value; +import org.springframework.boot.CommandLineRunner; import org.springframework.boot.SpringApplication; import org.springframework.boot.autoconfigure.SpringBootApplication; import org.springframework.cache.annotation.EnableCaching; +import org.springframework.context.annotation.PropertySource; +import org.springframework.scheduling.annotation.Async; import org.springframework.scheduling.annotation.EnableAsync; import org.springframework.scheduling.annotation.EnableScheduling; import org.springframework.transaction.annotation.EnableTransactionManagement; @@ -19,10 +24,24 @@ @EnableTransactionManagement(proxyTargetClass = true) @EnableAsync @Slf4j +@PropertySource("classpath:/config/socket.properties") @EnableScheduling -public class CasicApplication { +public class CasicApplication implements CommandLineRunner { + + @Value("${socket.port}") + private Integer port; + public static void main(String[] args) { SpringApplication.run(CasicApplication.class, args); log.info("CasicApplication is success!"); } + + @Async + @Override + public void run(String... args) { + /** + * 使用异步注解方式启动netty服务端服务 + */ + new BootNettyServer().bind(port); + } } diff --git a/casic-shelter/pom.xml b/casic-shelter/pom.xml index c1dc630..3c096f1 100644 --- a/casic-shelter/pom.xml +++ b/casic-shelter/pom.xml @@ -57,6 +57,10 @@ spatial4j 0.8 + + io.netty + netty-all + diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java index 4af70db..ea31852 100644 --- a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/Constant.java @@ -6,31 +6,9 @@ * @author a203 */ public class Constant { - public static final int[] DATA = new int[]{ - 0x2A,//帧头 - 0x23,//帧长度 - 0x01,//帧类型 - 0x00,//子类型 - 0x03,//机器人ID - 0x4D, 0xBD, 0x80, 0xCF,//位置经度 - 0x0C, 0x5E, 0x28, 0xC2,//位置纬度 - 0x01, //航行状态 - 0xEB, //仪表电压 - 0xEF, //动力电压 - 0x5F, //仪表电余量 - 0x5D, //动力电余量 - 0x13, 0x5D, //俯仰角 - 0x40, 0x3D, //横滚角 - 0x30, 0xB2, //航向角 - 0x25, 0x4E, //浮力值 - 0x1B, 0x4E, //滑块位置 - 0x00, //探测状态 - 0x00, 0x9C, //目标距离 - 0x42, 0xBD, //目标方位 - 0x07, 0xD0, //目标频率 - 0xC7, 0x16, //CRC16校验码 - 0x0A //帧尾 - }; + + public static final int BITS_OF_HEAD = 0x2A; + public static final int BITS_OF_END = 0x0A; /** * 洋流相关常量 diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java deleted file mode 100644 index dc6ab54..0000000 --- a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DataParser.java +++ /dev/null @@ -1,609 +0,0 @@ -package com.casic.missiles.modular.system.utils; - -import com.alibaba.fastjson.JSON; -import com.alibaba.fastjson.TypeReference; -import com.casic.missiles.modular.system.dto.AisDTO; -import com.casic.missiles.modular.system.dto.EnvironmentDTO; -import com.casic.missiles.modular.system.dto.OceanDetailDTO; -import com.casic.missiles.modular.system.dto.remote.OceanDetailRemoteData; -import com.casic.missiles.modular.system.model.RobotInfo; -import org.locationtech.spatial4j.distance.DistanceUtils; - -import java.text.DecimalFormat; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Map; - -/** - * 西工大数据解析协议 - * - * @author a203 - */ -public class DataParser { - - private static final int BITS_OF_HEAD = 0x2A; - private static final int BITS_OF_END = 0x0A; - //赤道半径(单位km) - private static final double EARTH_RADIUS = 6371.393; - - private static boolean isCorrectData(int[] bytes) { - if (bytes[0] != BITS_OF_HEAD) { - return false; - } - return bytes[bytes.length - 1] == BITS_OF_END; - } - - /** - * 水下机器人信息 - */ - public static RobotInfo decodeRobotInfo(int[] bytes) { - RobotInfo robot = new RobotInfo(); - if (!isCorrectData(bytes)) { - return robot; - } - robot.setRobotId(bytes[4]); - - int[] lngBytes = new int[4]; - System.arraycopy(bytes, 5, lngBytes, 0, 4); - robot.setLng(covertLngLat(lngBytes)); - - int[] latBytes = new int[4]; - System.arraycopy(bytes, 9, latBytes, 0, 4); - robot.setLat(covertLngLat(latBytes)); - - robot.setNavigateState(covertState(bytes[13])); - robot.setInstrumentV((float) (bytes[14] * 0.1)); - robot.setPowerV((float) bytes[15]); - robot.setInstrumentE((float) (bytes[16])); - robot.setPowerE((float) (bytes[17])); - - int[] pitchAngleBytes = new int[2]; - System.arraycopy(bytes, 18, pitchAngleBytes, 0, 2); - robot.setPitchAngle(covertPitchAngle(pitchAngleBytes)); - - int[] rollAngleBytes = new int[2]; - System.arraycopy(bytes, 20, rollAngleBytes, 0, 2); - robot.setRollAngle(covertRollAngle(rollAngleBytes)); - - int[] headingAngleBytes = new int[2]; - System.arraycopy(bytes, 22, headingAngleBytes, 0, 2); - robot.setHeadingAngle(covertHeadingAngle(headingAngleBytes)); - - int[] buoyancyBytes = new int[2]; - System.arraycopy(bytes, 24, buoyancyBytes, 0, 2); - robot.setBuoyancy(covertBuoyancy(buoyancyBytes)); - - int[] positionBytes = new int[2]; - System.arraycopy(bytes, 26, positionBytes, 0, 2); - //滑块位置算法和浮力算法一样 - robot.setPosition(covertBuoyancy(positionBytes)); - - int status = bytes[28]; - if (status == 0) { - robot.setDetectStatus("没有目标"); - } else { - robot.setDetectStatus("发现目标"); - } - - int[] targetDisBytes = new int[2]; - System.arraycopy(bytes, 29, targetDisBytes, 0, 2); - robot.setTargetDistance(covertTargetDistance(targetDisBytes)); - - int[] targetDirBytes = new int[2]; - System.arraycopy(bytes, 31, targetDirBytes, 0, 2); - //目标方位算法和航向角算法一样 - robot.setTargetDirection(covertHeadingAngle(targetDirBytes)); - - int[] targetHzBytes = new int[2]; - System.arraycopy(bytes, 33, targetHzBytes, 0, 2); - robot.setTargetHertz(covertTargetHertz(targetHzBytes)); - return robot; - } - - /** - * 区域覆盖 - */ - public static String decodeRobotRegion(int[] bytes) { - if (!isCorrectData(bytes)) { - return ""; - } - Map regionMap = new HashMap<>(); - int count = bytes[4]; - regionMap.put("count", String.valueOf(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]); - } - } - regionMap.put("robotIds", builder.toString()); - - //经度和纬度，所以需要*2 - int targetSize = count * 2 * 4; - int[] targetBytes = new int[targetSize]; - //起始4个字节，机器人个数1个字节 - System.arraycopy(bytes, 5 + idsBytes.length, targetBytes, 0, targetSize); - regionMap.put("robotTargets", formatLanLat(targetBytes)); - - regionMap.put("pointCount", String.valueOf(bytes[5 + idsBytes.length + targetBytes.length])); - - int[] radiusBytes = new int[2]; - System.arraycopy(bytes, 5 + idsBytes.length + targetBytes.length + 1, radiusBytes, 0, 2); - regionMap.put("regionRadius", covertTargetDistance(radiusBytes)); - - int aByte = bytes[5 + idsBytes.length + targetBytes.length + 1 + radiusBytes.length]; - if (aByte == 0) { - regionMap.put("contour", "轮廓标志位:多边形，" + count + "个轮廓点"); - } else { - regionMap.put("contour", "轮廓标志位:圆形，" + 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); - regionMap.put("robotPoints", formatLanLat(pointBytes)); - return JSON.toJSONString(regionMap); - } - - /** - * 路径规划 - */ - public static String decodeRobotRoute(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 decodeDirectedTask(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 decodeTaskRoute(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 decodeDirectFlightTask(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 decodeResideTask(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 decodePutTask(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); - } - - /** - * 短信内容 - */ - public static String decodeSMS(int[] bytes) { - //帧头、帧长度、帧类型、子类型、机器人ID，CRC16校验码、帧尾总长度=8 - int dataLen = bytes.length - 8; - - int[] dataBytes = new int[dataLen]; - System.arraycopy(bytes, 5, dataBytes, 0, dataLen); - StringBuilder builder = new StringBuilder(); - for (int dataByte : dataBytes) { - builder.append((char) dataByte); - } - return builder.toString(); - } - - /** - * AIS数据请求 - */ - public static String decodeAISRequest(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); - } - - /** - * 方舱位置 - * 编码encode - * 解码decode - */ - public static String decodeShelterPosition(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) { - return ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF) + "hz"; - } - - /** - * 目标距离转换算法 - */ - private static String covertTargetDistance(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01) + "km"; - } - - /** - * 横滚角转换算法 - */ - private static float covertBuoyancy(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 100); - } - - /** - * 横滚角转换算法 - */ - private static float covertHeadingAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01); - } - - /** - * 横滚角转换算法 - */ - private static float covertRollAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 180); - } - - /** - * 俯仰角转换算法 - */ - private static float covertPitchAngle(int[] bytes) { - float a = ((bytes[0] & 0xFF) << 8) + - (bytes[1] & 0xFF); - return (float) (a * 0.01 - 90); - } - - /** - * 航行状态转换 - */ - private static String covertState(int b) { - String state = ""; - switch (b) { - case 0: - state = "任务中"; - break; - case 1: - state = "待机"; - break; - case 2: - state = "故障"; - break; - case 3: - state = "未启用"; - break; - default: - break; - } - return state; - } - - /** - * 经纬度转换算法 - */ - private static double covertLngLat(int[] bytes) { - double per = ( - ((bytes[0] & 0xFF) << 24) + - ((bytes[1] & 0xFF) << 16) + - ((bytes[2] & 0xFF) << 8) + - (bytes[3] & 0xFF) - ) / (Math.pow(2, 31) - 1); - return 180 * per; - } - - /** - * 环境数据 - */ - public static Map encodeEnvironment(String data) { - Map resultMap = createDataHead("AIS"); - resultMap.put("type", "Env"); - - EnvironmentDTO dto = new EnvironmentDTO(); - //台风中心经度 - double lng = 109.322222; - dto.setTyphoonLng(lng); - - //台风中心纬度 - double lat = 17.392232; - dto.setTyphoonLat(lat); - - //台风风力 - int power = 14; - dto.setTyphoonPower(power); - - //台风半径 - double radius = 155.56; - dto.setTyphoonRadius(radius); - - //机器人与台风中心距离 - double distance = distance(115.6314213, 12.9691386, 112.0623457, 19.9936680); - dto.setDistance(distance); - - //机器人与台风的方位角 - double angle = azimuth(115.6314213, 12.9691386, 112.0623457, 19.9936680); - dto.setAzimuth(angle); - - //洋流数据 - String oceanData = HttpRequestHelper.obtainOceanData(115.6314213, 12.9691386, "2021-10-22", "0", "10"); - OceanDetailDTO oceanDetailDTO = null; - if ("0".equals(HttpRequestHelper.getOceanCode(oceanData))) { - OceanDetailRemoteData remoteData = JSON.parseObject(oceanData, new TypeReference() { - }); - oceanDetailDTO = new OceanDetailDTO(remoteData); - } - dto.setOceanDetail(oceanDetailDTO); - - resultMap.put("data", dto); - return resultMap; - } - - /** - * AIS数据 - */ - public static Map encodeAIS(String data) { - Map resultMap = createDataHead("AIS"); - resultMap.put("type", "AIS"); - //船只数量 - int shipCount = 2; - List positionBeans = new ArrayList<>(); - for (int i = 0; i < shipCount; i++) { - AisDTO ais = new AisDTO(); - ais.setCourse(187.2); - ais.setHeading(189.0); - //船只经度 - double lng = 109.322222; - ais.setLng(lng); - //船只纬度 - double lat = 17.392232; - ais.setLat(lat); - ais.setPositionTime("2021-09-29 16:12:25"); - ais.setSpeed(16.3); - - positionBeans.add(ais); - } - resultMap.put("data", positionBeans); - return resultMap; - } - - /** - * 短信数据 - */ - public static Map encodeSMS(String data) { - Map resultMap = createDataHead("SMS"); - resultMap.put("data", data); - return resultMap; - } - - private static Map createDataHead(String type) { - Map resultMap = new HashMap<>(8); - resultMap.put("code", 200); - resultMap.put("type", type); - resultMap.put("message", "请求成功"); - resultMap.put("success", true); - return resultMap; - } - - /** - * 经纬度转弧度 - */ - private static double lnglatToRadian(double d) { - return d * Math.PI / 180.0; - } - - /** - * 经纬度距离计算 - */ - private static double distance(double lonA, double latA, double lonB, double latB) { - double aLng = lnglatToRadian(lonA); - double aLat = lnglatToRadian(latA); - double bLng = lnglatToRadian(lonB); - double bLat = lnglatToRadian(latB); - - double dist = DistanceUtils.distHaversineRAD(aLat, aLng, bLat, bLng) * EARTH_RADIUS; - DecimalFormat df = new DecimalFormat("#.00"); - return Double.parseDouble(df.format(dist)); - } - - /** - * 方位角 - *

- * https://blog.csdn.net/dulingwen/article/details/96868530 - */ - private static int azimuth(double lonA, double latA, double lonB, double latB) { - double deltaLng = lonB - lonA; - - double y = Math.sin(deltaLng) * Math.cos(latB); - double x = Math.cos(latA) * Math.sin(latB) - Math.sin(latA) * Math.cos(latB) * Math.cos(deltaLng); - - double bearing = Math.atan2(y, x); - bearing = Math.toDegrees(bearing); - bearing = (bearing + 360) % 360; - - return (int) bearing; - } -} \ No newline at end of file diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java new file mode 100644 index 0000000..9cb0e99 --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/DecodeData.java @@ -0,0 +1,429 @@ +package com.casic.missiles.modular.system.utils; + +import com.alibaba.fastjson.JSON; +import com.casic.missiles.modular.system.model.RobotInfo; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * 西工大数据解析 + * + * @author a203 + */ +public class DecodeData { + /** + * 水下机器人信息 + */ + public static RobotInfo decodeRobotInfo(int[] bytes) { + RobotInfo robot = new RobotInfo(); + + robot.setRobotId(bytes[4]); + + int[] lngBytes = new int[4]; + System.arraycopy(bytes, 5, lngBytes, 0, 4); + robot.setLng(covertLngLat(lngBytes)); + + int[] latBytes = new int[4]; + System.arraycopy(bytes, 9, latBytes, 0, 4); + robot.setLat(covertLngLat(latBytes)); + + robot.setNavigateState(covertState(bytes[13])); + robot.setInstrumentV((float) (bytes[14] * 0.1)); + robot.setPowerV((float) bytes[15]); + robot.setInstrumentE((float) (bytes[16])); + robot.setPowerE((float) (bytes[17])); + + int[] pitchAngleBytes = new int[2]; + System.arraycopy(bytes, 18, pitchAngleBytes, 0, 2); + robot.setPitchAngle(covertPitchAngle(pitchAngleBytes)); + + int[] rollAngleBytes = new int[2]; + System.arraycopy(bytes, 20, rollAngleBytes, 0, 2); + robot.setRollAngle(covertRollAngle(rollAngleBytes)); + + int[] headingAngleBytes = new int[2]; + System.arraycopy(bytes, 22, headingAngleBytes, 0, 2); + robot.setHeadingAngle(covertHeadingAngle(headingAngleBytes)); + + int[] buoyancyBytes = new int[2]; + System.arraycopy(bytes, 24, buoyancyBytes, 0, 2); + robot.setBuoyancy(covertBuoyancy(buoyancyBytes)); + + int[] positionBytes = new int[2]; + System.arraycopy(bytes, 26, positionBytes, 0, 2); + //滑块位置算法和浮力算法一样 + robot.setPosition(covertBuoyancy(positionBytes)); + + int status = bytes[28]; + if (status == 0) { + robot.setDetectStatus("没有目标"); + } else { + robot.setDetectStatus("发现目标"); + } + + int[] targetDisBytes = new int[2]; + System.arraycopy(bytes, 29, targetDisBytes, 0, 2); + robot.setTargetDistance(covertTargetDistance(targetDisBytes)); + + int[] targetDirBytes = new int[2]; + System.arraycopy(bytes, 31, targetDirBytes, 0, 2); + //目标方位算法和航向角算法一样 + robot.setTargetDirection(covertHeadingAngle(targetDirBytes)); + + int[] targetHzBytes = new int[2]; + System.arraycopy(bytes, 33, targetHzBytes, 0, 2); + robot.setTargetHertz(covertTargetHertz(targetHzBytes)); + return robot; + } + + /** + * 区域覆盖 + */ + public static String decodeRobotRegion(int[] bytes) { + Map regionMap = new HashMap<>(); + int count = bytes[4]; + regionMap.put("count", String.valueOf(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]); + } + } + regionMap.put("robotIds", builder.toString()); + + //经度和纬度，所以需要*2 + int targetSize = count * 2 * 4; + int[] targetBytes = new int[targetSize]; + //起始4个字节，机器人个数1个字节 + System.arraycopy(bytes, 5 + idsBytes.length, targetBytes, 0, targetSize); + regionMap.put("robotTargets", formatLanLat(targetBytes)); + + regionMap.put("pointCount", String.valueOf(bytes[5 + idsBytes.length + targetBytes.length])); + + int[] radiusBytes = new int[2]; + System.arraycopy(bytes, 5 + idsBytes.length + targetBytes.length + 1, radiusBytes, 0, 2); + regionMap.put("regionRadius", covertTargetDistance(radiusBytes)); + + int aByte = bytes[5 + idsBytes.length + targetBytes.length + 1 + radiusBytes.length]; + if (aByte == 0) { + regionMap.put("contour", "轮廓标志位:多边形，" + count + "个轮廓点"); + } else { + regionMap.put("contour", "轮廓标志位:圆形，" + 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); + regionMap.put("robotPoints", formatLanLat(pointBytes)); + return JSON.toJSONString(regionMap); + } + + /** + * 路径规划 + */ + public static String decodeRobotRoute(int[] bytes) { + 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 decodeDirectedTask(int[] bytes) { + 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 decodeTaskRoute(int[] bytes) { + 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 decodeDirectFlightTask(int[] bytes) { + 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 decodeResideTask(int[] bytes) { + 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 decodePutTask(int[] bytes) { + 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); + } + + /** + * 短信内容 + */ + public static String decodeSMS(int[] bytes) { + //帧头、帧长度、帧类型、子类型、机器人ID，CRC16校验码、帧尾总长度=8 + int dataLen = bytes.length - 8; + + int[] dataBytes = new int[dataLen]; + System.arraycopy(bytes, 5, dataBytes, 0, dataLen); + StringBuilder builder = new StringBuilder(); + for (int dataByte : dataBytes) { + builder.append((char) dataByte); + } + return builder.toString(); + } + + /** + * AIS数据请求 + */ + public static String decodeAISRequest(int[] bytes) { + 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); + } + + /** + * 方舱位置 + * 编码encode + * 解码decode + */ + public static String decodeShelterPosition(int[] bytes) { + 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) { + return ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF) + "hz"; + } + + /** + * 目标距离转换算法 + */ + private static String covertTargetDistance(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01) + "km"; + } + + /** + * 横滚角转换算法 + */ + private static float covertBuoyancy(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 100); + } + + /** + * 横滚角转换算法 + */ + private static float covertHeadingAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01); + } + + /** + * 横滚角转换算法 + */ + private static float covertRollAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 180); + } + + /** + * 俯仰角转换算法 + */ + private static float covertPitchAngle(int[] bytes) { + float a = ((bytes[0] & 0xFF) << 8) + + (bytes[1] & 0xFF); + return (float) (a * 0.01 - 90); + } + + /** + * 航行状态转换 + */ + private static String covertState(int b) { + String state = ""; + switch (b) { + case 0: + state = "任务中"; + break; + case 1: + state = "待机"; + break; + case 2: + state = "故障"; + break; + case 3: + state = "未启用"; + break; + default: + break; + } + return state; + } + + /** + * 经纬度转换算法 + */ + private static double covertLngLat(int[] bytes) { + double per = ( + ((bytes[0] & 0xFF) << 24) + + ((bytes[1] & 0xFF) << 16) + + ((bytes[2] & 0xFF) << 8) + + (bytes[3] & 0xFF) + ) / (Math.pow(2, 31) - 1); + return 180 * per; + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java new file mode 100644 index 0000000..bc0e41e --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/modular/system/utils/EncodeData.java @@ -0,0 +1,160 @@ +package com.casic.missiles.modular.system.utils; + +import com.alibaba.fastjson.JSON; +import com.alibaba.fastjson.TypeReference; +import com.casic.missiles.modular.system.dto.AisDTO; +import com.casic.missiles.modular.system.dto.EnvironmentDTO; +import com.casic.missiles.modular.system.dto.OceanDetailDTO; +import com.casic.missiles.modular.system.dto.remote.OceanDetailRemoteData; +import org.locationtech.spatial4j.distance.DistanceUtils; + +import java.text.DecimalFormat; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * 203所数据编码 + * + * @author a203 + */ +public class EncodeData { + /** + * 赤道半径(单位km) + */ + private static final double EARTH_RADIUS = 6371.393; + + /** + * 环境数据 + */ + public static Map encodeEnvironment(String data) { + Map resultMap = createDataHead("AIS"); + resultMap.put("type", "Env"); + resultMap.put("robotID", 0); + + EnvironmentDTO dto = new EnvironmentDTO(); + //台风中心经度 + double lng = 109.322222; + dto.setTyphoonLng(lng); + + //台风中心纬度 + double lat = 17.392232; + dto.setTyphoonLat(lat); + + //台风风力 + int power = 14; + dto.setTyphoonPower(power); + + //台风半径 + double radius = 155.56; + dto.setTyphoonRadius(radius); + + //机器人与台风中心距离 + double distance = distance(115.6314213, 12.9691386, 112.0623457, 19.9936680); + dto.setDistance(distance); + + //机器人与台风的方位角 + double angle = azimuth(115.6314213, 12.9691386, 112.0623457, 19.9936680); + dto.setAzimuth(angle); + + //洋流数据 + String oceanData = HttpRequestHelper.obtainOceanData(115.6314213, 12.9691386, "2021-10-22", "0", "10"); + OceanDetailDTO oceanDetailDTO = null; + if ("0".equals(HttpRequestHelper.getOceanCode(oceanData))) { + OceanDetailRemoteData remoteData = JSON.parseObject(oceanData, new TypeReference() { + }); + oceanDetailDTO = new OceanDetailDTO(remoteData); + } + dto.setOceanDetail(oceanDetailDTO); + + resultMap.put("data", dto); + return resultMap; + } + + /** + * AIS数据 + */ + public static Map encodeAIS(String data) { + Map resultMap = createDataHead("AIS"); + resultMap.put("type", "AIS"); + resultMap.put("robotID", 0); + //船只数量 + int shipCount = 2; + List positionBeans = new ArrayList<>(); + for (int i = 0; i < shipCount; i++) { + AisDTO ais = new AisDTO(); + ais.setCourse(187.2); + ais.setHeading(189.0); + //船只经度 + double lng = 109.322222; + ais.setLng(lng); + //船只纬度 + double lat = 17.392232; + ais.setLat(lat); + ais.setPositionTime("2021-09-29 16:12:25"); + ais.setSpeed(16.3); + + positionBeans.add(ais); + } + resultMap.put("data", positionBeans); + return resultMap; + } + + /** + * 短信数据 + */ + public static Map encodeSMS(String data) { + Map resultMap = createDataHead("SMS"); + resultMap.put("data", data); + return resultMap; + } + + private static Map createDataHead(String type) { + Map resultMap = new HashMap<>(8); + resultMap.put("code", 200); + resultMap.put("type", type); + resultMap.put("message", "请求成功"); + resultMap.put("success", true); + return resultMap; + } + + /** + * 经纬度转弧度 + */ + private static double lnglatToRadian(double d) { + return d * Math.PI / 180.0; + } + + /** + * 经纬度距离计算 + */ + private static double distance(double lonA, double latA, double lonB, double latB) { + double aLng = lnglatToRadian(lonA); + double aLat = lnglatToRadian(latA); + double bLng = lnglatToRadian(lonB); + double bLat = lnglatToRadian(latB); + + double dist = DistanceUtils.distHaversineRAD(aLat, aLng, bLat, bLng) * EARTH_RADIUS; + DecimalFormat df = new DecimalFormat("#.00"); + return Double.parseDouble(df.format(dist)); + } + + /** + * 方位角 + *

+ * https://blog.csdn.net/dulingwen/article/details/96868530 + */ + private static int azimuth(double lonA, double latA, double lonB, double latB) { + double deltaLng = lonB - lonA; + + double y = Math.sin(deltaLng) * Math.cos(latB); + double x = Math.cos(latA) * Math.sin(latB) - Math.sin(latA) * Math.cos(latB) * Math.cos(deltaLng); + + double bearing = Math.atan2(y, x); + bearing = Math.toDegrees(bearing); + bearing = (bearing + 360) % 360; + + return (int) bearing; + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/socket/BootNettyServer.java b/casic-shelter/src/main/java/com/casic/missiles/socket/BootNettyServer.java new file mode 100644 index 0000000..02f5ba2 --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/socket/BootNettyServer.java @@ -0,0 +1,59 @@ +package com.casic.missiles.socket; + +import io.netty.bootstrap.ServerBootstrap; +import io.netty.channel.AdaptiveRecvByteBufAllocator; +import io.netty.channel.ChannelFuture; +import io.netty.channel.ChannelOption; +import io.netty.channel.EventLoopGroup; +import io.netty.channel.nio.NioEventLoopGroup; +import io.netty.channel.socket.SocketChannel; +import io.netty.channel.socket.nio.NioServerSocketChannel; +import lombok.extern.slf4j.Slf4j; + +/** + * @author a203 + */ +@Slf4j +public class BootNettyServer { + public void bind(int port) { + EventLoopGroup bossGroup = new NioEventLoopGroup(1); + EventLoopGroup workerGroup = new NioEventLoopGroup(); + try { + /** + * ServerBootstrap 是一个启动NIO服务的辅助启动类 + */ + ServerBootstrap serverBootstrap = new ServerBootstrap(); + /** + * 设置group，将bossGroup，workerGroup线程组传递到ServerBootstrap + */ + serverBootstrap = serverBootstrap.group(bossGroup, workerGroup); + /** + * ServerSocketChannel是以NIO的selector为基础进行实现的，用来接收新的连接，这里告诉Channel通过NioServerSocketChannel获取新的连接 + */ + serverBootstrap = serverBootstrap.channel(NioServerSocketChannel.class); + serverBootstrap = serverBootstrap.option(ChannelOption.RCVBUF_ALLOCATOR, new AdaptiveRecvByteBufAllocator(64, 10496, 1048576)); + serverBootstrap = serverBootstrap.childOption(ChannelOption.RCVBUF_ALLOCATOR, new AdaptiveRecvByteBufAllocator(64, 10496, 1048576)); + /** + * 设置 I/O处理类,主要用于网络I/O事件，记录日志，编码、解码消息 + */ + serverBootstrap = serverBootstrap.childHandler(new ChannelInitializer()); + log.info("端口已开启，占用" + port + "端口号...."); + /** + * 绑定端口，同步等待成功 + */ + ChannelFuture channelFuture = serverBootstrap.bind(port).sync(); + /** + * 等待服务器监听端口关闭 + */ + channelFuture.channel().closeFuture().sync(); + } catch (InterruptedException e) { + e.printStackTrace(); + } finally { + /** + * 退出，释放线程池资源 + */ + bossGroup.shutdownGracefully(); + workerGroup.shutdownGracefully(); + } + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelHandlerAdapter.java b/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelHandlerAdapter.java new file mode 100644 index 0000000..7aa4a6a --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelHandlerAdapter.java @@ -0,0 +1,119 @@ +package com.casic.missiles.socket; + +import com.casic.missiles.modular.system.utils.Constant; +import com.casic.missiles.modular.system.utils.DecodeData; +import com.casic.missiles.modular.system.utils.EncodeData; +import io.netty.channel.ChannelHandlerContext; +import io.netty.channel.ChannelInboundHandlerAdapter; +import lombok.extern.slf4j.Slf4j; + +import java.net.InetSocketAddress; + +/** + * @author a203 + */ +@Slf4j +public class ChannelHandlerAdapter extends ChannelInboundHandlerAdapter { + + public static boolean isCorrectData(int[] bytes) { + if (bytes[0] != Constant.BITS_OF_HEAD) { + return false; + } + return bytes[bytes.length - 1] == Constant.BITS_OF_END; + } + + @Override + public void channelRead(ChannelHandlerContext ctx, Object msg) { + String temp = msg.toString(); + log.info("channelRead message ===> " + temp); + /** + * 回应客户端 + * */ + String[] dataArray = temp.split(" "); + int[] data = new int[dataArray.length]; + for (int i = 0; i < dataArray.length; i++) { + data[i] = Integer.parseInt(dataArray[i], 16); + } + if (isCorrectData(data)) { + int dataType = data[2]; + /** + * 先解析再回应 + * */ + switch (dataType) { + case 0x01: + //水下机器人信息 + DecodeData.decodeRobotInfo(data); + break; + case 0x02: + //算法结果 + + break; + case 0x03: + //任务数据还需要细分 + + break; + case 0x04: + //短信内容 + DecodeData.decodeSMS(data); + + /** + * Socket返回数据给西工大 + * */ + ctx.write(EncodeData.encodeSMS("")); + break; + case 0x05: + //环境数据请求 + EncodeData.encodeEnvironment(""); + break; + case 0x06: + //AIS数据请求 + EncodeData.encodeAIS(""); + break; + case 0x07: + //方舱位置 + DecodeData.decodeShelterPosition(data); + break; + default: + break; + } + } + } + + @Override + public void channelReadComplete(ChannelHandlerContext ctx) { + ctx.flush(); + } + + @Override + public void exceptionCaught(ChannelHandlerContext ctx, Throwable cause) { + cause.printStackTrace(); + ctx.close();//抛出异常，断开与客户端的连接 + } + + @Override + public void channelActive(ChannelHandlerContext ctx) throws Exception { + super.channelActive(ctx); + ctx.channel().read(); + InetSocketAddress socketAddress = (InetSocketAddress) ctx.channel().remoteAddress(); + String ip = socketAddress.getAddress().getHostAddress(); + log.info("channelActive ===> " + ip); + } + + @Override + public void channelInactive(ChannelHandlerContext ctx) throws Exception { + super.channelInactive(ctx); + InetSocketAddress socketAddress = (InetSocketAddress) ctx.channel().remoteAddress(); + String ip = socketAddress.getAddress().getHostAddress(); + log.info("channelInactive ===> " + ip); + ctx.close(); //断开连接时，必须关闭，否则造成资源浪费，并发量很大情况下可能造成宕机 + } + + @Override + public void userEventTriggered(ChannelHandlerContext ctx, Object evt) throws Exception { + super.userEventTriggered(ctx, evt); + InetSocketAddress socketAddress = (InetSocketAddress) ctx.channel().remoteAddress(); + String ip = socketAddress.getAddress().getHostAddress(); + log.info("userEventTriggered ===> " + ip); + ctx.close();//超时时断开连接 + } +} diff --git a/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelInitializer.java b/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelInitializer.java new file mode 100644 index 0000000..4c9513b --- /dev/null +++ b/casic-shelter/src/main/java/com/casic/missiles/socket/ChannelInitializer.java @@ -0,0 +1,24 @@ +package com.casic.missiles.socket; + +import io.netty.channel.Channel; +import io.netty.handler.codec.string.StringDecoder; +import io.netty.handler.codec.string.StringEncoder; + +/** + * @author a203 + */ +public class ChannelInitializer extends io.netty.channel.ChannelInitializer { + + @Override + protected void initChannel(Channel channel) { + // ChannelOutboundHandler，依照逆序执行 + channel.pipeline().addLast("encoder", new StringEncoder()); + + // 属于ChannelInboundHandler，依照顺序执行 + channel.pipeline().addLast("decoder", new StringDecoder()); + /** + * 自定义ChannelInboundHandlerAdapter + */ + channel.pipeline().addLast(new ChannelHandlerAdapter()); + } +} diff --git a/casic-web/src/main/java/com/casic/missiles/CasicApplication.java b/casic-web/src/main/java/com/casic/missiles/CasicApplication.java index dd2413c..7ad77bb 100644 --- a/casic-web/src/main/java/com/casic/missiles/CasicApplication.java +++ b/casic-web/src/main/java/com/casic/missiles/CasicApplication.java @@ -1,9 +1,14 @@ package com.casic.missiles; +import com.casic.missiles.socket.BootNettyServer; import lombok.extern.slf4j.Slf4j; +import org.springframework.beans.factory.annotation.Value; +import org.springframework.boot.CommandLineRunner; import org.springframework.boot.SpringApplication; import org.springframework.boot.autoconfigure.SpringBootApplication; import org.springframework.cache.annotation.EnableCaching; +import org.springframework.context.annotation.PropertySource; +import org.springframework.scheduling.annotation.Async; import org.springframework.scheduling.annotation.EnableAsync; import org.springframework.scheduling.annotation.EnableScheduling; import org.springframework.transaction.annotation.EnableTransactionManagement; @@ -19,10 +24,24 @@ @EnableTransactionManagement(proxyTargetClass = true) @EnableAsync @Slf4j +@PropertySource("classpath:/config/socket.properties") @EnableScheduling -public class CasicApplication { +public class CasicApplication implements CommandLineRunner { + + @Value("${socket.port}") + private Integer port; + public static void main(String[] args) { SpringApplication.run(CasicApplication.class, args); log.info("CasicApplication is success!"); } + + @Async + @Override + public void run(String... args) { + /** + * 使用异步注解方式启动netty服务端服务 + */ + new BootNettyServer().bind(port); + } } diff --git a/casic-web/src/main/resources/config/socket.properties b/casic-web/src/main/resources/config/socket.properties new file mode 100644 index 0000000..20d5b14 --- /dev/null +++ b/casic-web/src/main/resources/config/socket.properties @@ -0,0 +1 @@ +socket.port=9091 \ No newline at end of file