Imdroid/security-monitor
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

增加GGA的解析

Browse Source
This commit is contained in:
weidong 2024-01-31 20:03:18 +08:00
parent b47f9a377e
commit 0ef10ee7a9
4 changed files with 159 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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 +
'}';
}
}

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

@ -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;
}

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

@ -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;}
}

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

@ -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;
}
}