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

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<D331RtcmMessage, Void>
     @Autowired
     UdpNtripServer ntripServer;
 
+    // 添加一个成员变量用于追踪每个测站最后一次转发D300数据的时间
+    private final Map<String, Long> 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<D331RtcmMessage, Void>
                     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()){
+                // 读取数据库中model字段，判断基站类型
-                    if (logger.isDebugEnabled()) {
+                Short baseStationModel = deviceBs.getModel();
-                        logger.debug("forward d331 rtcm to device {}", deviceId);
+                // 如果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")) {
+                } else if (baseStationModel == 0) {
-                        forwardBytes[2] = (byte) (forwardBytes[2] & 0x07);//兼容不带序号的测站
+                    //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);
                 }
             }
         }