Merge remote-tracking branch 'origin/master'

uavps-framework/src/main/java/com/uavps/framework/udp/UdpServerService.java

@@ -28,10 +28,10 @@ public class UdpServerService {
 
     private static final Logger log = LoggerFactory.getLogger(UdpServerService.class);
 
-    private UdpConfig udpConfig = SpringUtils.getBean(UdpConfig.class);
+    private final UdpConfig udpConfig = SpringUtils.getBean(UdpConfig.class);
 
 
-    private IUavpsAircraftService uavpsAircraftService = SpringUtils.getBean(IUavpsAircraftService.class);
+    private final IUavpsAircraftService uavpsAircraftService = SpringUtils.getBean(IUavpsAircraftService.class);
 
     private static final DatagramSocket INSTANCE;
 
@@ -56,21 +56,17 @@ public class UdpServerService {
             long initMilli = Instant.now().toEpochMilli();
 
             while (true) {
+
+                //使用数据包接收客户端发送的数据
+                INSTANCE.receive(packet);
                 // 设置初始时间，用于计算每个数据的时间差
                 if(TaskInfo.INSTANCE.getInitFlag()){
                     initMilli = Instant.now().toEpochMilli();
                     log.info("任务Id：{}，任务开始，开始时间是：{}", TaskInfo.INSTANCE.getBizId(), initMilli);
                 }
-
-                //使用数据包接收客户端发送的数据
-                INSTANCE.receive(packet);
                 uavData = new String(data, 0, packet.getLength());
                 byte[] remoteData = UdpDataUtils.hexStringToBytes(uavData);
                 // 将日志条目放入队列
-                // 复制数据以避免后续修改影响
-//                int length = remoteData.length;
-//                byte[] dataCopy = new byte[length];
-//                System.arraycopy(remoteData, 0, dataCopy, 0, length);
                 long lastTime = Instant.now().toEpochMilli() - initMilli;
                 String jsonResult = UdpUtils.getJson(remoteData);
                 TaskInfo.INSTANCE.getLogQueue().add(new TaskInfo.LogEntry(jsonResult, lastTime));
@@ -82,6 +78,4 @@ public class UdpServerService {
             throw new RuntimeException(e);
         }
     }
-
-
 }

uavps-framework/src/main/java/com/uavps/framework/udp/utils/UdpDataUtils.java

@@ -5,7 +5,10 @@ import com.fasterxml.jackson.databind.ObjectMapper;
 import com.uavps.system.domain.UavpsAlgorithmParameter;
 import com.uavps.system.udp.vo.Aircraft;
 import com.uavps.system.udp.vo.AircraftFormation;
+import com.uavps.system.udp.vo.PlatformAircraft;
 import com.uavps.system.udp.vo.UdpData;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
 
 import java.io.BufferedReader;
 import java.io.File;
@@ -18,6 +21,7 @@ import java.util.List;
 
 public class UdpDataUtils {
 
+    protected static final Logger logger = LoggerFactory.getLogger(UdpDataUtils.class);
     // 真实目标数据包
     public static final byte REAL_TARGET_DATA_HEAD_1 = (byte) 0xBB;
     public static final byte REAL_TARGET_DATA_HEAD_2 = (byte) 0x14;
@@ -39,13 +43,14 @@ public class UdpDataUtils {
      */
     public static UdpData parseFrame(byte[] frame) {
         if (frame == null || frame.length < 4) {
+            logger.warn("parseFrame warn, frame: {}", frame);
             return null;
         }
 
         int index = 0;
         byte start_1 = frame[index++];
         byte start_2 = frame[index++];
-
+        UdpData udpData = new UdpData();
         if (start_1 == REAL_TARGET_DATA_HEAD_1 && start_2 == REAL_TARGET_DATA_HEAD_2) {
             // 真实目标数据包
             index++;
@@ -57,9 +62,9 @@ public class UdpDataUtils {
             formation.setAircrafts(aircrafts);
             List<AircraftFormation> targetAircraft = new ArrayList<>();
             targetAircraft.add(formation);
-            UdpData udpData = new UdpData();
             udpData.setBizId(bizId);
             udpData.setFinished("false");
+            udpData.setType(UdpData.REAL_TARGET);
             udpData.setTargetAircraft(targetAircraft);
             return udpData;
         } else if (start_1 == NOISE_TARGET_DATA_HEAD_1 && start_2 == NOISE_TARGET_DATA_HEAD_2) {
@@ -73,9 +78,10 @@ public class UdpDataUtils {
             formation.setAircrafts(aircrafts);
             List<AircraftFormation> targetAircraft = new ArrayList<>();
             targetAircraft.add(formation);
-            UdpData udpData = new UdpData();
+
             udpData.setBizId(bizId);
             udpData.setFinished("false");
+            udpData.setType(UdpData.NOISE_TARGET);
             udpData.setTargetAircraft(targetAircraft);
             return udpData;
         } else if (start_1 == PLATFORM_DATA_HEAD_1 && start_2 == PLATFORM_DATA_HEAD_2) {
@@ -83,9 +89,18 @@ public class UdpDataUtils {
             index++;
             // 数据区
             byte[] data = Arrays.copyOfRange(frame, index, frame.length - 1);
-            parsePlatformData(data);
+
+            PlatformAircraft platformAircraft = parsePlatformData(data);
+            udpData.setBizId(bizId);
+            udpData.setFinished("false");
+            udpData.setType(UdpData.PLANT_UAV);
+            udpData.setPlatformAircraft(platformAircraft);
+            return udpData;
+        } else {
+            logger.warn("parseFrame warn, frame: {}", frame);
+            return null;
         }
-        return null;
+
     }
 
     public static List<Aircraft> parseTargetData(byte[] data) {
@@ -150,36 +165,62 @@ public class UdpDataUtils {
         return aircrafts;
     }
 
-    public static void parsePlatformData(byte[] data) {
+    public static PlatformAircraft parsePlatformData(byte[] data) {
+        PlatformAircraft platformAircraft = new PlatformAircraft();
         byte[] xByte = BitUtils.copyFrom(data, 0, 4);
-        //System.out.println(BitUtils.toHexString(xByte));
         double x = BitUtils.toInt(xByte) * 0.0000001;
-        //System.out.println(x);
+        platformAircraft.setLongitude(x);
         byte[] yByte = BitUtils.copyFrom(data, 4, 4);
-        //System.out.println(BitUtils.toHexString(yByte));
         double y = BitUtils.toInt(yByte) * 0.0000001;
-        //System.out.println(y);
+        platformAircraft.setLatitude(y);
         byte[] hByte = BitUtils.copyFrom(data, 8, 2);
-        //System.out.println(BitUtils.toHexString(hByte));
         short h = BitUtils.toShort(hByte);
-        //System.out.println(h);
-        byte[] dvByte = BitUtils.copyFrom(data, 16, 2);
-        //System.out.println(BitUtils.toHexString(dvByte));
-        double dv = BitUtils.toShort(dvByte) * 0.1;
-        //System.out.println(dv);
-        byte[] bvByte = BitUtils.copyFrom(data, 18, 2);
-        //System.out.println(BitUtils.toHexString(bvByte));
-        double bv = BitUtils.toShort(bvByte) * 0.1;
-        //System.out.println(bv);
-        byte[] tvByte = BitUtils.copyFrom(data, 20, 2);
-        //System.out.println(BitUtils.toHexString(tvByte));
-        double tv = BitUtils.toShort(tvByte) * 0.1;
-        //System.out.println(tv);
-        byte[] hjjByte = BitUtils.copyFrom(data, 22, 2);
-        //System.out.println(BitUtils.toHexString(hjjByte));
-        double hjj = BitUtils.toShort(hjjByte) * 0.01;
-        //System.out.println(hjj);
-        //System.out.println("x: " + doubleToFloat(x) + " y: " + doubleToFloat(y) + " h: " + h + " dv: " + dv + " bv: " + bv + " tv: " + tv + " hjj: " + hjj);
+        platformAircraft.setAltitude(h);
+
+        byte[] azimuthAngleByte = BitUtils.copyFrom(data, 10, 2);
+        short azimuthAngle = BitUtils.toShort(azimuthAngleByte);
+        platformAircraft.setAzimuthAngle(azimuthAngle);
+
+        byte[] pitchAngleByte = BitUtils.copyFrom(data, 12, 2);
+        short pitchAngle = BitUtils.toShort(pitchAngleByte);
+        platformAircraft.setPitchAngle(pitchAngle);
+
+        byte[] rollAngleByte = BitUtils.copyFrom(data, 14, 2);
+        short rollAngle = BitUtils.toShort(rollAngleByte);
+        platformAircraft.setRollAngle(rollAngle);
+
+        byte[] eastSpeedByte = BitUtils.copyFrom(data, 16, 2);
+        short eastSpeed = BitUtils.toShort(eastSpeedByte);
+        platformAircraft.setEastSpeed(eastSpeed);
+
+        byte[] northSpeedByte = BitUtils.copyFrom(data, 18, 2);
+        short northSpeed = BitUtils.toShort(northSpeedByte);
+        platformAircraft.setNorthSpeed(northSpeed);
+
+        byte[] skySpeedByte = BitUtils.copyFrom(data, 20, 2);
+        short skySpeed = BitUtils.toShort(skySpeedByte);
+        platformAircraft.setSkySpeed(skySpeed);
+
+        byte[] trackAngleByte = BitUtils.copyFrom(data, 22, 2);
+        short trackAngle = BitUtils.toShort(trackAngleByte);
+        platformAircraft.setTrackAngle(trackAngle);
+
+        byte[] yearByte = BitUtils.copyFrom(data, 24, 2);
+        short year = BitUtils.toShort(yearByte);
+        byte[] monthByte = BitUtils.copyFrom(data, 26, 1);
+        int month = BitUtils.toInt(monthByte);
+        byte[] dayByte = BitUtils.copyFrom(data, 27, 1);
+        int day = BitUtils.toInt(dayByte);
+        byte[] hourByte = BitUtils.copyFrom(data, 28, 1);
+        int hour = BitUtils.toInt(hourByte);
+        byte[] minByte = BitUtils.copyFrom(data, 29, 1);
+        int min = BitUtils.toInt(minByte);
+        byte[] secondByte = BitUtils.copyFrom(data, 30, 1);
+        int second = BitUtils.toInt(secondByte);
+        byte[] millisecondByte = BitUtils.copyFrom(data, 31, 1);
+        int millisecond = BitUtils.toInt(millisecondByte);
+        platformAircraft.setUtcTime(String.format("%04d/%02d/%02d %02d:%02d:%02d %03d", year, month, day, hour, min, second, millisecond));
+        return platformAircraft;
     }
 
     public static Aircraft parseTargetSubData(byte[] data) {

uavps-framework/src/main/java/com/uavps/framework/utils/UdpUtils.java

@@ -60,6 +60,7 @@ public class UdpUtils {
         //byte[] remoteData = packet.getData();
         String json = "";
         if (remoteData != null && remoteData.length > 3) {
+            // 封装成对象
             UdpData udpData = UdpDataUtils.parseFrame(remoteData);
             if (udpData != null) {
 //                        UdpDataUtils.time += 1;
@@ -84,9 +85,6 @@ public class UdpUtils {
     }
 
     private static void getNewXYAircraftCoordinate(Aircraft aircraft) {
-        aircraft.setCoordinateOX(aircraft.getCoordinateX());
-        aircraft.setCoordinateOY(aircraft.getCoordinateY());
-        aircraft.setCoordinateOZ(aircraft.getCoordinateZ());
         aircraft.setCoordinateX((aircraft.getCoordinateX()- TaskInfo.INSTANCE.getX())/ TaskInfo.INSTANCE.getConversionRate());
         aircraft.setCoordinateY((aircraft.getCoordinateY()- TaskInfo.INSTANCE.getY())/ TaskInfo.INSTANCE.getConversionRate());
     }

uavps-framework/src/main/java/com/uavps/framework/websocket/WebSocketServer.java

@@ -58,8 +58,6 @@ public class WebSocketServer {
     @OnOpen
     public void onOpen(Session session) throws Exception {
         log.info("\nWebSocket建立连接 - {}", session);
-        closeUdpServer();
-
         //启动服务，接收算法返回的数据
         CompletableFuture.runAsync(() -> {
             udpServerService.receive(session);
@@ -243,18 +241,6 @@ public class WebSocketServer {
     }
 
     private void closeUdpServer() {
-        try {
-            DatagramSocket socket = new DatagramSocket();
-            //封装数据
-            byte[] data = "EXIT".getBytes();
-            DatagramPacket packet = new DatagramPacket(data, data.length, InetAddress.getLocalHost(), 9099);
-
-            //发送数据
-            socket.send(packet);
-            UdpDataUtils.bizId = "";
-            UdpDataUtils.time = 0;
-        } catch (IOException e) {
-            log.error("\n关闭UDP服务异常: ", e);
-        }
+        // do noting
     }
 }

uavps-system/src/main/java/com/uavps/system/udp/vo/Aircraft.java

@@ -11,9 +11,6 @@ public class Aircraft {
     private float coordinateX;
     private float coordinateY;
     private float coordinateZ;
-    private float coordinateOX;
-    private float coordinateOY;
-    private float coordinateOZ;
     private float velocityX;
     private float velocityY;
     private float velocityZ;
@@ -182,27 +179,4 @@ public class Aircraft {
         this.type = type;
     }
 
-    public float getCoordinateOX() {
-        return coordinateOX;
-    }
-
-    public void setCoordinateOX(float coordinateOX) {
-        this.coordinateOX = coordinateOX;
-    }
-
-    public float getCoordinateOY() {
-        return coordinateOY;
-    }
-
-    public void setCoordinateOY(float coordinateOY) {
-        this.coordinateOY = coordinateOY;
-    }
-
-    public float getCoordinateOZ() {
-        return coordinateOZ;
-    }
-
-    public void setCoordinateOZ(float coordinateOZ) {
-        this.coordinateOZ = coordinateOZ;
-    }
 }

uavps-system/src/main/java/com/uavps/system/udp/vo/PlatformAircraft.java

@@ -9,113 +9,142 @@ import java.util.List;
  * @Version: 1.0
  **/
 public class PlatformAircraft {
-    private float coordinateX;
-    private float coordinateY;
-    private float coordinateZ;
-    private float velocityX;
-    private float velocityY;
-    private float velocityZ;
-    private float velocity;
-    private float aircraftPoseX;
-    private float aircraftPoseY;
-    private float aircraftPoseZ;
-    private String targetAircraftNumber;
 
-    private List<FlightPath> flightpath;
+    // 经度
+    private double longitude;
+
+    // 纬度
+    private double latitude;
+
+    // 高度
+    private double altitude;
+
+    // 方位角
+    private double azimuthAngle;
+    // 仰视角
+    private double pitchAngle;
+    //横滚角
+    private double rollAngle;
+    //向东速度
+    private double eastSpeed;
+
+    // 向北速度
+    private double northSpeed;
+
+    // 向天速度
+    private double skySpeed;
+
+    // 航迹角
+    private short trackAngle;
+
+    private String utcTime;
+//    private String targetAircraftNumber;
+//
+//    private List<FlightPath> flightpath;
 
-    public float getCoordinateX() {
-        return coordinateX;
-    }
 
-    public void setCoordinateX(float coordinateX) {
-        this.coordinateX = coordinateX;
+    public double getLongitude() {
+        return longitude;
     }
 
-    public float getCoordinateY() {
-        return coordinateY;
+    public void setLongitude(double longitude) {
+        this.longitude = longitude;
     }
 
-    public void setCoordinateY(float coordinateY) {
-        this.coordinateY = coordinateY;
+    public double getLatitude() {
+        return latitude;
     }
 
-    public float getCoordinateZ() {
-        return coordinateZ;
+    public void setLatitude(double latitude) {
+        this.latitude = latitude;
     }
 
-    public void setCoordinateZ(float coordinateZ) {
-        this.coordinateZ = coordinateZ;
+    public double getAltitude() {
+        return altitude;
     }
 
-    public float getVelocityX() {
-        return velocityX;
+    public void setAltitude(double altitude) {
+        this.altitude = altitude;
     }
 
-    public void setVelocityX(float velocityX) {
-        this.velocityX = velocityX;
+    public double getEastSpeed() {
+        return eastSpeed;
     }
 
-    public float getVelocityY() {
-        return velocityY;
+    public void setEastSpeed(double eastSpeed) {
+        this.eastSpeed = eastSpeed;
     }
 
-    public void setVelocityY(float velocityY) {
-        this.velocityY = velocityY;
+    public double getNorthSpeed() {
+        return northSpeed;
     }
 
-    public float getVelocityZ() {
-        return velocityZ;
+    public void setNorthSpeed(double northSpeed) {
+        this.northSpeed = northSpeed;
     }
 
-    public void setVelocityZ(float velocityZ) {
-        this.velocityZ = velocityZ;
+    public double getSkySpeed() {
+        return skySpeed;
     }
 
-    public float getVelocity() {
-        return velocity;
+    public void setSkySpeed(double skySpeed) {
+        this.skySpeed = skySpeed;
     }
 
-    public void setVelocity(float velocity) {
-        this.velocity = velocity;
+    public short getTrackAngle() {
+        return trackAngle;
     }
 
-    public float getAircraftPoseX() {
-        return aircraftPoseX;
+    public void setTrackAngle(short trackAngle) {
+        this.trackAngle = trackAngle;
     }
 
-    public void setAircraftPoseX(float aircraftPoseX) {
-        this.aircraftPoseX = aircraftPoseX;
+    public String getUtcTime() {
+        return utcTime;
     }
 
-    public float getAircraftPoseY() {
-        return aircraftPoseY;
+    public void setUtcTime(String utcTime) {
+        this.utcTime = utcTime;
     }
 
-    public void setAircraftPoseY(float aircraftPoseY) {
-        this.aircraftPoseY = aircraftPoseY;
+    public double getAzimuthAngle() {
+        return azimuthAngle;
     }
 
-    public float getAircraftPoseZ() {
-        return aircraftPoseZ;
+    public void setAzimuthAngle(double azimuthAngle) {
+        this.azimuthAngle = azimuthAngle;
     }
 
-    public void setAircraftPoseZ(float aircraftPoseZ) {
-        this.aircraftPoseZ = aircraftPoseZ;
+    public double getPitchAngle() {
+        return pitchAngle;
     }
 
-    public String getTargetAircraftNumber() {
-        return targetAircraftNumber;
+    public void setPitchAngle(double pitchAngle) {
+        this.pitchAngle = pitchAngle;
     }
 
-    public void setTargetAircraftNumber(String targetAircraftNumber) {
-        this.targetAircraftNumber = targetAircraftNumber;
+    public double getRollAngle() {
+        return rollAngle;
     }
 
-    public List<FlightPath> getFlightpath() {
-        return flightpath;
+    public void setRollAngle(double rollAngle) {
+        this.rollAngle = rollAngle;
     }
 
-    public void setFlightpath(List<FlightPath> flightpath) {
-        this.flightpath = flightpath;
+    @Override
+    public String toString() {
+        return "PlatformAircraft{" +
+                "longitude=" + longitude +
+                ", latitude=" + latitude +
+                ", altitude=" + altitude +
+                ", azimuthAngle=" + azimuthAngle +
+                ", pitchAngle=" + pitchAngle +
+                ", rollAngle=" + rollAngle +
+                ", eastSpeed=" + eastSpeed +
+                ", northSpeed=" + northSpeed +
+                ", skySpeed=" + skySpeed +
+                ", trackAngle=" + trackAngle +
+                ", utcTime='" + utcTime + '\'' +
+                '}';
     }
 }

uavps-system/src/main/java/com/uavps/system/udp/vo/UdpData.java

@@ -9,11 +9,17 @@ import java.util.List;
  * @Version: 1.0
  **/
 public class UdpData {
+    public static final String PLANT_UAV = "1"; //平台无人机
+    public static final String REAL_TARGET = "2"; // 真实目标
+    public static final String NOISE_TARGET = "3"; // 噪声目标
+
     //业务id，代表一次仿真任务的唯一标识
     private String bizId;
 
     //是否结束 true代表本次bizId的任务已结束，以后接收该id的数据都会废弃
     private String finished;
+    // 数据类型1=平台无人机，2=真实目标，3=噪声目标
+    private String type;
 
     //时间单位ms，代表本次数据的时间
     private float time;
@@ -62,6 +68,14 @@ public class UdpData {
         this.platformAircraft = platformAircraft;
     }
 
+    public String getType() {
+        return type;
+    }
+
+    public void setType(String type) {
+        this.type = type;
+    }
+
     public String getTargetAircraftsString() {
         String returnString = "";
         if (targetAircraft!= null) {