From 74f50e47baa99679fae78f955a6edeb4a10c2b58 Mon Sep 17 00:00:00 2001 From: weidong Date: Mon, 11 Aug 2025 08:45:22 +0800 Subject: [PATCH] =?UTF-8?q?1=E3=80=81=E6=B5=8B=E7=AB=99d331=E4=B9=9F?= =?UTF-8?q?=E8=A7=A3=E7=AE=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sideslope/calc/GNSSDataCalcService.java | 2 + .../calc/SingleLineGNSSCalcService.java | 87 ++++++ .../executor/D331RtcmMessageExecutor.java | 287 ++++++++---------- 3 files changed, 224 insertions(+), 152 deletions(-) diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSDataCalcService.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSDataCalcService.java index e8fa445e..e8e43d6b 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSDataCalcService.java +++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSDataCalcService.java @@ -1,6 +1,7 @@ package com.imdroid.sideslope.calc; import com.imdroid.secapi.dto.GnssGroupCalc; +import com.imdroid.sideslope.message.D331RtcmMessage; import com.imdroid.sideslope.message.D341LocationMessage; import com.imdroid.sideslope.service.Device; @@ -14,6 +15,7 @@ public interface GNSSDataCalcService { * @param completeWhenIdle 是否根据空闲时间判断本轮结束 */ void calcSingle(D341LocationMessage message, boolean completeWhenIdle); + void calcSingle(D331RtcmMessage message, boolean completeWhenIdle); /** * 单轮解算结束，计算平滑值 diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/SingleLineGNSSCalcService.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/SingleLineGNSSCalcService.java index bae6a5c8..5d434d47 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/SingleLineGNSSCalcService.java +++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/SingleLineGNSSCalcService.java @@ -5,6 +5,7 @@ import com.imdroid.common.util.DataTypeUtil; import com.imdroid.common.util.ThreadManager; import com.imdroid.secapi.dto.*; import com.imdroid.sideslope.bd.*; +import com.imdroid.sideslope.message.D331RtcmMessage; import com.imdroid.sideslope.message.D341LocationMessage; import com.imdroid.sideslope.service.Device; import com.imdroid.sideslope.service.DeviceService; @@ -159,6 +160,92 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { } + @Override + public void calcSingle(D331RtcmMessage message, boolean completeWhenIdle) { + String deviceId = message.getId(); + Device device = deviceService.findByDeviceId(deviceId); + if(device == null) return; + GnssGroupCalc groupCalc = groupParaService.getCalcGroup(device.getCalcGroupId()); + if(groupCalc==null) return; + device.setB562AsCalc(groupCalc.getVer()!=3); + + //todo 创建FocusCalculator对象需获取该测站的杆长度，上一小时的Tilt平均值，上一小时的测站相对坐标融合值ekfResult + FocusCalculator focusCalculator; + if(groupCalc.getVer() == 6){ + focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator6(deviceId, 50)); + } + else if(groupCalc.getVer() == 5){ + focusCalculator = calculatorMap.get(deviceId); + if(focusCalculator ==null) { + Short removeRate = groupCalc.getRemove_rate(); + if (removeRate == null) removeRate = 20; + focusCalculator = new FocusCalculator5(deviceId, 50, removeRate); + calculatorMap.put(deviceId,focusCalculator); + } + } + else if(groupCalc.getVer() == 4){ + focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator4()); + } + else if(groupCalc.getVer() == 3){ + Device bsDevice = deviceService.findByDeviceId(device.getParentId()); + focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator3(bsDevice)); + } + else if(groupCalc.getVer() == 2){ + focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator2()); + } + else { + focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator1()); + } + + // 读取惯导 + Tilt tilt = message.getTilt(); + if(tilt != null) { + focusCalculator.addTilt(tilt); + if (logger.isDebugEnabled()) { + logger.debug("测站{}惯导单次解析结果:{}", deviceId,tilt); + } + } + + // 延迟 + focusCalculator.addDelayMs(message.getPps()); + + // 单次GGA + Gga gga = message.getGga(); + if(gga != null) { + focusCalculator.addGGA(gga); + logger.debug("测站{}的gga单次解析结果:{},{},{},{}",deviceId, + gga.getLongitude(), gga.getLatitude(), gga.getAltitude(), gga.getQuality()); + if(groupCalc.getVer() == 3 && focusCalculator.isJump()){ + logger.info("{}发生周跳",deviceId); + hardResetDevice(deviceId); + device.setJumpCount(device.getJumpCount()+1); + } + + if(completeWhenIdle && + (gga.getQuality()==Gga.FIX_RESULT || gga.getQuality()==Gga.FLOAT_RESULT)) { + resultOutputTimer(device, groupCalc, message.getCreateTime()); + } + + } + + // 若是该设备开启了日志记录，则保存单次解析的数据 + if(device.getLoggingmode() == GnssDevice.LOGGING_MODE_FULL){ + // 保存单次解析的原始数据，受 loggingmode 字段控制 + GnssSingleData gnssSingleData = new GnssSingleData(); + gnssSingleData.setDeviceid(device.getDeviceId()); + gnssSingleData.setCreatetime(LocalDateTime.now()); + gnssSingleData.setModel(device.getModel()); + if(gga !=null){ + gnssSingleData.setX(gga.getLatitude()); + gnssSingleData.setY(gga.getLongitude()); + gnssSingleData.setZ(gga.getAltitude()); + gnssSingleData.setStatus(gga.getQuality()); + gnssSingleDataService.addData(gnssSingleData); + } + } + + } + @Override public void calSingleDone(Device device, GnssGroupCalc groupCalc, LocalDateTime resultTime) { ScheduledFuture future = timerMap.get(device.getDeviceId()); diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/executor/D331RtcmMessageExecutor.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/executor/D331RtcmMessageExecutor.java index 3cd53ee5..7287ef55 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/executor/D331RtcmMessageExecutor.java +++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/executor/D331RtcmMessageExecutor.java @@ -7,6 +7,7 @@ import com.imdroid.secapi.dto.GnssDevice; import com.imdroid.common.util.ByteUtil; import com.imdroid.sideslope.bd.Gga; import com.imdroid.sideslope.bd.Rtcm1005; +import com.imdroid.sideslope.calc.GNSSDataCalcService; import com.imdroid.sideslope.message.D331RtcmMessage; import com.imdroid.sideslope.ntrip.UdpNtripServer; import com.imdroid.sideslope.service.Device; @@ -45,6 +46,8 @@ public class D331RtcmMessageExecutor implements Executor private DataPersistService dataPersistService; @Autowired UdpNtripServer ntripServer; + @Autowired + private GNSSDataCalcService gnssCalcService; // 添加一个成员变量用于追踪每个测站最后一次转发D300数据的时间 private final Map lastD300ForwardTimeMap = new ConcurrentHashMap<>(); @@ -59,158 +62,14 @@ public class D331RtcmMessageExecutor implements Executor // 推送基站数据 if(deviceBs.getOpMode() == GnssDevice.OP_MODE_USE) { - byte[] forwardBytes = message.getSrcData(); - // 要求快速转发，因此用缓存，不要每次都查数据库 - List deviceList = deviceService.findByParentId(id); - //logger.debug("base station {} has {} rovers: ", message.getId(),deviceList.size()); - DeviceChannel deviceChannel = null; - for (Device device : deviceList) { - if (device.getOpMode() != GnssDevice.OP_MODE_USE) continue; - if ((device.getModel()==GnssDevice.MODEL_G510) && - (device.getFixedNum()>100) && (device.getGnssSampleRate()>1) - && (deviceBs.getD3xxCount()%device.getGnssSampleRate()) != 0) { - //if(!UBXUtil.has1005(forwardBytes)) continue; //1005必推 - continue; - } - String deviceId = device.getDeviceId(); - // 获取设备通道并发送数据 - if(device.getDataChannelType() == Device.CHANNEL_TYPE_UDP) { - deviceChannel = OnlineChannels.INSTANCE.getDataChannel(deviceId); - } else { - deviceChannel = OnlineChannels.INSTANCE.getConfigChannel(deviceId); - } - - // 读取数据库中model字段，判断基站类型 - Short baseStationModel = deviceBs.getModel(); - // 如果model为null，使用默认值0 - if (baseStationModel == null) { - baseStationModel = 0; - logger.warn("Base station model is null for device: {}, using default value 0", id); - } - - if (baseStationModel == 1) { - // 基站类型为1，正常执行 - if(deviceChannel != null && deviceChannel.isOnline()) { - if (logger.isDebugEnabled()) { - logger.debug("forward d331 rtcm from {} to device {}", id, deviceId); - } - ByteBuf buf = Unpooled.buffer(); - buf.writeBytes(forwardBytes); - deviceChannel.writeAndFlush(buf); - } - } else if (baseStationModel == 0) { - //logger.info("Base station model is 0 for device: {}", deviceId); - - Short deviceModel = device.getModel(); - // 如果model为null，使用默认值0 - if (deviceModel == null) { - deviceModel = 0; - //logger.warn("Device model is null for device: {}, using default value 0", deviceId); - } - - if(deviceModel == 0){ - // 测站类型为0，正常执行 - if(deviceId.startsWith("2307")){ - // 处理2307型号的测站 - forwardBytes[2] = (byte) (forwardBytes[2] & 0x07);//兼容不带序号的测站 - } - // 对所有测站类型为0的设备执行转发 - if(deviceChannel != null && deviceChannel.isOnline()) { - if (logger.isDebugEnabled()) { - logger.debug("forward d331 rtcm from {} to device {}", id, deviceId); - } - ByteBuf buf = Unpooled.buffer(); - buf.writeBytes(forwardBytes); - deviceChannel.writeAndFlush(buf); - } - } - else if(deviceModel == 1){ - //logger.info("Device model is 1 for device: {}", deviceId); - - if(deviceChannel != null && deviceChannel.isOnline()) { - //logger.info("Device channel is online for device: {}", deviceId); - - ByteBuf buf = Unpooled.buffer(); - buf.writeBytes(forwardBytes); - - // 检查是否满足10秒转发间隔，只有满足条件时才添加D300字符串 - long currentTime = System.currentTimeMillis(); - Long lastForwardTime = lastD300ForwardTimeMap.getOrDefault(deviceId, 0L); - - if(currentTime - lastForwardTime >= D300_FORWARD_INTERVAL) { - //logger.info("Adding D300 string for device: {}", deviceId); - - // 获取当前buf中的数据 - byte[] originalData = buf.array(); - String originalHex = ByteUtil.bytesToHexString(originalData); - - // 找到D300和D301的位置 - int d300Index = originalHex.indexOf("d300"); - int d301Index = originalHex.indexOf("d301"); - - // 确定插入位置：如果两个都存在，取位置靠前的；如果只存在一个，就用那个位置 - int insertIndex = -1; - if (d300Index != -1 && d301Index != -1) { - // 两个都存在，取位置靠前的 - insertIndex = (d300Index < d301Index) ? d300Index : d301Index; - //logger.info("Found both D300 and D301, D300 at {}, D301 at {}, will insert before position {}", - /// d300Index, d301Index, insertIndex); - } else if (d300Index != -1) { - insertIndex = d300Index; - //logger.info("Found D300 at position {}", d300Index); - } else if (d301Index != -1) { - insertIndex = d301Index; - //logger.info("Found D301 at position {}", d301Index); - } - - - if (insertIndex != -1) { - // 创建新的buf - ByteBuf newBuf = Unpooled.buffer(); - // 写入D300/D301之前的数据 - newBuf.writeBytes(originalData, 0, insertIndex / 2); - - // 使用f9p坐标生成1005消息，并插入 - double[] ecef =new double[3]; - ecef[0] = deviceBs.getEcefx(); - ecef[1] = deviceBs.getEcefy(); - ecef[2] = deviceBs.getEcefz(); - String rtcm1005 = Rtcm1005.generateRtcm1005Hex(ecef, 0); - if (rtcm1005 != null) { - // 写入RTCM 1005消息 - byte[] rtcm1005Bytes = ByteUtil.hexStringTobyte(rtcm1005); - newBuf.writeBytes(rtcm1005Bytes); - // logger.info("Generated RTCM 1005 message for base station {}: {}", deviceBs.getDeviceId(), rtcm1005); - } else { - //logger.warn("Failed to generate RTCM 1005 message for base station: {}", deviceBs.getDeviceId()); - } - - // 写入剩余的数据 - newBuf.writeBytes(originalData, insertIndex / 2, originalData.length - insertIndex / 2); - - // 更新buf - buf = newBuf; - - // 添加日志，记录插入位置和完整数据 -// logger.info("Inserted RTCM 1005 message before position {}, complete data: {}", -// insertIndex, -// ByteUtil.bytesToHexString(buf.array())); - } - - // 更新最后转发时间 - lastD300ForwardTimeMap.put(deviceId, currentTime); - - // 添加日志，记录测站转发的完整数据 - //logger.info("Forward data to device: {}, time: {}, complete data: {}", - // deviceId, - // LocalDateTime.now(), - // ByteUtil.bytesToHexString(buf.array())); - } - - deviceChannel.writeAndFlush(buf); - } - } - } + if(deviceBs.getDeviceType()==GnssDevice.TYPE_REFERENCE_STATION) { + byte[] forwardBytes = message.getSrcData(); + sendToRovers(deviceBs, forwardBytes); + } + else{ + ThreadManager.getFixedThreadPool().submit(() -> { + gnssCalcService.calcSingle(message, true); + }); } } @@ -275,6 +134,130 @@ public class D331RtcmMessageExecutor implements Executor return null; } + private void sendToRovers(Device deviceBs, byte[] forwardBytes){ + List deviceList = deviceService.findByParentId(deviceBs.getDeviceId()); + //logger.debug("base station {} has {} rovers: ", message.getId(),deviceList.size()); + DeviceChannel deviceChannel = null; + + for (Device device : deviceList) { + // 不是使用状态不推送 + if (device.getOpMode() != GnssDevice.OP_MODE_USE) continue; + // 间隔推送 + if ((device.getModel()==GnssDevice.MODEL_G510) && + (device.getFixedNum()>100) && (device.getGnssSampleRate()>1) + && (deviceBs.getD3xxCount()%device.getGnssSampleRate()) != 0) { + //if(!UBXUtil.has1005(forwardBytes)) continue; //1005必推 + continue; + } + // 不在线的不推送 + String deviceId = device.getDeviceId(); + // 获取设备通道并发送数据 + if(device.getDataChannelType() == Device.CHANNEL_TYPE_UDP) { + deviceChannel = OnlineChannels.INSTANCE.getDataChannel(deviceId); + } else { + deviceChannel = OnlineChannels.INSTANCE.getConfigChannel(deviceId); + } + if(deviceChannel == null || !deviceChannel.isOnline()){ + continue; + } + + //推送 + if(deviceId.startsWith("2307")){ + // 处理2307型号的测站 + forwardBytes[2] = (byte) (forwardBytes[2] & 0x07);//兼容不带序号的测站 + } + // 对所有测站类型为0的设备执行转发 + if (logger.isDebugEnabled()) { + logger.debug("forward d331 rtcm from {} to device {}", deviceBs.getDeviceId(), deviceId); + } + ByteBuf buf = Unpooled.buffer(); + buf.writeBytes(forwardBytes); + + //推送策略：F9P基站（505）兼容博通（510）推送 + if ((deviceBs.getModel()==null || deviceBs.getModel() == GnssDevice.MODEL_G505) && + (device.getModel()!=null && device.getModel()==GnssDevice.MODEL_G510)){ + buf = insert1005(deviceBs, deviceId, buf); + } + + deviceChannel.writeAndFlush(buf); + } + } + + private ByteBuf insert1005(Device deviceBs, String deviceId, ByteBuf buf){ + long currentTime = System.currentTimeMillis(); + Long lastForwardTime = lastD300ForwardTimeMap.getOrDefault(deviceId, 0L); + + ByteBuf buf1005 = buf; + + // 检查是否满足10秒转发间隔，只有满足条件时才添加D300字符串 + if(currentTime - lastForwardTime >= D300_FORWARD_INTERVAL) { + //logger.info("Adding D300 string for device: {}", deviceId); + + // 获取当前buf中的数据 + byte[] originalData = buf.array(); + String originalHex = ByteUtil.bytesToHexString(originalData); + + // 找到D300和D301的位置 + int d300Index = originalHex.indexOf("d300"); + int d301Index = originalHex.indexOf("d301"); + + // 确定插入位置：如果两个都存在，取位置靠前的；如果只存在一个，就用那个位置 + int insertIndex = -1; + if (d300Index != -1 && d301Index != -1) { + // 两个都存在，取位置靠前的 + insertIndex = (d300Index < d301Index) ? d300Index : d301Index; + //logger.info("Found both D300 and D301, D300 at {}, D301 at {}, will insert before position {}", + /// d300Index, d301Index, insertIndex); + } else if (d300Index != -1) { + insertIndex = d300Index; + //logger.info("Found D300 at position {}", d300Index); + } else if (d301Index != -1) { + insertIndex = d301Index; + //logger.info("Found D301 at position {}", d301Index); + } + + if (insertIndex != -1) { + // 创建新的buf + buf1005 = Unpooled.buffer(); + // 写入D300/D301之前的数据 + buf1005.writeBytes(originalData, 0, insertIndex / 2); + + // 使用f9p坐标生成1005消息，并插入 + double[] ecef =new double[3]; + ecef[0] = deviceBs.getEcefx(); + ecef[1] = deviceBs.getEcefy(); + ecef[2] = deviceBs.getEcefz(); + String rtcm1005 = Rtcm1005.generateRtcm1005Hex(ecef, 0); + if (rtcm1005 != null) { + // 写入RTCM 1005消息 + byte[] rtcm1005Bytes = ByteUtil.hexStringTobyte(rtcm1005); + buf1005.writeBytes(rtcm1005Bytes); + // logger.info("Generated RTCM 1005 message for base station {}: {}", deviceBs.getDeviceId(), rtcm1005); + } else { + //logger.warn("Failed to generate RTCM 1005 message for base station: {}", deviceBs.getDeviceId()); + } + + // 写入剩余的数据 + buf1005.writeBytes(originalData, insertIndex / 2, originalData.length - insertIndex / 2); + + // 添加日志，记录插入位置和完整数据 +// logger.info("Inserted RTCM 1005 message before position {}, complete data: {}", +// insertIndex, +// ByteUtil.bytesToHexString(buf.array())); + } + + // 更新最后转发时间 + lastD300ForwardTimeMap.put(deviceId, currentTime); + + // 添加日志，记录测站转发的完整数据 + //logger.info("Forward data to device: {}, time: {}, complete data: {}", + // deviceId, + // LocalDateTime.now(), + // ByteUtil.bytesToHexString(buf.array())); + } + return buf1005; + } + private void sendToNtrip(String mountpoint, String hexData) { try {