增加GGA的解析

sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/Gga.java

@@ -1,9 +1,19 @@
 package com.imdroid.sideslope.bd;
 
+import io.netty.buffer.ByteBuf;
 import lombok.Data;
 
+import java.nio.charset.Charset;
+
 @Data
 public class Gga {
+    final static byte[] GGA_FLAG =  {'$', 'G'};//$xxGGA
+
+    final static byte[] d331_flag = {(byte) 0xd3, (byte)0x31};
+    final static byte rtcm_flag = (byte)0xd3;
+    final static byte[] d341_flag = {(byte) 0xd3, (byte)0x41};
+    final static byte[] b562_flag = {(byte) 0xb5, (byte)0x62};
+
     /**
      * 纬度
      */
@@ -22,11 +32,109 @@ public class Gga {
     /**
      * GPS状态，0初始化，1单点定位，2码差分，3无效PPS，4固定解，5浮点解，6正在估算7，人工输入固定值，8模拟模式，9WAAS差分
      */
-    private int status;
+    private int quality;
+    private int satellitesInUsed;
 
     public Gga() {
     }
 
+    static public Gga getFrom(ByteBuf data){
+        int msgFlag = data.getUnsignedShort(0);
+        if(msgFlag == 0xD341){
+            return getFromD341(data);
+        }
+        else if(msgFlag == 0xD331){
+            return getFromD331(data);
+        }
+        else return null;
+
+    }
+
+    static Gga getFromD331(ByteBuf data){
+        int pos = 26; // 从惯导之后开始
+        boolean found = false;
+        while(pos<data.readableBytes()){
+            byte d0 = data.getByte(pos);
+            byte d1 = data.getByte(pos+1);
+            if(GGA_FLAG[0] == d0 && GGA_FLAG[1] == d1){
+                found = true;
+                break;
+            }
+            else if(d0 == rtcm_flag){
+                if(d1 == 0x31) { // d331粘包
+                    pos += 26;
+                }
+                else {
+                    pos++;
+                    pos += data.getUnsignedShort(pos) + 5;//length+payload+crc
+                }
+            }
+            else{
+                break;
+            }
+        }
+        if(found){
+            return getFromGGAString(data, pos);
+        }
+        else return null;
+    }
+
+    static Gga getFromD341(ByteBuf data){
+        int pos = 26; // 从惯导之后开始
+        boolean found = false;
+        while(pos<data.readableBytes()){
+            byte d0 = data.getByte(pos);
+            byte d1 = data.getByte(pos+1);
+            if(GGA_FLAG[0] == d0 && GGA_FLAG[1] == d1){
+                found = true;
+                break;
+            }
+            else if(b562_flag[0] == d0 && b562_flag[1] == d1){
+                pos += 4; // type, class id
+                pos += data.getUnsignedShortLE(pos) + 4;//length+payload+crc
+            }
+            else{
+                break;
+            }
+        }
+        if(found){
+            return getFromGGAString(data, pos);
+        }
+        else return null;
+    }
+
+    static Gga getFromGGAString(ByteBuf data, int pos){
+        String ggaStr = null;
+        int length = data.readableBytes();
+        for(int i=pos; i<length-1; i++) {
+            byte d0 = data.getByte(i);
+            byte d1 = data.getByte(i + 1);
+            if ((byte)0xd == d0 && (byte)0xa == d1) {
+                ggaStr = (String)data.getCharSequence(pos, i-pos, Charset.defaultCharset());
+                break;
+            }
+        }
+        if(ggaStr != null){
+            String[] params = ggaStr.split(",",11);
+            if(params.length == 11){
+                Gga gga = new Gga();
+                try {
+                    gga.setLatitude(Double.parseDouble(params[2]));
+                    gga.setLongitude(Double.parseDouble(params[4]));
+                    gga.setAltitude(Double.parseDouble(params[9]));
+                    gga.setQuality(Integer.parseInt(params[6]));
+                    gga.setSatellitesInUsed(Integer.parseInt(params[7]));
+                    return gga;
+                }
+                catch (Exception e){
+
+                }
+
+            }
+        }
+        return null;
+    }
+
     public Gga(double latitude, double longitude) {
         this.latitude = latitude;
         this.longitude = longitude;
@@ -36,16 +144,7 @@ public class Gga {
         this.latitude = latitude;
         this.longitude = longitude;
         this.altitude = altitude;
-        this.status = status;
+        this.quality = status;
     }
 
-    @Override
-    public String toString() {
-        return "Gga{" +
-                "latitude=" + latitude +
-                ", longitude=" + longitude +
-                ", altitude=" + altitude +
-                ", status=" + status +
-                '}';
-    }
 }

sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/executor/D331RtcmMessageExecutor.java

@@ -1,6 +1,7 @@
 package com.imdroid.sideslope.executor;
 
 import com.imdroid.secapi.dto.GnssDevice;
+import com.imdroid.sideslope.bd.Gga;
 import com.imdroid.sideslope.message.D331RtcmMessage;
 import com.imdroid.sideslope.sal.Device;
 import com.imdroid.sideslope.sal.DeviceService;
@@ -28,15 +29,6 @@ public class D331RtcmMessageExecutor implements Executor<D331RtcmMessage, Void>
 
     @Override
     public Void execute(D331RtcmMessage message) {
-        if (logger.isDebugEnabled()) {
-            logger.debug("receive d331 rtcm message of device: "+message.getId()+", seq:"+message.getSeq()+", len:"+message.getLen());
-        }
-        // 补齐tenantId
-        Device device1 = deviceService.findByDeviceId(message.getId());
-        if(device1 == null || device1.getOpMode()!= GnssDevice.OP_MODE_USE) return null;
-        message.setTenantId(device1.getTenantId());
-        device1.updateD331Bytes(message.getLen());
-
         String id = message.getId();
         byte[] forwardBytes = message.getSrcData();
         // 要求快速转发，因此用缓存，不要每次都查数据库
@@ -53,6 +45,26 @@ public class D331RtcmMessageExecutor implements Executor<D331RtcmMessage, Void>
                 deviceChannel.writeAndFlush(buf);
             });
         }
+
+        // 补齐tenantId
+        Device device1 = deviceService.findByDeviceId(message.getId());
+        if(device1 == null || device1.getOpMode()!= GnssDevice.OP_MODE_USE) return null;
+        message.setTenantId(device1.getTenantId());
+        device1.updateD331Bytes(message.getLen());
+
+        if (logger.isDebugEnabled()) {
+            logger.debug("receive d331 rtcm message of device: "+message.getId()+", seq:"+message.getSeq()+", len:"+message.getLen());
+            Gga gga = message.getGga();
+            if(gga != null) {
+                logger.debug(message.getId()+
+                        " lat: "+gga.getLatitude()+
+                        " lon:"+gga.getLatitude()+
+                        " alt: "+gga.getAltitude()+
+                        " sat: "+gga.getSatellitesInUsed()+
+                        " quality: "+gga.getQuality());
+            }
+        }
+
         return null;
     }
 

sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/message/D331RtcmMessage.java

@@ -1,6 +1,7 @@
 package com.imdroid.sideslope.message;
 
 import com.imdroid.common.util.WrongMessageRecorder;
+import com.imdroid.sideslope.bd.Gga;
 import io.netty.buffer.ByteBuf;
 
 /**
@@ -8,6 +9,7 @@ import io.netty.buffer.ByteBuf;
  * @date  2023/2/2 20:47
  */
 public class D331RtcmMessage extends BaseMessage {
+    Gga gga;
 
     @Override
     public void decodeBody(ByteBuf src) {
@@ -21,6 +23,7 @@ public class D331RtcmMessage extends BaseMessage {
             String msg = String.format("id[%s],长度字段值[%s]效益包的消息体长度[%s]", id, this.len, src.readableBytes() - 4);
             WrongMessageRecorder.INSTANCE.append("receive wrong message," + msg);
         }
+        gga = Gga.getFrom(src);
         this.srcData = new byte[src.readableBytes()];
         src.readBytes(this.srcData);
     }
@@ -29,4 +32,6 @@ public class D331RtcmMessage extends BaseMessage {
     public boolean shouldDecodeHeader() {
         return false;
     }
+
+    public Gga getGga() {return gga;}
 }

sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/message/D341LocationMessage.java

@@ -1,5 +1,6 @@
 package com.imdroid.sideslope.message;
 
+import com.imdroid.sideslope.bd.Gga;
 import com.imdroid.sideslope.bd.Tilt;
 import com.imdroid.sideslope.bd.UBXUtil;
 import com.imdroid.common.util.WrongMessageRecorder;
@@ -16,12 +17,32 @@ import lombok.EqualsAndHashCode;
 public class D341LocationMessage extends BaseMessage {
     Tilt tilt;
     double[] b562_loc;
+    Gga gga;
     @Override
     public void decodeBody(ByteBuf src) {
-        // 读到pitch了，readable不不含pitch之前
+        // read操作会移动ByteBuf内部指针，除D331外，其他都用read来读
-        tilt = new Tilt(src.readFloat(), src.readFloat(),src.readFloat(),src.readFloat());
+        int packetLen = src.readableBytes();
+        int pos = 0;
+        this.header = src.getUnsignedShort(pos);pos+=2;
+        this.len = src.getUnsignedShort(pos);pos+=2;
+        this.seq = this.len >> 11;
+        this.len = this.len & 0x7FF;
+        this.id = String.valueOf(src.getUnsignedInt(pos));pos+=4; //id
+        if (packetLen - 4 != this.len) {
+            String msg = (String.format("id[%s],长度字段值[%s]与包的消息体长度[%s]不匹配", id, this.len, packetLen - 4));
+            WrongMessageRecorder.INSTANCE.append("receive wrong message," + msg);
+        }
+        this.pps = src.getUnsignedShort(pos);pos+=2;
+
+        tilt = new Tilt(src.getFloat(pos), src.getFloat(pos+4),src.getFloat(pos+8),src.getFloat(pos+12));
+
         // b562
         b562_loc = UBXUtil.getLocation(src);
+        gga = Gga.getFrom(src);
     }
 
+    @Override
+    public boolean shouldDecodeHeader() {
+        return false;
+    }
 }