修改平台无人机

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

@@ -11,10 +11,10 @@ public enum TaskInfo {
     INSTANCE(0.0f, 0.0f, true, 1.0f);
 
     // 枚举字段
-    private float x;
-    private float y;
+    private double x;
+    private double y;
     private boolean initFlag;
-    private float conversionRate;
+    private double conversionRate;
     private final int initCenterPixel = 500;
     private Long bizId;
     private String filePath;
@@ -39,7 +39,7 @@ public enum TaskInfo {
         this.x = 0f;
         this.y = 0f;
         this.initFlag = true;
-        this.conversionRate = 0.00001f;
+        this.conversionRate = 0.0001;
         this.bizId = bizId;
         this.filePath = filePath;
         logQueue.clear();
@@ -106,19 +106,19 @@ public enum TaskInfo {
     }
 
     // Getter和Setter方法
-    public float getX() {
+    public double getX() {
         return x;
     }
 
-    public void setX(float x) {
+    public void setX(double x) {
         this.x = x;
     }
 
-    public float getY() {
+    public double getY() {
         return y;
     }
 
-    public void setY(float y) {
+    public void setY(double y) {
         this.y = y;
     }
 
@@ -130,7 +130,7 @@ public enum TaskInfo {
         this.initFlag = initFlag;
     }
 
-    public float getConversionRate() {
+    public double getConversionRate() {
         return conversionRate;
     }
 
@@ -139,11 +139,11 @@ public enum TaskInfo {
     }
 
     // 转换坐标方法
-    public float convertValue(float value) {
+    public double convertValue(float value) {
         return value * conversionRate;
     }
 
-    public float getConversionCenterPixel() {
+    public double getConversionCenterPixel() {
         return initCenterPixel * conversionRate;
     }
 

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

@@ -1,5 +1,6 @@
 package com.uavps.framework.udp;
 
+import com.uavps.common.utils.StringUtils;
 import com.uavps.common.utils.spring.SpringUtils;
 import com.uavps.framework.config.UdpConfig;
 import com.uavps.framework.udp.utils.UdpDataUtils;
@@ -69,6 +70,9 @@ public class UdpServerService {
                 // 将日志条目放入队列
                 long lastTime = Instant.now().toEpochMilli() - initMilli;
                 String jsonResult = UdpUtils.getJson(remoteData);
+                if(StringUtils.isEmpty(jsonResult)){ // 如果解析完数据为null 则跳过
+                    continue;
+                }
                 TaskInfo.INSTANCE.getLogQueue().add(new TaskInfo.LogEntry(jsonResult, lastTime));
                 TaskInfo.INSTANCE.setLastTime(lastTime);
                 session.getBasicRemote().sendText(jsonResult);

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

@@ -211,11 +211,9 @@ public class BitUtils {
      */
 
     public static short toShort(byte[] bytes) {
-        if (isLittleEndian()) {
-            return (short) ((0xff & bytes[0]) | (0xff00 & (bytes[1] << 8)));
-        } else {
-            return (short) ((0xff & bytes[1]) | (0xff00 & (bytes[0] << 8)));
-        }
+        ByteBuffer buffer = ByteBuffer.wrap(bytes);
+        buffer.order(ByteOrder.LITTLE_ENDIAN); // 设置为小端模式
+        return buffer.getShort();
     }
 
     /**
@@ -238,11 +236,9 @@ public class BitUtils {
      */
 
     public static char toChar(byte[] bytes) {
-        if (isLittleEndian()) {
-            return (char) ((0xff & bytes[0]) | (0xff00 & (bytes[1] << 8)));
-        } else {
-            return (char) ((0xff & bytes[1]) | (0xff00 & (bytes[0] << 8)));
-        }
+        ByteBuffer buffer = ByteBuffer.wrap(bytes);
+        buffer.order(ByteOrder.LITTLE_ENDIAN); // 设置为小端模式
+        return buffer.getChar();
     }
 
     /**
@@ -265,17 +261,6 @@ public class BitUtils {
      */
 
     public static int toInt(byte[] bytes) {
-//        if (isLittleEndian()) {
-//            return (0xff & bytes[0])
-//                    | (0xff00 & (bytes[1] << 8))
-//                    | (0xff0000 & (bytes[2] << 16))
-//                    | (0xff000000 & (bytes[3] << 24));
-//        } else {
-//            return (0xff & bytes[3])
-//                    | (0xff00 & (bytes[2] << 8))
-//                    | (0xff0000 & (bytes[1] << 16))
-//                    | (0xff000000 & (bytes[0] << 24));
-//        }
         ByteBuffer buffer = ByteBuffer.wrap(bytes);
         buffer.order(ByteOrder.LITTLE_ENDIAN); // 设置为小端模式
         return buffer.getInt();
@@ -301,25 +286,9 @@ public class BitUtils {
      */
 
     public static long toLong(byte[] bytes) {
-        if (isLittleEndian()) {
-            return (0xffL & (long) bytes[0])
-                    | (0xff00L & ((long) bytes[1] << 8))
-                    | (0xff0000L & ((long) bytes[2] << 16))
-                    | (0xff000000L & ((long) bytes[3] << 24))
-                    | (0xff00000000L & ((long) bytes[4] << 32))
-                    | (0xff0000000000L & ((long) bytes[5] << 40))
-                    | (0xff000000000000L & ((long) bytes[6] << 48))
-                    | (0xff00000000000000L & ((long) bytes[7] << 56));
-        } else {
-            return (0xffL & (long) bytes[7])
-                    | (0xff00L & ((long) bytes[6] << 8))
-                    | (0xff0000L & ((long) bytes[5] << 16))
-                    | (0xff000000L & ((long) bytes[4] << 24))
-                    | (0xff00000000L & ((long) bytes[3] << 32))
-                    | (0xff0000000000L & ((long) bytes[2] << 40))
-                    | (0xff000000000000L & ((long) bytes[1] << 48))
-                    | (0xff00000000000000L & ((long) bytes[0] << 56));
-        }
+        ByteBuffer buffer = ByteBuffer.wrap(bytes);
+        buffer.order(ByteOrder.LITTLE_ENDIAN); // 设置为小端模式
+        return buffer.getLong();
     }
 
     /**

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

@@ -178,48 +178,48 @@ public class UdpDataUtils {
         platformAircraft.setAltitude(h);
 
         byte[] azimuthAngleByte = BitUtils.copyFrom(data, 10, 2);
-        short azimuthAngle = BitUtils.toShort(azimuthAngleByte);
+        double azimuthAngle = BitUtils.toShort(azimuthAngleByte) * 0.01 ;
         platformAircraft.setAzimuthAngle(azimuthAngle);
 
         byte[] pitchAngleByte = BitUtils.copyFrom(data, 12, 2);
-        short pitchAngle = BitUtils.toShort(pitchAngleByte);
+        double pitchAngle = BitUtils.toShort(pitchAngleByte) * 0.01;
         platformAircraft.setPitchAngle(pitchAngle);
 
         byte[] rollAngleByte = BitUtils.copyFrom(data, 14, 2);
-        short rollAngle = BitUtils.toShort(rollAngleByte);
+        double rollAngle = BitUtils.toShort(rollAngleByte) * 0.01;
         platformAircraft.setRollAngle(rollAngle);
 
         byte[] eastSpeedByte = BitUtils.copyFrom(data, 16, 2);
-        short eastSpeed = BitUtils.toShort(eastSpeedByte);
+        double eastSpeed = BitUtils.toShort(eastSpeedByte) * 0.1;
         platformAircraft.setEastSpeed(eastSpeed);
 
         byte[] northSpeedByte = BitUtils.copyFrom(data, 18, 2);
-        short northSpeed = BitUtils.toShort(northSpeedByte);
+        double northSpeed = BitUtils.toShort(northSpeedByte) * 0.1;
         platformAircraft.setNorthSpeed(northSpeed);
 
         byte[] skySpeedByte = BitUtils.copyFrom(data, 20, 2);
-        short skySpeed = BitUtils.toShort(skySpeedByte);
+        double skySpeed = BitUtils.toShort(skySpeedByte) * 0.1;
         platformAircraft.setSkySpeed(skySpeed);
 
         byte[] trackAngleByte = BitUtils.copyFrom(data, 22, 2);
-        short trackAngle = BitUtils.toShort(trackAngleByte);
+        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));
+//        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;
     }
 
@@ -316,11 +316,6 @@ public class UdpDataUtils {
         double tv = BitUtils.toShort(tvByte) * 0.1;
         af.setVelocityZ(doubleToFloat(tv));
         af.setSkySpeed(tv);
-        //System.out.println(tv);
-        //System.out.println(id + " " + x + " " + y + " " + h + " " + dv + " " + bv + " " + tv + " ");
-        if(id == 1){
-            System.out.println(id + " = " + ix + "," + iy + " " + " | " + af.getCoordinateX() + "," + af.getCoordinateY());
-        }
         return af;
     }
 

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

@@ -63,19 +63,29 @@ public class UdpUtils {
             // 封装成对象
             UdpData udpData = UdpDataUtils.parseFrame(remoteData);
             if (udpData != null) {
-//                        UdpDataUtils.time += 1;
-//                        udpData.setTime(UdpDataUtils.time);
-                if(udpData.getTargetAircraft() != null){
-                    for (AircraftFormation aircraftFormation : udpData.getTargetAircraft()) {
-                        for (Aircraft aircraft : aircraftFormation.getAircrafts()) {
-                            if(TaskInfo.INSTANCE.getInitFlag()){
-                                TaskInfo.INSTANCE.setX(aircraft.getCoordinateX() - TaskInfo.INSTANCE.getConversionCenterPixel());
-                                TaskInfo.INSTANCE.setY(aircraft.getCoordinateY() - TaskInfo.INSTANCE.getConversionCenterPixel());
-                                TaskInfo.INSTANCE.setInitFlag(false);
+                PlatformAircraft platformAircraft = udpData.getPlatformAircraft();
+                // 第一次数据是平台无人机的数据，基于平台无人机数据进行初始化xy坐标为中心坐标
+                if (TaskInfo.INSTANCE.getInitFlag() && UdpData.PLANT_UAV.equals(udpData.getType()) && null != platformAircraft) {
+                    // 初始化基坐标
+                    TaskInfo.INSTANCE.setX( (platformAircraft.getLongitude() - TaskInfo.INSTANCE.getConversionCenterPixel()));
+                    TaskInfo.INSTANCE.setY( (platformAircraft.getLatitude() - TaskInfo.INSTANCE.getConversionCenterPixel()));
+                    TaskInfo.INSTANCE.setInitFlag(false);
+                    getNewXYAircraftCoordinate(platformAircraft);
+                } else if (!TaskInfo.INSTANCE.getInitFlag() && UdpData.PLANT_UAV.equals(udpData.getType()) && null != platformAircraft) {
+                    // 基于初始化基坐标计算平台无人机坐标
+                    getNewXYAircraftCoordinate(platformAircraft);
+                } else if (!TaskInfo.INSTANCE.getInitFlag() && !UdpData.PLANT_UAV.equals(udpData.getType()) && udpData.getTargetAircraft() != null) {
+                    // 基于初始化基坐标计算目标飞机坐标
+                    if (udpData.getTargetAircraft() != null) {
+                        for (AircraftFormation aircraftFormation : udpData.getTargetAircraft()) {
+                            for (Aircraft aircraft : aircraftFormation.getAircrafts()) {
+                                getNewXYAircraftCoordinate(aircraft);
                             }
-                            getNewXYAircraftCoordinate(aircraft);
                         }
                     }
+                } else {
+                    //其他情况轮空
+                    return null;
                 }
                 ObjectMapper mapper = new ObjectMapper();
                 json = mapper.writeValueAsString(udpData);
@@ -84,6 +94,11 @@ public class UdpUtils {
         return json;
     }
 
+    private static void getNewXYAircraftCoordinate(PlatformAircraft platformAircraft) {
+        platformAircraft.setCoordinateX(((platformAircraft.getLongitude()- TaskInfo.INSTANCE.getX())/ TaskInfo.INSTANCE.getConversionRate()));
+        platformAircraft.setCoordinateY(((platformAircraft.getLatitude()- TaskInfo.INSTANCE.getY())/ TaskInfo.INSTANCE.getConversionRate()));
+    }
+
     private static void getNewXYAircraftCoordinate(Aircraft aircraft) {
         aircraft.setCoordinateX((aircraft.getCoordinateX()- TaskInfo.INSTANCE.getX())/ TaskInfo.INSTANCE.getConversionRate());
         aircraft.setCoordinateY((aircraft.getCoordinateY()- TaskInfo.INSTANCE.getY())/ TaskInfo.INSTANCE.getConversionRate());

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

@@ -8,9 +8,9 @@ package com.uavps.system.udp.vo;
  **/
 public class Aircraft {
     private String aircraftNumber;
-    private float coordinateX;
-    private float coordinateY;
-    private float coordinateZ;
+    private double coordinateX;
+    private double coordinateY;
+    private double coordinateZ;
     private float velocityX;
     private float velocityY;
     private float velocityZ;
@@ -35,23 +35,23 @@ public class Aircraft {
         this.aircraftNumber = aircraftNumber;
     }
 
-    public float getCoordinateX() {
+    public double getCoordinateX() {
         return coordinateX;
     }
 
-    public void setCoordinateX(float coordinateX) {
+    public void setCoordinateX(double coordinateX) {
         this.coordinateX = coordinateX;
     }
 
-    public float getCoordinateY() {
+    public double getCoordinateY() {
         return coordinateY;
     }
 
-    public void setCoordinateY(float coordinateY) {
+    public void setCoordinateY(double coordinateY) {
         this.coordinateY = coordinateY;
     }
 
-    public float getCoordinateZ() {
+    public double getCoordinateZ() {
         return coordinateZ;
     }
 

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

@@ -19,6 +19,10 @@ public class PlatformAircraft {
     // 高度
     private double altitude;
 
+    private double coordinateX;
+    private double coordinateY;
+    private double coordinateZ;
+
     // 方位角
     private double azimuthAngle;
     // 仰视角
@@ -131,12 +135,39 @@ public class PlatformAircraft {
         this.rollAngle = rollAngle;
     }
 
+    public double getCoordinateX() {
+        return coordinateX;
+    }
+
+    public void setCoordinateX(double coordinateX) {
+        this.coordinateX = coordinateX;
+    }
+
+    public double getCoordinateY() {
+        return coordinateY;
+    }
+
+    public void setCoordinateY(double coordinateY) {
+        this.coordinateY = coordinateY;
+    }
+
+    public double getCoordinateZ() {
+        return coordinateZ;
+    }
+
+    public void setCoordinateZ(double coordinateZ) {
+        this.coordinateZ = coordinateZ;
+    }
+
     @Override
     public String toString() {
         return "PlatformAircraft{" +
                 "longitude=" + longitude +
                 ", latitude=" + latitude +
                 ", altitude=" + altitude +
+                ", coordinateX=" + coordinateX +
+                ", coordinateY=" + coordinateY +
+                ", coordinateZ=" + coordinateZ +
                 ", azimuthAngle=" + azimuthAngle +
                 ", pitchAngle=" + pitchAngle +
                 ", rollAngle=" + rollAngle +