From 0ef10ee7a915c16be36b17999db766cce902927d Mon Sep 17 00:00:00 2001 From: weidong Date: Wed, 31 Jan 2024 20:03:18 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0GGA=E7=9A=84=E8=A7=A3?= =?UTF-8?q?=E6=9E=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../java/com/imdroid/sideslope/bd/Gga.java | 121 ++++++++++++++++-- .../executor/D331RtcmMessageExecutor.java | 30 +++-- .../sideslope/message/D331RtcmMessage.java | 5 + .../message/D341LocationMessage.java | 25 +++- 4 files changed, 159 insertions(+), 22 deletions(-) diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/Gga.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/Gga.java index e4b74562..838c1124 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/Gga.java +++ b/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 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; } diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/message/D331RtcmMessage.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/message/D331RtcmMessage.java index a15bbfc3..ab9cc606 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/message/D331RtcmMessage.java +++ b/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;} } diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/message/D341LocationMessage.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/message/D341LocationMessage.java index 1edae58e..85d3d89a 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/message/D341LocationMessage.java +++ b/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之前 - tilt = new Tilt(src.readFloat(), src.readFloat(),src.readFloat(),src.readFloat()); + // read操作会移动ByteBuf内部指针，除D331外，其他都用read来读 + 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; + } }