Compare commits
No commits in common. "f9ae16563dbdbd313986e21baa8502497dd02012" and "7f66b1ae7b1d6679e732a80762a173f12ed84c33" have entirely different histories.
f9ae16563d
...
7f66b1ae7b
@ -1,179 +0,0 @@
|
|||||||
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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -6,7 +6,6 @@ import com.imdroid.secapi.client.BeidouClient;
|
|||||||
import com.imdroid.secapi.dto.GnssDevice;
|
import com.imdroid.secapi.dto.GnssDevice;
|
||||||
import com.imdroid.common.util.ByteUtil;
|
import com.imdroid.common.util.ByteUtil;
|
||||||
import com.imdroid.sideslope.bd.Gga;
|
import com.imdroid.sideslope.bd.Gga;
|
||||||
import com.imdroid.sideslope.bd.Rtcm1005;
|
|
||||||
import com.imdroid.sideslope.message.D331RtcmMessage;
|
import com.imdroid.sideslope.message.D331RtcmMessage;
|
||||||
import com.imdroid.sideslope.ntrip.UdpNtripServer;
|
import com.imdroid.sideslope.ntrip.UdpNtripServer;
|
||||||
import com.imdroid.sideslope.service.Device;
|
import com.imdroid.sideslope.service.Device;
|
||||||
@ -23,9 +22,7 @@ import org.springframework.beans.factory.annotation.Autowired;
|
|||||||
import org.springframework.stereotype.Component;
|
import org.springframework.stereotype.Component;
|
||||||
import java.time.LocalDateTime;
|
import java.time.LocalDateTime;
|
||||||
import java.util.List;
|
import java.util.List;
|
||||||
import java.util.Map;
|
|
||||||
import java.util.Optional;
|
import java.util.Optional;
|
||||||
import java.util.concurrent.ConcurrentHashMap;
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -46,10 +43,6 @@ public class D331RtcmMessageExecutor implements Executor<D331RtcmMessage, Void>
|
|||||||
@Autowired
|
@Autowired
|
||||||
UdpNtripServer ntripServer;
|
UdpNtripServer ntripServer;
|
||||||
|
|
||||||
// 添加一个成员变量用于追踪每个测站最后一次转发D300数据的时间
|
|
||||||
private final Map<String, Long> lastD300ForwardTimeMap = new ConcurrentHashMap<>();
|
|
||||||
private static final long D300_FORWARD_INTERVAL = 10000; // 10秒，单位毫秒
|
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public Void execute(D331RtcmMessage message) {
|
public Void execute(D331RtcmMessage message) {
|
||||||
String id = message.getId();
|
String id = message.getId();
|
||||||
@ -72,145 +65,25 @@ public class D331RtcmMessageExecutor implements Executor<D331RtcmMessage, Void>
|
|||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
String deviceId = device.getDeviceId();
|
String deviceId = device.getDeviceId();
|
||||||
// 获取设备通道并发送数据
|
|
||||||
if(device.getDataChannelType() == Device.CHANNEL_TYPE_UDP) {
|
if(device.getDataChannelType() == Device.CHANNEL_TYPE_UDP) {
|
||||||
deviceChannel = OnlineChannels.INSTANCE.getDataChannel(deviceId);
|
deviceChannel = OnlineChannels.INSTANCE.getDataChannel(deviceId);
|
||||||
} else {
|
}
|
||||||
|
else {
|
||||||
deviceChannel = OnlineChannels.INSTANCE.getConfigChannel(deviceId);
|
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(deviceChannel!=null && deviceChannel.isOnline()){
|
||||||
if (logger.isDebugEnabled()) {
|
if (logger.isDebugEnabled()) {
|
||||||
logger.debug("forward d331 rtcm from {} to device {}", id, deviceId);
|
logger.debug("forward d331 rtcm to device {}", 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")) {
|
if (deviceId.startsWith("2307")) {
|
||||||
// 处理2307型号的测站
|
|
||||||
forwardBytes[2] = (byte) (forwardBytes[2] & 0x07);//兼容不带序号的测站
|
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();
|
ByteBuf buf = Unpooled.buffer();
|
||||||
buf.writeBytes(forwardBytes);
|
buf.writeBytes(forwardBytes);
|
||||||
deviceChannel.writeAndFlush(buf);
|
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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// 如果30分钟内收到不到d3f0和d3f2，则根据UDP最后一个报文触发状态更新和统计
|
// 如果30分钟内收到不到d3f0和d3f2，则根据UDP最后一个报文触发状态更新和统计
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user