算法7改为ECEF+判断周跳

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

@@ -2,13 +2,16 @@ 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 long scale = 100000000L;//地球1°：111km，放大到mm乘以100,000,000
     final static int bad_change_mm = 500;//固定解跳变连续10次超过500mm，认为是周跳
     final static int bad_duration = 10;
 
@@ -18,7 +21,7 @@ public class FocusCalculator7 extends FocusCalculator3{
         super(bsDevice);
     }
 
-    @Override
+    /*@Override
     public void addGGA(Gga gga) {
         if(gga == null) return;
 
@@ -41,6 +44,55 @@ public class FocusCalculator7 extends FocusCalculator3{
         }
         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