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

修改 modle #1

Merged
admin merged 1 commits from feature/rtcm1005 into develop 2025-05-20 04:04:23 +00:00
Conversation 0 Commits 1 Files Changed 2 +316 -10
2 changed files with 316 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 3d39e5d07e - Show all commits

179
sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/Rtcm1005.java Normal file
View File

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

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

@ -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,24 +72,144 @@ 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()){
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 (deviceId.startsWith("2307")) {
forwardBytes[2] = (byte) (forwardBytes[2] & 0x07);//兼容不带序号的测站
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);
}
}
}
}
}