1、国产算法增加周跳判断

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<double[]>{
-         @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<double[]>{
-        @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<double[]>{
-        @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<double[]> 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<end; i++){
-            sum += list.get(i)[index];
-        }
-        return sum/(end-begin);
-    }
-
-    @Override
-    public double[] resultB562(){
-        if(pointList.size() >= 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;
+    }
+
 }

sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator7.java

@@ -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;
-    }
-
-}

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){
+        if(groupCalc.getVer() == 6){
-            focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator7(null));
-        }
-        else 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 {