From 2f8d570d195c46f072426a9ed6d9b9120f2a1594 Mon Sep 17 00:00:00 2001 From: weidong Date: Thu, 18 Apr 2024 23:09:55 +0800 Subject: [PATCH] =?UTF-8?q?1=E3=80=81=E8=A7=A3=E7=AE=97=E5=8F=82=E6=95=B0?= =?UTF-8?q?=E7=BB=84=E5=A2=9E=E5=8A=A0=E7=AE=97=E6=B3=95=E7=89=88=E6=9C=AC?= =?UTF-8?q?=E9=85=8D=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../com/imdroid/secapi/dto/GnssGroupCalc.java | 1 + .../sideslope/bd/FocusCalculator1.java | 2 +- .../sideslope/bd/FocusCalculator2.java | 52 +++++++++- .../calc/SingleLineGNSSCalcService.java | 6 +- sec-beidou/src/main/resources/db/schema.sql | 4 + .../templates/page/gnss_group_cfg.html | 1 + .../page/table/gnss_add_group_calc.html | 10 ++ .../beidou/test_device/task/BeidouDevice.java | 95 +++++++++++++++---- 8 files changed, 146 insertions(+), 25 deletions(-) diff --git a/sec-api/src/main/java/com/imdroid/secapi/dto/GnssGroupCalc.java b/sec-api/src/main/java/com/imdroid/secapi/dto/GnssGroupCalc.java index c6a3a5ef..dda625a8 100644 --- a/sec-api/src/main/java/com/imdroid/secapi/dto/GnssGroupCalc.java +++ b/sec-api/src/main/java/com/imdroid/secapi/dto/GnssGroupCalc.java @@ -16,4 +16,5 @@ public class GnssGroupCalc { Float auto_threshold; Integer device_num; Boolean auto_upload; + Short ver; } diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator1.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator1.java index e1190cb6..db8cc6d7 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator1.java +++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator1.java @@ -14,7 +14,7 @@ import java.util.*; * 6.。。。。。。 */ public class FocusCalculator1 implements FocusCalculator{ - private final Logger logger = LoggerFactory.getLogger(FocusCalculator1.class); + final Logger logger = LoggerFactory.getLogger(FocusCalculator1.class); // b562算法相关：b562固定解的点、计算球心初始半径、迭代步长、最少点数 final List pointList = Collections.synchronizedList(new ArrayList<>()); diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator2.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator2.java index b76d02e0..923ea2e1 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator2.java +++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator2.java @@ -15,8 +15,10 @@ public class FocusCalculator2 extends FocusCalculator1{ final static int MAX_B562_NUM = 3600; final static int B562_EXPIRED_HOUR = 2; final List pointTimeList = Collections.synchronizedList(new ArrayList<>()); + final List pointAuxList = Collections.synchronizedList(new ArrayList<>()); + final List pointTimeAuxList = Collections.synchronizedList(new ArrayList<>()); -/** + /** * 构建计算器，如果tilt0和position0是null，那么计算过程将不考虑与tilts数据融合 * @param height 杆的长度，单位厘米 * @param tilt0 一个小时前的tilt平均值，用一小时的avgTilt计算得到，如果第一次，用null @@ -25,7 +27,6 @@ public class FocusCalculator2 extends FocusCalculator1{ public FocusCalculator2(double height, Tilt tilt0, double[] position0){ super(height,tilt0,position0); iterStep = 0.2; - gravityMaxCount = MAX_B562_NUM; } public FocusCalculator2(){ @@ -35,6 +36,8 @@ public class FocusCalculator2 extends FocusCalculator1{ @Override public void reset(){ //pointList.clear(); + pointAuxList.clear(); + pointTimeAuxList.clear(); counterNoB562 = 0; counterNoFixed = 0; counterFixedResult = 0; @@ -47,10 +50,28 @@ public class FocusCalculator2 extends FocusCalculator1{ @Override public void addXyz(double[] xyz){ LocalDateTime now = LocalDateTime.now(); + + if((int)xyz[3] == UBXUtil.NO_B562) counterNoB562++; + else if((int)xyz[3] == UBXUtil.NO_FIX_RESULT) counterNoFixed++; + else { + counterFixedResult ++; + if (filter(xyz)) { + pointAuxList.add(xyz); + pointTimeAuxList.add(now); + } + if (pointAuxList.size() > gravityMaxCount) { + pointAuxList.remove(0); + pointTimeAuxList.remove(0); + } + } + } + /*public void addXyz(double[] xyz){ + LocalDateTime now = LocalDateTime.now(); Iterator iterPointTime = pointTimeList.iterator(); Iterator iterPoint = pointList.iterator(); while(iterPointTime.hasNext() && iterPoint.hasNext()){ LocalDateTime pointTime = iterPointTime.next(); + iterPoint.next(); if(now.isAfter(pointTime.plusHours(B562_EXPIRED_HOUR))){ iterPointTime.remove(); iterPoint.remove(); @@ -73,11 +94,32 @@ public class FocusCalculator2 extends FocusCalculator1{ pointTimeList.remove(0); } } - } + }*/ public double[] resultB562(double[] lastResult){ - if(counterFixedResult<30) return null; - else return super.resultB562(lastResult); + if(counterFixedResult<50) return null; + // 把本轮的固定解添加到pointList一起解算 + int totalNum = pointList.size()+pointAuxList.size(); + LocalDateTime latestTime = pointTimeAuxList.get(pointTimeAuxList.size()-1); + Iterator iterPointTime = pointTimeList.iterator(); + Iterator iterPoint = pointList.iterator(); + while(iterPointTime.hasNext() && iterPoint.hasNext()){ + LocalDateTime pointTime = iterPointTime.next(); + iterPoint.next(); + if(totalNum>MAX_B562_NUM || latestTime.isAfter(pointTime.plusHours(B562_EXPIRED_HOUR))){ + iterPointTime.remove(); + iterPoint.remove(); + totalNum--; + } + else{ + break; + } + } + pointList.addAll(pointAuxList); + pointTimeList.addAll(pointTimeAuxList); + logger.debug("point list: {},{},{},{}",pointTimeList.get(0),pointTimeAuxList.get(pointTimeAuxList.size()-1),pointTimeList.size(),pointList.size()); + // 求pointList的重心 + return super.resultB562(lastResult); } @Override 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 b4851fc0..033704db 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 @@ -75,14 +75,14 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { //todo 创建FocusCalculator对象需获取该测站的杆长度，上一小时的Tilt平均值，上一小时的测站相对坐标融合值ekfResult FocusCalculator focusCalculator; - /*if(groupCalc!=null && groupCalc.getVer() == 2){ + if(groupCalc!=null && groupCalc.getVer() == 2){ focusCalculator = calculatorMap.computeIfAbsent(deviceId, s -> new FocusCalculator2(200,tiltMap.get(deviceId),positionMap.get(deviceId))); } - else {*/ + else { focusCalculator = calculatorMap.computeIfAbsent(deviceId, s -> new FocusCalculator1(200, tiltMap.get(deviceId),positionMap.get(deviceId))); - //} + } // 读取惯导 Tilt tilt = message.getTilt(); diff --git a/sec-beidou/src/main/resources/db/schema.sql b/sec-beidou/src/main/resources/db/schema.sql index 15a5ecbe..c06affcc 100644 --- a/sec-beidou/src/main/resources/db/schema.sql +++ b/sec-beidou/src/main/resources/db/schema.sql @@ -83,6 +83,7 @@ CREATE TABLE IF NOT EXISTS `gnssgroupcalc` ( `auto_filter` bit(1) DEFAULT 0, `filter_min_hour` int DEFAULT NULL COMMENT '最小平滑窗口', `auto_threshold` float DEFAULT NULL, + `ver` smallint DEFAULT 1 COMMENT '算法版本', `device_num` int DEFAULT 0, `auto_upload` bit(1) DEFAULT 0, PRIMARY KEY (`id`) @@ -143,6 +144,9 @@ CREATE TABLE IF NOT EXISTS `gnssdevicelocationrecords` ( `r9250e` double DEFAULT NULL, `r9250n` double DEFAULT NULL, `r9250d` double DEFAULT NULL, + /*`m9250e` double DEFAULT NULL, + `m9250n` double DEFAULT NULL, + `m9250d` double DEFAULT NULL,*/ `rpose` double DEFAULT NULL COMMENT '相对位置东E', `rposn` double DEFAULT NULL COMMENT '相对位置北N', `rposd` double DEFAULT NULL COMMENT '相对位置天D', diff --git a/sec-beidou/src/main/resources/templates/page/gnss_group_cfg.html b/sec-beidou/src/main/resources/templates/page/gnss_group_cfg.html index d35c9772..41961538 100644 --- a/sec-beidou/src/main/resources/templates/page/gnss_group_cfg.html +++ b/sec-beidou/src/main/resources/templates/page/gnss_group_cfg.html @@ -143,6 +143,7 @@ **/ var cfg2_cols = [ {field: 'id', title: '组号', sort: true}, + {field: 'ver', title: '算法版本'}, {field: 'filter_hour', title: '滤波周期(小时)'}, {field: 'xy_threshold', title: '水平异常门限(mm)'}, {field: 'z_threshold', title: '垂直异常门限(mm)'}, 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 fb6f98e7..e98f1f4a 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 @@ -23,6 +23,15 @@ +
+ +
+ +
+
@@ -133,6 +142,7 @@ $('#filter_min_hour').val(data.filter_min_hour); $('#auto_threshold').val(data.auto_threshold); $('#auto_upload').val(data.auto_upload?'1':'0'); + $('#ver').val(data.ver); form.render(); } diff --git a/sec-test-device/src/main/java/com/imdroid/beidou/test_device/task/BeidouDevice.java b/sec-test-device/src/main/java/com/imdroid/beidou/test_device/task/BeidouDevice.java index ed58de38..fdde6753 100644 --- a/sec-test-device/src/main/java/com/imdroid/beidou/test_device/task/BeidouDevice.java +++ b/sec-test-device/src/main/java/com/imdroid/beidou/test_device/task/BeidouDevice.java @@ -2,11 +2,15 @@ package com.imdroid.beidou.test_device.task; import com.imdroid.beidou.test_device.service.UDPClient; import com.imdroid.common.util.ByteUtil; +import io.netty.buffer.ByteBuf; +import io.netty.buffer.Unpooled; import java.io.BufferedReader; import java.io.FileReader; import java.io.IOException; import java.text.SimpleDateFormat; +import java.time.LocalDateTime; +import java.time.format.DateTimeFormatter; import java.util.Date; public class BeidouDevice { @@ -22,23 +26,24 @@ public class BeidouDevice { public void run(){ try{ + executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_1xx.log"); + executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_2xx.log"); + executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_3xx.log"); + executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_4xx.log"); + executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_5xx.log"); + executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_6xx.log"); + + executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_1.log"); + executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_2.log"); + executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_1xx.log"); + executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_2xx.log"); + executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_3xx.log"); + executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_4xx.log"); + executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_5xx.log"); + executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_6xx.log"); + //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412270_1xx.log"); //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412270_2xx.log"); - //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_1xx.log"); - //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_2xx.log"); - //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_3xx.log"); - //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_4xx.log"); - //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_5xx.log"); - //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_6xx.log"); - - execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_1.log"); - execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_2.log"); - execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_1xx.log"); - execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_2xx.log"); - execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_3xx.log"); - execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_4xx.log"); - execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_5xx.log"); - execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_6xx.log"); } catch (Exception e){ @@ -66,9 +71,9 @@ public class BeidouDevice { Date time = sdf.parse(arrs[0] + " " + arrs[1]); if (lastTime!=0 && Math.abs(time.getTime() - lastTime) > 60 * 1000) {//超过1分钟为一个周期 + System.out.println(time+" cycle "+cycleCount+", b562 num "+b562Count); Thread.sleep(30 * 1000); cycleCount++; - System.out.println(time+" cycle "+cycleCount+", b562 num "+b562Count); } udpClient.sendData(ByteUtil.hexStringTobyte(arrs[2])); b562Count++; @@ -84,4 +89,62 @@ public class BeidouDevice { if(fr!=null) fr.close(); } } + + public void executeAsD342(String dataFileName) throws IOException { + // d3 51 length device_id(4bytes) seq(2bytes) year mon day hour minute + FileReader fr=null; + BufferedReader br=null; + byte[] d342Head = new byte[]{(byte) 0xd3,(byte) 0x42}; + ByteBuf dataBuffer = Unpooled.buffer(); + ByteBuf sendBuffer = Unpooled.buffer(); + try { + fr = new FileReader(dataFileName); + br = new BufferedReader(fr); + String line = ""; + String[] arrs = null; + + DateTimeFormatter formatter = DateTimeFormatter.ofPattern("yyyy-MM-dd HH:mm:ss.SSS"); + + LocalDateTime lastTime=null; + int b562Count = 0; + + while ((line = br.readLine()) != null) { + arrs = line.split(" "); + if(arrs.length != 3) continue; + LocalDateTime time = LocalDateTime.parse(arrs[0] + " " + arrs[1],formatter); + + dataBuffer.writeBytes(ByteUtil.hexStringTobyte(arrs[2])); + b562Count++; + + if((b562Count%10)==0 || + (lastTime!=null && time.isAfter(lastTime.plusMinutes(1)))) { + sendBuffer.writeBytes(d342Head); + sendBuffer.writeShort(dataBuffer.readableBytes()+11); + String deviceId = arrs[2].substring(4*2,8*2); + sendBuffer.writeBytes(ByteUtil.hexStringTobyte(deviceId)); + sendBuffer.writeShort(b562Count); + byte[] byteTime = new byte[]{(byte) (time.getYear() - 2000), (byte) time.getMonthValue(), (byte) time.getDayOfMonth(), (byte) time.getHour(), (byte) time.getMinute()}; + sendBuffer.writeBytes(byteTime); + sendBuffer.writeBytes(dataBuffer); + byte[] data = new byte[sendBuffer.readableBytes()]; + sendBuffer.getBytes(0,data); + udpClient.sendData(data); + Thread.sleep(100); + sendBuffer.clear(); + dataBuffer.clear(); + } + if ((b562Count%100)==0) { + System.out.println(time+" b562 num "+b562Count); + + } + } + br.close(); + fr.close(); + } + catch (Exception e){ + e.printStackTrace(); + if(br!=null) br.close(); + if(fr!=null) fr.close(); + } + } }