From 6a8c3731881971a373ed491bc794993d076b613c Mon Sep 17 00:00:00 2001 From: weidong Date: Sun, 7 Jul 2024 07:49:02 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=A7=A3=E7=AE=97=E7=AE=97?= =?UTF-8?q?=E6=B3=954=EF=BC=8C=E4=B8=80=E8=B5=B7=E6=80=A7=E5=8F=96?= =?UTF-8?q?=E5=8F=82=E8=80=83=E7=82=B9=E6=9C=80=E8=BF=91=E7=9A=8450%?= =?UTF-8?q?=E7=9A=84=E7=82=B9=E6=B1=82=E5=9D=87=E5=80=BC=EF=BC=8C=E5=8F=82?= =?UTF-8?q?=E8=80=83=E7=82=B9=E4=B8=BA=E5=B9=B3=E6=BB=91=E5=90=8E=E7=9A=84?= =?UTF-8?q?=E5=80=BC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sideslope/bd/FocusCalculator4.java | 69 +++++++++++++++++++ .../sideslope/calc/GNSSCalcFilterService.java | 17 ++--- .../calc/SingleLineGNSSCalcService.java | 16 ++++- .../executor/D331RtcmMessageExecutor.java | 7 +- .../page/table/gnss_add_group_calc.html | 1 + 5 files changed, 94 insertions(+), 16 deletions(-) create mode 100644 sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator4.java diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator4.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator4.java new file mode 100644 index 00000000..0eaf7b73 --- /dev/null +++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator4.java @@ -0,0 +1,69 @@ +package com.imdroid.sideslope.bd; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.util.*; + +/** + * 1.简化重心计算，选取和参考点最近的50%的点求均值 + */ +public class FocusCalculator4 extends FocusCalculator1{ + final Logger logger = LoggerFactory.getLogger(FocusCalculator1.class); + // b562算法相关：b562固定解的点、计算球心初始半径、迭代步长、最少点数 + + public FocusCalculator4(){ + super(); + gravityMinCount = 80; + } + + @Override + public double[] resultB562(){ + try { + if (pointList.size() >= gravityMinCount) { + List selectPoints = new ArrayList<>(); + if (referPoint != null && referPoint.length > 0) { + // 计算所有点位与变量之间的距离，并存入集合 + for (double[] point : pointList) { + Point pointObj = new Point(point[0], point[1], point[2]); + pointObj.setXDistance(Math.abs(point[0] - referPoint[0])); // 设置该点到变量的水平距离 + pointObj.setYDistance(Math.abs(point[1] - referPoint[1])); // 设置该点到变量的水平距离 + pointObj.setZDistance(Math.abs(point[2] - referPoint[2])); // 设置该点到变量的垂直距离 + selectPoints.add(pointObj); // 将点位加入集合 + } + //分别对东北天排序 + Collections.sort(selectPoints, Comparator.comparing(Point::getXDistance));//排序 + List points = selectPoints.subList(0, selectPoints.size() / 2); + double[] focusX = focusPointObj(points); + Collections.sort(selectPoints, Comparator.comparing(Point::getYDistance));//排序 + points = selectPoints.subList(0, selectPoints.size() / 2); + double[] focusY = focusPointObj(points); + Collections.sort(selectPoints, Comparator.comparing(Point::getZDistance));//排序 + points = selectPoints.subList(0, selectPoints.size() / 2); + double[] focusZ = focusPointObj(points); + return new double[]{focusX[0], focusY[1], focusZ[2]}; + + } + else { + for (int i = pointList.size()/2; i < pointList.size(); i++) { + selectPoints.add(new Point(pointList.get(i)[0], pointList.get(i)[1], pointList.get(i)[2])); + } + // 求重心 + double[] focus = focusPointObj(selectPoints); + return focus; + } + } + } + catch (Exception e){ + logger.error("error!",e); + return null; + } + return resultB562Sub(); + } + + @Override + public int getVer() { + return 4; + } + +} diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSCalcFilterService.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSCalcFilterService.java index 4c2c4eac..897ec163 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSCalcFilterService.java +++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSCalcFilterService.java @@ -10,7 +10,6 @@ import org.springframework.beans.factory.annotation.Autowired; import org.springframework.stereotype.Service; import java.time.LocalDateTime; -import java.time.format.DateTimeFormatter; import java.util.List; import java.util.concurrent.ConcurrentHashMap; @@ -31,8 +30,6 @@ public class GNSSCalcFilterService { } final private ConcurrentHashMap autoCycleDevices = new ConcurrentHashMap<>(); - final DateTimeFormatter dateFormatter = DateTimeFormatter.ofPattern("yyyy-MM-dd HH:mm:ss"); - public void calc(Device gnssDevice, GnssGroupCalc groupCalc, GnssCalcData newRecord, double[] referPos) { String deviceId = newRecord.getDeviceid(); @@ -114,8 +111,8 @@ public class GNSSCalcFilterService { LocalDateTime minCycleTime = newRecordTime.minusHours(minCycleHour); QueryWrapper query = new QueryWrapper<>(); query.eq("deviceid", deviceId); - query.ge("createtime", filterAfterTime.format(dateFormatter)); - query.le("createtime", newRecordTime.format(dateFormatter)); + query.ge("createtime", filterAfterTime); + query.le("createtime", newRecordTime); //query.eq("enabled", true);// 坏点参与比较，不参与滤波 query.orderByDesc("createtime"); @@ -183,8 +180,8 @@ public class GNSSCalcFilterService { LocalDateTime minCycleTime = newRecordTime.minusHours(minCycleHour); QueryWrapper query = new QueryWrapper<>(); query.eq("deviceid", deviceId); - query.ge("createtime", filterAfterTime.format(dateFormatter)); - query.le("createtime", newRecordTime.format(dateFormatter)); + query.ge("createtime", filterAfterTime); + query.le("createtime", newRecordTime); //query.eq("enabled", true);// 坏点参与比较，不参与滤波 query.orderByDesc("createtime"); @@ -254,7 +251,7 @@ public class GNSSCalcFilterService { LocalDateTime cmpTime = now.minusHours(groupCalc.getFilter_min_hour()); QueryWrapper queryWrapper = new QueryWrapper<>(); queryWrapper.eq("deviceid", deviceId); - queryWrapper.le("createtime",cmpTime.format(dateFormatter)); + queryWrapper.le("createtime",cmpTime); queryWrapper.eq("enabled",true); queryWrapper.isNotNull("rpose"); queryWrapper.last("limit 1"); @@ -299,8 +296,8 @@ public class GNSSCalcFilterService { LocalDateTime beforTime = afterTime.plusHours(groupCalc.getFilter_hour()); QueryWrapper query = new QueryWrapper<>(); query.eq("deviceid", deviceId); - query.le("createtime", beforTime.format(dateFormatter)); - query.ge("createtime", afterTime.format(dateFormatter)); + query.le("createtime", beforTime); + query.ge("createtime", afterTime); //query.eq("enabled",true); //好点坏点重新判断 query.orderByAsc("createtime"); diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/SingleLineGNSSCalcService.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/SingleLineGNSSCalcService.java index e3705061..b4fc7e49 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/SingleLineGNSSCalcService.java +++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/SingleLineGNSSCalcService.java @@ -77,7 +77,10 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { //todo 创建FocusCalculator对象需获取该测站的杆长度，上一小时的Tilt平均值，上一小时的测站相对坐标融合值ekfResult FocusCalculator focusCalculator; - if(groupCalc!=null && groupCalc.getVer() == 3){ + if(groupCalc!=null && groupCalc.getVer() == 4){ + focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator4()); + } + else if(groupCalc!=null && groupCalc.getVer() == 3){ focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator3()); } else if(groupCalc!=null && groupCalc.getVer() == 2){ @@ -213,7 +216,16 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { // 记录本次位置，作为下次的参考 // 算法1：以上轮位置作为参考，跟随变化速度快 // 算法2：以滤波后的位置作为参考，跟随变化速度慢；如果倾角大于某一门限，则跟随上轮位置 - if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.isShocked()) { + if(focusCalculator.getVer()==4) { + if(locationRecord.getRpose()!=null) { + focusCalculator.setReferPoint( + new double[]{locationRecord.getRpose(),locationRecord.getRposn(),locationRecord.getRposd()}); + } + else if(focusCalculator.getReferPoint() == null){ + focusCalculator.setReferPoint(b562Result); + } + } + else if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.isShocked()) { focusCalculator.setReferPoint(b562Result); } else { 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 de157746..5a73219b 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 @@ -7,7 +7,6 @@ import com.imdroid.sideslope.bd.Gga; import com.imdroid.sideslope.message.D331RtcmMessage; import com.imdroid.sideslope.sal.Device; import com.imdroid.sideslope.sal.DeviceService; -import com.imdroid.sideslope.server.DeviceChannel; import com.imdroid.sideslope.server.OnlineChannels; import com.imdroid.sideslope.service.DataPersistService; import io.netty.buffer.ByteBuf; @@ -53,7 +52,7 @@ public class D331RtcmMessageExecutor implements Executor for (Device device : deviceList) { if (device.getOpMode() != GnssDevice.OP_MODE_USE) continue; String deviceId = device.getDeviceId(); - if(deviceId.equals("2419385")){ + /*if(deviceId.equals("2419385")){ DeviceChannel channel = OnlineChannels.INSTANCE.getConfigChannel(deviceId); if(channel != null && channel.isOnline()){ if (logger.isDebugEnabled()) { @@ -64,7 +63,7 @@ public class D331RtcmMessageExecutor implements Executor channel.writeAndFlush(buf); } } - else { + else {*/ OnlineChannels.INSTANCE.get(deviceId).ifPresent(deviceChannel -> { if (logger.isDebugEnabled()) { logger.debug("forward d331 rtcm message to device {}", deviceId); @@ -76,7 +75,7 @@ public class D331RtcmMessageExecutor implements Executor buf.writeBytes(forwardBytes); deviceChannel.writeAndFlush(buf); }); - } + //} } } diff --git a/sec-beidou/src/main/resources/templates/page/table/gnss_add_group_calc.html b/sec-beidou/src/main/resources/templates/page/table/gnss_add_group_calc.html index ad208e39..e45cd701 100644 --- a/sec-beidou/src/main/resources/templates/page/table/gnss_add_group_calc.html +++ b/sec-beidou/src/main/resources/templates/page/table/gnss_add_group_calc.html @@ -36,6 +36,7 @@ +