diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator.java index e972e202..6804db81 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator.java +++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator.java @@ -1,11 +1,13 @@ package com.imdroid.sideslope.bd; +import java.time.LocalDateTime; + public interface FocusCalculator { void reset(); // 数据收集：b562位置、惯导角度、基站到测站时延 - void addXyz(double[] xyz); + void addXyz(double[] xyz, LocalDateTime dataTime); void addTilt(Tilt tilt); void addDelayMs(int ms); 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 09ebf6b0..00263f54 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 @@ -76,7 +76,7 @@ public class FocusCalculator1 implements FocusCalculator{ * @param xyz */ @Override - public void addXyz(double[] xyz){ + public void addXyz(double[] xyz, LocalDateTime dataTime){ if((int)xyz[3] == UBXUtil.NO_B562) counterNoB562++; else if((int)xyz[3] == UBXUtil.FLOAT_RESULT) counterNoFixed++; else { @@ -112,9 +112,12 @@ public class FocusCalculator1 implements FocusCalculator{ } @Override public double[] getReferPoint(){ - if(referPoint==null || referPointTime==null) return null; + if(referPoint==null || referPointTime==null) { + return null; + } if(LocalDateTime.now().isAfter(referPointTime.plusHours(24))) { + logger.info("refer time is expired"); referPoint = null; } return referPoint; 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 1d215d4b..0df10d7d 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 @@ -12,35 +12,35 @@ import java.util.*; * 6.。。。。。。 */ public class FocusCalculator2 extends FocusCalculator1{ - public final static int MAX_B562_NUM = 1800; - final static int B562_EXPIRED_HOUR = 1; + final static int B562_EXPIRED_HOUR = 4; - final List pointTimeList = Collections.synchronizedList(new ArrayList<>()); - final List pointAuxList = Collections.synchronizedList(new ArrayList<>()); - final List pointTimeAuxList = Collections.synchronizedList(new ArrayList<>()); + final List>> cyclePointList = Collections.synchronizedList(new ArrayList<>()); - boolean referPointFilteredFlag = false; - LocalDateTime initReferPointTime; + final static int INIT_REFER_POINT_NOT_SET = 0; + final static int INIT_REFER_POINT_IN_USE = 1; + final static int INIT_REFER_POINT_EXPIRED = 2; + int originalReferPointFlag= INIT_REFER_POINT_NOT_SET; // 0:not set; 1:in use; 2:expired + LocalDateTime originalReferPointTime; - /* public FocusCalculator2(double height, Tilt tilt0, double[] position0){ - super(); - iterStep = 0.2; + class EComparator implements Comparator{ + @Override + public int compare(double[] point1, double[] point2) { + return (int) ((point1[0] - point2[0])*100); + } + } + + class NComparator implements Comparator{ + @Override + public int compare(double[] point1, double[] point2) { + return (int) ((point1[1] - point2[1])*100); + } } -*/ - public FocusCalculator2(){ - super(); - iterStep = 2;//2mm - gravityMaxR = 500; - } - - @Override - public void reset(){ - //pointList.clear(); - pointAuxList.clear(); - pointTimeAuxList.clear(); - counterNoB562 = 0; - counterNoFixed = 0; - counterFixedResult = 0; + + class DComparator implements Comparator{ + @Override + public int compare(double[] point1, double[] point2) { + return (int) ((point1[2] - point2[2])*100); + } } /** @@ -48,85 +48,70 @@ public class FocusCalculator2 extends FocusCalculator1{ * @param xyz */ @Override - public void addXyz(double[] xyz){ - LocalDateTime now = LocalDateTime.now(); + public void addXyz(double[] xyz, LocalDateTime dataTime){ + Map.Entry entry = null; + if(cyclePointList.size()==0 || counterFixedResult+counterNoB562+counterNoFixed == 0){ + entry = new AbstractMap.SimpleEntry(dataTime, new ArrayList()); + cyclePointList.add(entry); + // 检查有没有过期的数据 + Iterator>> iterator = cyclePointList.iterator(); + while (iterator.hasNext()){ + LocalDateTime t = iterator.next().getKey(); + if(t.isBefore(dataTime.minusHours(B562_EXPIRED_HOUR))){ + iterator.remove(); + } + } + } + else{ + entry = cyclePointList.get(cyclePointList.size()-1); + } if((int)xyz[3] == UBXUtil.NO_B562) counterNoB562++; else if((int)xyz[3] == UBXUtil.FLOAT_RESULT) counterNoFixed++; else { counterFixedResult ++; if (filter(xyz)) { - pointAuxList.add(xyz); - pointTimeAuxList.add(now); - } - if (pointAuxList.size() > gravityMaxCount) { - pointAuxList.remove(0); - pointTimeAuxList.remove(0); + List list = (List) entry.getValue(); + list.add(xyz); } } } - /*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(); - } - else{ - break; - } - } - if((int)xyz[3] == UBXUtil.NO_B562) counterNoB562++; - else if((int)xyz[3] == UBXUtil.NO_FIX_RESULT) counterNoFixed++; - else { - counterFixedResult ++; - if (filter(xyz)) { - pointList.add(xyz); - pointTimeList.add(now); - } - if (pointList.size() > gravityMaxCount) { - pointList.remove(0); - pointTimeList.remove(0); - } + double calcGravity(List list , int index){ + double sum = 0; + int begin = (int) (list.size() * 0.25); + int end = (int) (list.size() * 0.75); + if(end-begin == 0) return 0; + + for(int i=begin; i 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; - } + if(counterFixedResult<30) return null; + // 求pointList的重心：当固定解大于300个时，去掉25%最大值和25%最小值 + pointList.clear(); + Iterator>> iterator = cyclePointList.iterator(); + while (iterator.hasNext()){ + pointList.addAll(iterator.next().getValue()); } - pointList.addAll(pointAuxList); - pointTimeList.addAll(pointTimeAuxList); - logger.info("point list: {},{},{},{}",pointTimeList.get(0),pointTimeAuxList.get(pointTimeAuxList.size()-1),pointTimeList.size(),pointList.size()); - // 求pointList的重心 - return super.resultB562(); - } + logger.info("total fixed point num to calculated: {},{},{}",pointList.size(), + cyclePointList.get(0).getKey(), cyclePointList.get(cyclePointList.size()-1).getKey()); - @Override - public int[] getB562Stat(){ - int[] b562Stat = new int[]{counterFixedResult,counterNoFixed,counterNoB562}; - reset(); - return b562Stat; + if(pointList.size() >= gravityMaxCount){ + Collections.sort(pointList, new EComparator()); + double e = calcGravity(pointList,0); + Collections.sort(pointList, new NComparator()); + double n = calcGravity(pointList,1); + Collections.sort(pointList, new DComparator()); + double d = calcGravity(pointList,2); + return new double[]{e,n,d}; + } + + return null; } @Override @@ -146,24 +131,25 @@ public class FocusCalculator2 extends FocusCalculator1{ @Override public void setReferPoint(double[] point){ - if(referPointFilteredFlag){ + if(originalReferPointFlag==INIT_REFER_POINT_IN_USE){ // 初始参考位置保持24小时 - if(initReferPointTime!=null && - LocalDateTime.now().isAfter(initReferPointTime.plusHours(24))){ - referPoint = point; + if(originalReferPointTime!=null && + LocalDateTime.now().isBefore(originalReferPointTime.plusHours(24))){ + return; } + else originalReferPointFlag = INIT_REFER_POINT_EXPIRED; } - else{ - referPoint = point; - } + referPoint = point; + referPointTime = LocalDateTime.now(); } - public boolean isReferPointFiltered(){ - return referPointFilteredFlag; + public boolean isOriginalReferPointSet(){ + return (originalReferPointFlag!=INIT_REFER_POINT_NOT_SET); } - public void setInitReferPoint(double[] point){ + public void setOriginalReferPoint(double[] point){ referPoint = point; - referPointFilteredFlag = true; - initReferPointTime = LocalDateTime.now(); + referPointTime = LocalDateTime.now(); + originalReferPointFlag = INIT_REFER_POINT_IN_USE; + originalReferPointTime = LocalDateTime.now(); } } 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 78551853..623ff1a3 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 @@ -97,7 +97,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { // 单次b562 double[] doubles = message.getB562_loc();//unit: mm - focusCalculator.addXyz(doubles); + focusCalculator.addXyz(doubles,message.getCreateTime()); if (logger.isDebugEnabled()) { logger.debug("测站" + deviceId + "的b562单次解析结果:{}", Arrays.toString(doubles)); } @@ -207,7 +207,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { } else { FocusCalculator2 focusCalculator2 = (FocusCalculator2)focusCalculator; - if(focusCalculator2.isReferPointFiltered()) { + if(focusCalculator2.isOriginalReferPointSet()) { if (locationRecord.getEnabled() && locationRecord.getRpose() != null) { focusCalculator2.setReferPoint(new double[]{locationRecord.getRpose(), locationRecord.getRposn(), locationRecord.getRposd()}); logger.info("{} set reference pos to filter result:{},{},{}", @@ -220,7 +220,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { else{ // 查询设备有没有配置初始位置 if(device.getIPose()!=null){ - focusCalculator2.setInitReferPoint(new double[]{device.getIPose(),device.getIPosn(),device.getIPosd()}); + focusCalculator2.setOriginalReferPoint(new double[]{device.getIPose(),device.getIPosn(),device.getIPosd()}); logger.info("{} set reference pos to init pos:{},{},{}", deviceId,device.getIPose(),device.getIPosn(),device.getIPosd()); } 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 6db71de8..752d4ab3 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 @@ -42,7 +42,7 @@ public class BeidouDevice { execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_5xx.log"); execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_6xx.log"); */ -/* + 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"); @@ -69,16 +69,16 @@ public class BeidouDevice { //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412270_1.log"); //executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412270_1xx.log"); //executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412270_2xx.log"); - executeAsD342("C:\\Users\\wd\\Desktop\\log\\2345076_0424_1.log"); +/* executeAsD342("C:\\Users\\wd\\Desktop\\log\\2345076_0424_1.log"); executeAsD342("C:\\Users\\wd\\Desktop\\log\\2345076_0424_2.log"); executeAsD342("C:\\Users\\wd\\Desktop\\log\\2345076_0424_1xx.log"); executeAsD342("C:\\Users\\wd\\Desktop\\log\\2345076_0424_2xx.log"); executeAsD342("C:\\Users\\wd\\Desktop\\log\\2345076_0424_3xx.log"); executeAsD342("C:\\Users\\wd\\Desktop\\log\\2345076_0424_4xx.log"); executeAsD342("C:\\Users\\wd\\Desktop\\log\\2345076_0424_5xx.log"); - */ + execute("C:\\Users\\wd\\Desktop\\log\\2353120_0424_1.log"); -/* + executeAsD342("C:\\Users\\wd\\Desktop\\log\\2353120_0424_1.log"); executeAsD342("C:\\Users\\wd\\Desktop\\log\\2353120_0424_2.log"); executeAsD342("C:\\Users\\wd\\Desktop\\log\\2353120_0424_1xx.log");