1、测站d331也解算
This commit is contained in:
weidong
parent
9103284ee4
commit
74f50e47ba
@ -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);
|
||||
|
||||
/**
|
||||
* 单轮解算结束，计算平滑值
|
||||
|
||||
@ -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());
|
||||
|
||||
@ -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<D331RtcmMessage, Void>
|
||||
private DataPersistService dataPersistService;
|
||||
@Autowired
|
||||
UdpNtripServer ntripServer;
|
||||
@Autowired
|
||||
private GNSSDataCalcService gnssCalcService;
|
||||
|
||||
// 添加一个成员变量用于追踪每个测站最后一次转发D300数据的时间
|
||||
private final Map<String, Long> lastD300ForwardTimeMap = new ConcurrentHashMap<>();
|
||||
@ -59,158 +62,14 @@ public class D331RtcmMessageExecutor implements Executor<D331RtcmMessage, Void>
|
||||
|
||||
// 推送基站数据
|
||||
if(deviceBs.getOpMode() == GnssDevice.OP_MODE_USE) {
|
||||
byte[] forwardBytes = message.getSrcData();
|
||||
// 要求快速转发，因此用缓存，不要每次都查数据库
|
||||
List<Device> 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<D331RtcmMessage, Void>
|
||||
return null;
|
||||
}
|
||||
|
||||
private void sendToRovers(Device deviceBs, byte[] forwardBytes){
|
||||
List<Device> 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 {
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user