diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator3.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator3.java index 32a13028..eb94fd01 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator3.java +++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator3.java @@ -14,37 +14,19 @@ import static com.imdroid.sideslope.bd.GeoCoordConverterUtil.*; * 博通：用GGA绝对坐标代替相对位置 */ public class FocusCalculator3 extends FocusCalculator1{ + //final static long scale = 100000000L;//地球1°：111km，放大到mm乘以100,000,000 + final static int bad_change_mm = 500;//固定解跳变连续10次超过500mm，认为是周跳 + final static int bad_duration = 10; + + int bad_count = 0; Device bsDevice; + public FocusCalculator3(Device bsDevice){ super(); this.bsDevice = bsDevice; gravityMinCount = 80; } -/* - static class EComparator implements Comparator{ - @Override - public int compare(double[] point1, double[] point2) { - return Double.compare(point1[1], point2[1]); - //return (int) ((point1[0] - point2[0])*100); - } - } - static class NComparator implements Comparator{ - @Override - public int compare(double[] point1, double[] point2) { - return Double.compare(point1[1], point2[1]); - //return (int) ((point1[1] - point2[1])*100); - } - } - - static class DComparator implements Comparator{ - @Override - public int compare(double[] point1, double[] point2) { - return Double.compare(point1[1], point2[1]); - //return (int) ((point1[2] - point2[2])*100); - } - } -*/ @Override public void addGGA(Gga gga) { if(gga == null) return; @@ -69,7 +51,6 @@ public class FocusCalculator3 extends FocusCalculator1{ // 以基站为站心的 ENU 坐标 ENU_Coordinate difference_enu = ECEF2ENU(reference_ecef, rover_ecef); - System.out.println("DIFF ENU:" + difference_enu.ENU_E + " " + difference_enu.ENU_N + " " + difference_enu.ENU_U); end = new double[]{ difference_enu.ENU_E*1000, difference_enu.ENU_N*1000, @@ -79,7 +60,17 @@ public class FocusCalculator3 extends FocusCalculator1{ if(gga.isFixed()) { counterFixedResult++; - //pointList.add(xyz); + if(pointList.size()>0){ + double[] lastXyz = pointList.get(pointList.size()-1); + if(Math.abs(end[0]-lastXyz[0])>bad_change_mm || + Math.abs(end[1]-lastXyz[1])>bad_change_mm || + Math.abs(end[2]-lastXyz[2])>bad_change_mm){ + bad_count++; + return; + } + } + bad_count = 0; + pointList.add(end); } else if(gga.getQuality() == 5) counterNoFixed++; @@ -95,33 +86,6 @@ public class FocusCalculator3 extends FocusCalculator1{ } - /*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= 3){ - 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 public double[] resultB562(){ try { @@ -166,9 +130,23 @@ public class FocusCalculator3 extends FocusCalculator1{ return null; } + @Override + public void reset(){ + super.reset(); + bad_count = 0; + } + @Override public int getVer() { return 3; } + public boolean isJump(){ + if (bad_count>bad_duration){ + bad_count = 0; + return true; + } + else return false; + } + } diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator7.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator7.java deleted file mode 100644 index 821c78b7..00000000 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator7.java +++ /dev/null @@ -1,117 +0,0 @@ -package com.imdroid.sideslope.bd; - -import com.imdroid.sideslope.sal.Device; - -import static com.imdroid.sideslope.bd.GeoCoordConverterUtil.ECEF2ENU; -import static com.imdroid.sideslope.bd.GeoCoordConverterUtil.LLA2ECEF; - - -/** - * 博通：用GGA绝对坐标代替相对位置 - */ -public class FocusCalculator7 extends FocusCalculator3{ - - //final static long scale = 100000000L;//地球1°：111km，放大到mm乘以100,000,000 - final static int bad_change_mm = 500;//固定解跳变连续10次超过500mm，认为是周跳 - final static int bad_duration = 10; - - int bad_count = 0; - - public FocusCalculator7(Device bsDevice) { - super(bsDevice); - } - - /*@Override - public void addGGA(Gga gga) { - if(gga == null) return; - - double[] xyz = new double[]{gga.getLongitude()*scale, gga.getLatitude()*scale, gga.getAltitude()*1000, gga.getQuality()}; - - if(gga.isFixed()) { - counterFixedResult++; - if(pointList.size()>0){ - double[] lastXyz = pointList.get(pointList.size()-1); - if(Math.abs(xyz[0]-lastXyz[0])>bad_change_mm || - Math.abs(xyz[1]-lastXyz[1])>bad_change_mm || - Math.abs(xyz[2]-lastXyz[2])>bad_change_mm){ - bad_count++; - return; - } - } - bad_count = 0; - - pointList.add(xyz); - } - else if(gga.getQuality() == 5) counterNoFixed++; - else counterNoB562 ++; - }*/ - - @Override - public void addGGA(Gga gga) { - if(gga == null) return; - double[] end; - - // 测站的GGA - 测站 LLA 数据 - GeoCoordConverterUtil.LLA_Coordinate rover_lla = new GeoCoordConverterUtil.LLA_Coordinate(gga.getLatitude(),gga.getLongitude(),gga.getAltitude()); - // 测站 LLA 坐标转 ECEF 坐标，单位米 - GeoCoordConverterUtil.ECEF_Coordinate rover_ecef = LLA2ECEF(rover_lla); - - if(bsDevice==null || bsDevice.getEcefx()==null){ - end = new double[]{ - rover_ecef.getECEF_X()*1000, - rover_ecef.getECEF_Y()*1000, - rover_ecef.getECEF_Z()*1000, - gga.getQuality()}; - } - else { - // 查询测站的绑定的基站 - GeoCoordConverterUtil.ECEF_Coordinate reference_ecef = new GeoCoordConverterUtil.ECEF_Coordinate(bsDevice.getEcefx(), bsDevice.getEcefy(), bsDevice.getEcefz()); - - // 以基站为站心的 ENU 坐标 - GeoCoordConverterUtil.ENU_Coordinate difference_enu = ECEF2ENU(reference_ecef, rover_ecef); - end = new double[]{ - difference_enu.ENU_E*1000, - difference_enu.ENU_N*1000, - difference_enu.ENU_U*1000, - gga.getQuality()}; - } - - if(gga.isFixed()) { - counterFixedResult++; - if(pointList.size()>0){ - double[] lastXyz = pointList.get(pointList.size()-1); - if(Math.abs(end[0]-lastXyz[0])>bad_change_mm || - Math.abs(end[1]-lastXyz[1])>bad_change_mm || - Math.abs(end[2]-lastXyz[2])>bad_change_mm){ - bad_count++; - return; - } - } - bad_count = 0; - - pointList.add(end); - } - else if(gga.getQuality() == 5) counterNoFixed++; - else counterNoB562 ++; - } - - @Override - public void reset(){ - super.reset(); - bad_count = 0; - } - - @Override - public int getVer() { - return 7; - } - - public boolean isJump(){ - if (bad_count>bad_duration){ - bad_count = 0; - return true; - } - else return false; - } - -} 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 61fb2180..e9f745a5 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 @@ -85,16 +85,13 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { if(device == null) return; GnssGroupCalc groupCalc = getGroupCalc(device.getCalcGroupId()); if(groupCalc==null) return; - device.setB562AsCalc(groupCalc.getVer()!=3 && groupCalc.getVer()!=7); + device.setB562AsCalc(groupCalc.getVer()!=3); if(completeWhenIdle) resultOutputTimer(device, groupCalc, message.getCreateTime()); //todo 创建FocusCalculator对象需获取该测站的杆长度，上一小时的Tilt平均值，上一小时的测站相对坐标融合值ekfResult FocusCalculator focusCalculator; - if(groupCalc.getVer() == 7){ - focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator7(null)); - } - else if(groupCalc.getVer() == 6){ + if(groupCalc.getVer() == 6){ focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator6(deviceId, 50)); } else if(groupCalc.getVer() == 5){ @@ -145,7 +142,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { focusCalculator.addGGA(gga); logger.debug("测站{}的gga单次解析结果:{},{},{},{}",deviceId, gga.getLongitude(), gga.getLatitude(), gga.getAltitude(), gga.getQuality()); - if(groupCalc.getVer() == 7 && focusCalculator.isJump()){ + if(groupCalc.getVer() == 3 && focusCalculator.isJump()){ logger.info("{}发生周跳",deviceId); hardResetDevice(deviceId); } @@ -273,7 +270,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { gnssCalcFilterService.calc(device, groupCalc, locationRecord, latestRpos); } else { - if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.getVer()==5 || focusCalculator.getVer()==7){ + if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.getVer()==5){ gnssCalcFilterService.calc(device, groupCalc, locationRecord, null); } else { @@ -316,7 +313,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { else focusCalculator.setReferPoint(b562Result); } } - else if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.getVer()==5 || focusCalculator.getVer()==7) { + else if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.getVer()==5) { focusCalculator.setReferPoint(b562Result); } else {