From 3d39e5d07e672db162ab7efa2d86883cbeabde9b Mon Sep 17 00:00:00 2001 From: zms Date: Mon, 19 May 2025 14:37:33 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=20modle?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../com/imdroid/sideslope/bd/Rtcm1005.java | 179 ++++++++++++++++++ .../executor/D331RtcmMessageExecutor.java | 147 +++++++++++++- 2 files changed, 316 insertions(+), 10 deletions(-) create mode 100644 sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/Rtcm1005.java diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/Rtcm1005.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/Rtcm1005.java new file mode 100644 index 00000000..daf36a04 --- /dev/null +++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/Rtcm1005.java @@ -0,0 +1,179 @@ +package com.imdroid.sideslope.bd; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +/** + * RTCM 1005 message generation + * Converted from rtcm1005.c + */ +public class Rtcm1005 { + private static final Logger logger = LoggerFactory.getLogger(Rtcm1005.class); + private static final int MAXBUFF = 1024; + + static class RTCM { + byte[] buff = new byte[MAXBUFF]; // 消息缓冲区 + int nbit; // 缓冲区中的位数 + int staid; // 站点ID + double[] sta_pos = new double[3]; // 站点位置(ECEF)(m) + } + + /** + * 设置无符号位 + * @param buff 缓冲区 + * @param pos 起始位置 + * @param len 位长度 + * @param data 要设置的数据 + */ + private static void setbitu(byte[] buff, int pos, int len, long data) { + long mask = 1L << (len - 1); + + if (len <= 0 || 32 < len) return; + + for (int i = 0; i < len; i++, mask >>= 1) { + if ((data & mask) != 0) + buff[pos / 8] |= (byte)(0x80 >> (pos % 8)); + else + buff[pos / 8] &= (byte)(~(0x80 >> (pos % 8))); + pos++; + } + } + + /** + * 设置有符号38位字段 + * @param buff 缓冲区 + * @param pos 起始位置 + * @param value 要设置的值 + */ + private static void set38bits(byte[] buff, int pos, double value) { + long word = Math.round(value); + int word_h = (int)(word >> 32); + int word_l = (int)(word & 0xFFFFFFFFL); + + setbitu(buff, pos, 6, word_h); + setbitu(buff, pos + 6, 32, word_l); + } + + /** + * 计算CRC24Q校验和 + * @param buff 缓冲区 + * @param len 长度 + * @return 校验和 + */ + private static int crc24q(byte[] buff, int len) { + int crc = 0; + + for (int i = 0; i < len; i++) { + crc ^= (buff[i] & 0xFF) << 16; + for (int j = 0; j < 8; j++) { + crc <<= 1; + if ((crc & 0x1000000) != 0) crc ^= 0x1864CFB; + } + } + return crc & 0xFFFFFF; + } + + /** + * 生成RTCM 1005消息 + * @param rtcm RTCM控制结构 + * @param sync 同步标志(1:同步,0:异步) + * @return 状态(1:成功,0:错误) + */ + private static int encode_type1005(RTCM rtcm, int sync) { + double[] p = rtcm.sta_pos; + int i = 0; + int crc; + int len; + + // RTCM消息头 + setbitu(rtcm.buff, i, 8, 0xD3); i += 8; // 前导码 + setbitu(rtcm.buff, i, 6, 0); i += 6; // 保留位 + setbitu(rtcm.buff, i, 10, 19); i += 10; // 消息长度 + setbitu(rtcm.buff, i, 12, 1005); i += 12; // 消息号 + setbitu(rtcm.buff, i, 12, rtcm.staid); i += 12; // 参考站ID + setbitu(rtcm.buff, i, 6, 0); i += 6; // ITRF参考年份 + setbitu(rtcm.buff, i, 1, 1); i += 1; // GPS指示器 + setbitu(rtcm.buff, i, 1, 1); i += 1; // GLONASS指示器 + setbitu(rtcm.buff, i, 1, 0); i += 1; // Galileo指示器 + setbitu(rtcm.buff, i, 1, 0); i += 1; // 参考站指示器 + + // 站点坐标 + set38bits(rtcm.buff, i, p[0] / 0.0001); i += 38; // 天线参考点ECEF-X + setbitu(rtcm.buff, i, 1, 1); i += 1; // 振荡器指示器 + setbitu(rtcm.buff, i, 1, 0); i += 1; // 保留位 + set38bits(rtcm.buff, i, p[1] / 0.0001); i += 38; // 天线参考点ECEF-Y + setbitu(rtcm.buff, i, 2, 0); i += 2; // 四分之一周期指示器 + set38bits(rtcm.buff, i, p[2] / 0.0001); i += 38; // 天线参考点ECEF-Z + + // 添加CRC24Q + len = i / 8; + crc = crc24q(rtcm.buff, len); + setbitu(rtcm.buff, i, 24, crc); i += 24; + + rtcm.nbit = i; + return 1; + } + + /** + * 从ECEF坐标生成RTCM 1005消息 + * @param ecef ECEF坐标(m) [x,y,z] + * @param staid 站点ID + * @param buff 输出缓冲区 + * @return 输出缓冲区长度(字节)，失败返回-1 + */ + public static int gen_rtcm1005(double[] ecef, int staid, byte[] buff) { + RTCM rtcm = new RTCM(); + + // 检查输入 + if (ecef == null || buff == null) return -1; + + // 设置站点信息 + rtcm.staid = staid; + System.arraycopy(ecef, 0, rtcm.sta_pos, 0, 3); + + // 生成消息 + int ret = encode_type1005(rtcm, 0); + if (ret == 0) return -1; + + // 复制消息到输出缓冲区 + int len = (rtcm.nbit + 7) / 8; + System.arraycopy(rtcm.buff, 0, buff, 0, len); + + return len; + } + + /** + * 将字节数组转换为十六进制字符串 + * @param bytes 字节数组 + * @param len 长度 + * @param upperCase 是否大写 + * @param addSpaces 是否添加空格 + * @return 十六进制字符串 + */ + public static String bytesToHex(byte[] bytes, int len, boolean upperCase, boolean addSpaces) { + StringBuilder sb = new StringBuilder(); + String format = upperCase ? "%02X" : "%02x"; + + for (int i = 0; i < len; i++) { + sb.append(String.format(format, bytes[i] & 0xFF)); + if (addSpaces && i < len - 1) sb.append(" "); + } + + return sb.toString(); + } + + /** + * 生成RTCM 1005消息并返回十六进制字符串 + * @param ecef ECEF坐标(m) [x,y,z] + * @param staid 站点ID + * @return RTCM 1005消息的十六进制字符串，失败返回null + */ + public static String generateRtcm1005Hex(double[] ecef, int staid) { + byte[] buff = new byte[MAXBUFF]; + int len = gen_rtcm1005(ecef, staid, buff); + if (len > 0) { + return bytesToHex(buff, len, false, false); + } + return null; + } +} \ No newline at end of file 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 bd067da9..f6de9b97 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 @@ -6,6 +6,7 @@ import com.imdroid.secapi.client.BeidouClient; 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.message.D331RtcmMessage; import com.imdroid.sideslope.ntrip.UdpNtripServer; import com.imdroid.sideslope.service.Device; @@ -22,7 +23,9 @@ import org.springframework.beans.factory.annotation.Autowired; import org.springframework.stereotype.Component; import java.time.LocalDateTime; import java.util.List; +import java.util.Map; import java.util.Optional; +import java.util.concurrent.ConcurrentHashMap; /** @@ -43,6 +46,10 @@ public class D331RtcmMessageExecutor implements Executor @Autowired UdpNtripServer ntripServer; + // 添加一个成员变量用于追踪每个测站最后一次转发D300数据的时间 + private final Map lastD300ForwardTimeMap = new ConcurrentHashMap<>(); + private static final long D300_FORWARD_INTERVAL = 10000; // 10秒，单位毫秒 + @Override public Void execute(D331RtcmMessage message) { String id = message.getId(); @@ -65,23 +72,143 @@ public class D331RtcmMessageExecutor implements Executor continue; } String deviceId = device.getDeviceId(); + // 获取设备通道并发送数据 if(device.getDataChannelType() == Device.CHANNEL_TYPE_UDP) { deviceChannel = OnlineChannels.INSTANCE.getDataChannel(deviceId); - } - else { + } else { deviceChannel = OnlineChannels.INSTANCE.getConfigChannel(deviceId); } - if(deviceChannel!=null && deviceChannel.isOnline()){ - if (logger.isDebugEnabled()) { - logger.debug("forward d331 rtcm to device {}", 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); } - if (deviceId.startsWith("2307")) { - forwardBytes[2] = (byte) (forwardBytes[2] & 0x07);//兼容不带序号的测站 + } 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); + } } - ByteBuf buf = Unpooled.buffer(); - buf.writeBytes(forwardBytes); - deviceChannel.writeAndFlush(buf); } } }