1、增加算法5

sec-api/src/main/java/com/imdroid/secapi/dto/GnssGroupCalc.java

@@ -19,4 +19,5 @@ public class GnssGroupCalc {
     Integer device_num;
     Boolean auto_upload;
     Short ver;
+    Short remove_rate;
 }

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

@@ -0,0 +1,248 @@
+package com.imdroid.sideslope.bd;
+
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
+import java.time.LocalDateTime;
+import java.util.*;
+
+/**
+ * 1.如果本次有效解数量少于50个，本轮不处理，累计到下次
+ * 2.enu分别排序，去掉前后20%，然后取交集，再求平均
+ * 3.最多取最后500*6=3000个点计算
+ */
+public class FocusCalculator5 implements FocusCalculator{
+    final Logger logger = LoggerFactory.getLogger(FocusCalculator5.class);
+    // 固定解4元组：E、N、U、Quality
+    final List<double[]> pointList = Collections.synchronizedList(new ArrayList<>());
+    String deviceId;
+    int minPointNum = 50;
+    int maxPointNum = 3000;
+    double[] referPoint; //参考点，一般是上一次计算的位置，用来辅助计算重心
+    LocalDateTime referPointTime;
+    short removeRate;
+
+    // 惯导算法相关
+    final List<Tilt> tilts = Collections.synchronizedList(new ArrayList<>());
+    boolean isShock = false;
+    final double shockThreshold = 1.5;
+
+    //其他
+    int delay_ms;
+    int delay_counter;
+    int counterNoB562;
+    int counterNoFixed;
+    int counterFixedResult;
+
+    static class EComparator implements Comparator<double[]>{
+        @Override
+        public int compare(double[] point1, double[] point2) {
+            return (int) ((point1[0] - point2[0])*100);
+        }
+    }
+
+    static class NComparator implements Comparator<double[]>{
+        @Override
+        public int compare(double[] point1, double[] point2) {
+            return (int) ((point1[1] - point2[1])*100);
+        }
+    }
+
+    static class UComparator implements Comparator<double[]>{
+        @Override
+        public int compare(double[] point1, double[] point2) {
+            return (int) ((point1[2] - point2[2])*100);
+        }
+    }
+
+    public FocusCalculator5(){
+        counterNoB562 = 0;
+        counterNoFixed = 0;
+        counterFixedResult = 0;
+        delay_ms = 0;
+        delay_counter = 0;
+        removeRate = 10;
+    }
+
+    public FocusCalculator5(String deviceId, int minPointNum, short removeRate){
+        counterNoB562 = 0;
+        counterNoFixed = 0;
+        counterFixedResult = 0;
+        delay_ms = 0;
+        delay_counter = 0;
+        this.deviceId = deviceId;
+        this.minPointNum = minPointNum;
+        this.removeRate = (short) (removeRate/2);
+    }
+
+    public void setParas(String deviceId, int minPointNum, short removeRate){
+        this.deviceId = deviceId;
+        this.minPointNum = minPointNum;
+        this.removeRate = (short) (removeRate/2);
+    }
+
+    @Override
+    public void reset(){
+        if(pointList.size()>=minPointNum) {
+            pointList.clear();
+        }
+        else{
+            logger.info("{} num of fixed points:{}, less than {}",deviceId, pointList.size(),minPointNum);
+        }
+        tilts.clear();
+        counterNoB562 = 0;
+        counterNoFixed = 0;
+        counterFixedResult = 0;
+        delay_ms = 0;
+        delay_counter = 0;
+    }
+
+    /**
+     * 加入pointList
+     * @param xyz
+     */
+    @Override
+    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 {
+            counterFixedResult ++;
+            if (filter(xyz)) {//是否是无效值null
+                pointList.add(xyz);
+            }
+            if (pointList.size() > maxPointNum) {
+                pointList.remove(0);
+            }
+        }
+    }
+
+    @Override
+    public int[] getB562Stat(){
+        return new int[]{counterFixedResult,counterNoFixed,counterNoB562};
+    }
+
+    @Override
+    public void setReferPoint(double[] point){
+        referPoint = point;
+        referPointTime = LocalDateTime.now();
+    }
+    @Override
+    public double[] getReferPoint(){
+        if(referPoint==null || referPointTime==null) {
+            return null;
+        }
+
+        if(LocalDateTime.now().isAfter(referPointTime.plusHours(24))) {
+            logger.info("{} refer time is expired", deviceId);
+            referPoint = null;
+        }
+        return referPoint;
+    }
+    @Override
+    public double[] resultB562(){
+        if(pointList.size()<minPointNum) return null;
+        //enu分别排序并取中间值
+        List<double[]> eList = sortList(new EComparator());
+        List<double[]> nList = sortList(new NComparator());
+        List<double[]> uList = sortList(new UComparator());
+        //取交集
+        eList.retainAll(nList);
+        eList.retainAll(uList);
+        logger.info("{} fixed point selected: {}", deviceId, eList.size());
+        if(eList.size() == 0) return null;
+
+        //求均值
+        double eSum = 0;
+        double nSum = 0;
+        double uSum = 0;
+        for(double[] item : eList){
+            eSum += item[0];
+            nSum += item[1];
+            uSum += item[2];
+        }
+        return new double[]{eSum/eList.size(), nSum/eList.size(), uSum/eList.size()};
+    }
+
+    List<double[]> sortList(Comparator comparator){
+        List<double[]> pList = new ArrayList<>();
+        pList.addAll(pointList);
+        pList.sort(comparator);
+        int begin = pList.size()*removeRate/100;
+        int end = pList.size()-begin;
+        return pList.subList(begin, end);
+    }
+
+
+    /**
+     * 过滤，剔除0,无穷大，与nan
+     * @param xyz
+     * @return true表示数据正常
+     */
+    boolean filter(double[] xyz){
+        double a = xyz[0]*xyz[1]*xyz[2];
+        if(a == 0 || Double.isInfinite(a) || Double.isNaN(a)){
+            return false;
+        }
+        return true;
+    }
+
+    /**
+     * 加入tilts
+     * @param tilt
+     */
+    @Override
+    public void addTilt(Tilt tilt){
+        tilts.add(tilt);
+        if(tilt.getShock() > shockThreshold) isShock = true;
+
+        if(tilts.size() > maxPointNum){
+            tilts.remove(0);
+        }
+    }
+
+    @Override
+    public boolean isShocked() {
+        return isShock;
+    }
+
+    @Override
+    public int getVer() {
+        return 5;
+    }
+
+    @Override
+    public Tilt avgTilt() {
+        if (tilts.size() == 0) {
+            return null;
+        }
+        Iterator<Tilt> iterator = tilts.iterator();
+        double roll = 0;
+        double pitch = 0;
+        double yaw = 0;
+        while (iterator.hasNext()) {
+            Tilt tilt = iterator.next();
+            roll += tilt.getRoll();
+            pitch += tilt.getPitch();
+            yaw += tilt.getYaw();
+        }
+        return new Tilt(pitch/tilts.size(),roll/tilts.size(),yaw/tilts.size());
+    }
+
+    @Override
+    public void addDelayMs(int ms){
+        delay_ms += ms;
+        delay_counter ++;
+    }
+
+    @Override
+    public void addGGA(Gga gga) {
+
+    }
+
+    @Override
+    public int getAvgDelayMs(){
+        if(delay_counter==0) return -1;
+        else return delay_ms/delay_counter;
+    }
+
+}

sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/SingleLineGNSSCalcService.java

@@ -81,6 +81,16 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
 
         //todo 创建FocusCalculator对象需获取该测站的杆长度，上一小时的Tilt平均值，上一小时的测站相对坐标融合值ekfResult
         FocusCalculator focusCalculator;
+        if(groupCalc!=null && groupCalc.getVer() == 5){
+            focusCalculator = calculatorMap.get(deviceId);
+            if(focusCalculator ==null) {
+                Short removeRate = groupCalc.getRemove_rate();
+                if (removeRate == null) removeRate = 20;
+                focusCalculator = new FocusCalculator5(deviceId, 50, removeRate);
+                //((FocusCalculator5) focusCalculator).setParas(deviceId, 50, removeRate);
+                calculatorMap.put(deviceId,focusCalculator);
+            }
+        }
         if(groupCalc!=null && groupCalc.getVer() == 4){
             focusCalculator = calculatorMap.computeIfAbsent(deviceId,s -> new FocusCalculator4());
         }
@@ -111,14 +121,14 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
         double[] doubles = message.getB562_loc();//unit: mm
         if(doubles !=null) {
             focusCalculator.addXyz(doubles, message.getCreateTime());
-            logger.info("测站" + deviceId + "的b562单次解析结果:{}", Arrays.toString(doubles));
+            logger.debug("测站" + deviceId + "的b562单次解析结果:{}", Arrays.toString(doubles));
         }
 
         // 单次GGA
         Gga gga = message.getGga();
         if(gga != null) {
             focusCalculator.addGGA(message.getGga());
-            logger.info("测站" + deviceId + "的gga单次解析结果:{},{},{},{}",
+            logger.debug("测站" + deviceId + "的gga单次解析结果:{},{},{},{}",
                         gga.getLongitude(), gga.getLatitude(), gga.getAltitude(), gga.getQuality());
         }
     }
@@ -231,7 +241,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
                     else focusCalculator.setReferPoint(b562Result);
                 }
             }
-            else if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.isShocked()) {
+            else if(focusCalculator.getVer()==1 || focusCalculator.getVer()==3 || focusCalculator.getVer()==5 || focusCalculator.isShocked()) {
                 focusCalculator.setReferPoint(b562Result);
             }
             else {

sec-beidou-rtcm/src/test/java/FocusCalculatorTest.java

@@ -1,6 +1,7 @@
 import com.imdroid.sideslope.bd.FocusCalculator;
 import com.imdroid.sideslope.bd.FocusCalculator1;
 import com.imdroid.sideslope.bd.FocusCalculator2;
+import com.imdroid.sideslope.bd.FocusCalculator5;
 import org.ejml.simple.SimpleMatrix;
 import org.junit.Test;
 import org.junit.runner.RunWith;
@@ -23,7 +24,8 @@ import java.util.ListIterator;
         classes = {
                 FocusCalculatorTest.class,
                 FocusCalculator1.class,
-                FocusCalculator2.class
+                FocusCalculator2.class,
+                FocusCalculator5.class
         }
 )
 
@@ -46,15 +48,16 @@ public class FocusCalculatorTest {
     @Test
     public void run() throws IOException, ParseException {
         System.out.println("*********** 2307042 *************");
-        System.out.println("算法1");
+        /*System.out.println("算法1");
         focusCalculator = new FocusCalculator1();
         test();
         System.out.println("算法2");
         focusCalculator = new FocusCalculator2();
-        test();
-        /*System.out.println("算法3");
-        focusCalculator = new FocusCalculator3();
         test();*/
+        System.out.println("算法3");
+        focusCalculator = new FocusCalculator5();
+        ((FocusCalculator5)focusCalculator).setParas("2307042",50, (short) 20);
+        test();
     }
 
 
@@ -79,9 +82,9 @@ public class FocusCalculatorTest {
                 //if(calcResultList.size()>=36) break;//18个小时
             }
             focusCalculator.addXyz(new double[]{
-                    Double.parseDouble(arrs[2]),
+                    Double.parseDouble(arrs[2])*10,
-                    Double.parseDouble(arrs[3]),
+                    Double.parseDouble(arrs[3])*10,
-                    Double.parseDouble(arrs[4])},LocalDateTime.now());
+                    Double.parseDouble(arrs[4])*10,2},LocalDateTime.now());
         }
         br.close();
         fr.close();
@@ -97,6 +100,7 @@ public class FocusCalculatorTest {
         runFilter(4);
         calcStd(filterResults);
         filterResults.removeAll(filterResults);
+        /*
         System.out.println("4 hours filter, igg3");
         runFilter2(4);
         calcStd(filterResults);
@@ -110,7 +114,7 @@ public class FocusCalculatorTest {
         runFilter2(8);
         calcStd(filterResults);
         filterResults.removeAll(filterResults);
-
+*/
         calcResultList.removeAll(calcResultList);
 
     }

sec-beidou/src/main/resources/db/schema.sql

@@ -96,6 +96,7 @@ CREATE TABLE IF NOT EXISTS `gnssgroupcalc` (
     `ver` smallint DEFAULT 1 COMMENT '算法版本',
     `device_num` int DEFAULT 0,
     `auto_upload` bit(1) DEFAULT 0,
+    `remove_rate` smallint DEFAULT NULL COMMENT '固定解剔除比例',
     PRIMARY KEY (`id`)
     ) ENGINE=InnoDB DEFAULT CHARSET=utf8mb4;
 

sec-beidou/src/main/resources/templates/page/table/gnss_add_group_calc.html

@@ -37,6 +37,7 @@
                     <option value="2">2</option>
                     <option value="3">3</option>
                     <option value="4">4</option>
+                    <option value="5">5</option>
                 </select>
             </div>
         </div>
@@ -103,6 +104,12 @@
                 </select>
             </div>
         </div>
+        <div class="layui-form-item">
+            <label class="layui-form-label">剔除比例(%)</label>
+            <div class="layui-input-block">
+                <input type="number" name="remove_rate" id="remove_rate" class="layui-input">
+            </div>
+        </div>
 
         <div class="layui-form-item">
             <div class="layui-input-block">
@@ -157,6 +164,7 @@
             $('#auto_upload').val(data.auto_upload?'1':'0');
             $('#ver').val(data.ver);
             $('#name').val(data.name);
+            $('#remove_rate').val(data.remove_rate);
             form.render();
         }
     </script>

sec-beidou/src/test/java/UtilTest.java

@@ -1,4 +1,4 @@
-import com.imdroid.secapi.utils.HexUtil;
+import com.imdroid.common.util.HexUtil;
 
 public class UtilTest {
     public static void main(String[] args) {

sec-test-device/src/main/java/com/imdroid/beidou/test_device/BeidouTestApp.java

@@ -1,23 +1,22 @@
 package com.imdroid.beidou.test_device;
 
+import com.imdroid.beidou.test_device.task.BeidouDevice;
 import org.springframework.boot.SpringApplication;
 import org.springframework.boot.autoconfigure.SpringBootApplication;
-import org.springframework.context.annotation.ComponentScan;
 
 
 /**
  * @author Layton
  * @date 2023/1/31 20:33
  */
-@SpringBootApplication(scanBasePackages = {"com.imdroid"})
+@SpringBootApplication(scanBasePackages = {"com.imdroid.beidou.test_device"})
-@ComponentScan({"com.imdroid.*"})
 public class BeidouTestApp {
 
     public static void main(String[] args) {
 
         SpringApplication.run(BeidouTestApp.class, args);
-        /*BeidouDevice beidouDevice = new BeidouDevice();
+        BeidouDevice beidouDevice = new BeidouDevice();
         beidouDevice.connectServer();
-        beidouDevice.run();*/
+        beidouDevice.run();
     }
 }

sec-test-device/src/main/java/com/imdroid/beidou/test_device/task/BeidouDevice.java

@@ -119,9 +119,6 @@ public class BeidouDevice {
                     Thread.sleep(30 * 1000);
                     cycleCount++;
                 }
-                if(arrs[2].contains("24474e47")){
-                    System.out.println(time+" 24474e47");
-                }
 
                 udpClient.sendData(ByteUtil.hexStringTobyte(arrs[2]));
                 b562Count++;