1、解算参数组增加算法版本配置

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

@@ -16,4 +16,5 @@ public class GnssGroupCalc {
     Float auto_threshold;
     Integer device_num;
     Boolean auto_upload;
+    Short ver;
 }

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

@@ -14,7 +14,7 @@ import java.util.*;
  * 6.。。。。。。
  */
 public class FocusCalculator1 implements FocusCalculator{
-    private final Logger logger = LoggerFactory.getLogger(FocusCalculator1.class);
+    final Logger logger = LoggerFactory.getLogger(FocusCalculator1.class);
 
     // b562算法相关：b562固定解的点、计算球心初始半径、迭代步长、最少点数
     final List<double[]> pointList = Collections.synchronizedList(new ArrayList<>());

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

@@ -15,6 +15,8 @@ public class FocusCalculator2 extends FocusCalculator1{
     final static int MAX_B562_NUM = 3600;
     final static int B562_EXPIRED_HOUR = 2;
     final List<LocalDateTime> pointTimeList = Collections.synchronizedList(new ArrayList<>());
+    final List<double[]> pointAuxList = Collections.synchronizedList(new ArrayList<>());
+    final List<LocalDateTime> pointTimeAuxList = Collections.synchronizedList(new ArrayList<>());
 
     /**
      * 构建计算器，如果tilt0和position0是null，那么计算过程将不考虑与tilts数据融合
@@ -25,7 +27,6 @@ public class FocusCalculator2 extends FocusCalculator1{
     public FocusCalculator2(double height, Tilt tilt0, double[] position0){
         super(height,tilt0,position0);
         iterStep = 0.2;
-        gravityMaxCount = MAX_B562_NUM;
     }
 
     public FocusCalculator2(){
@@ -35,6 +36,8 @@ public class FocusCalculator2 extends FocusCalculator1{
     @Override
     public void reset(){
         //pointList.clear();
+        pointAuxList.clear();
+        pointTimeAuxList.clear();
         counterNoB562 = 0;
         counterNoFixed = 0;
         counterFixedResult = 0;
@@ -47,10 +50,28 @@ public class FocusCalculator2 extends FocusCalculator1{
     @Override
     public void addXyz(double[] xyz){
         LocalDateTime now = LocalDateTime.now();
+
+        if((int)xyz[3] == UBXUtil.NO_B562) counterNoB562++;
+        else if((int)xyz[3] == UBXUtil.NO_FIX_RESULT) counterNoFixed++;
+        else {
+            counterFixedResult ++;
+            if (filter(xyz)) {
+                pointAuxList.add(xyz);
+                pointTimeAuxList.add(now);
+            }
+            if (pointAuxList.size() > gravityMaxCount) {
+                pointAuxList.remove(0);
+                pointTimeAuxList.remove(0);
+            }
+        }
+    }
+    /*public void addXyz(double[] xyz){
+        LocalDateTime now = LocalDateTime.now();
         Iterator<LocalDateTime> iterPointTime = pointTimeList.iterator();
         Iterator<double[]> 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();
@@ -73,11 +94,32 @@ public class FocusCalculator2 extends FocusCalculator1{
                 pointTimeList.remove(0);
             }
         }
-    }
+    }*/
 
     public double[] resultB562(double[] lastResult){
-        if(counterFixedResult<30) return null;
+        if(counterFixedResult<50) return null;
-        else return super.resultB562(lastResult);
+        // 把本轮的固定解添加到pointList一起解算
+        int totalNum = pointList.size()+pointAuxList.size();
+        LocalDateTime latestTime = pointTimeAuxList.get(pointTimeAuxList.size()-1);
+        Iterator<LocalDateTime> iterPointTime = pointTimeList.iterator();
+        Iterator<double[]> 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;
+            }
+        }
+        pointList.addAll(pointAuxList);
+        pointTimeList.addAll(pointTimeAuxList);
+        logger.debug("point list: {},{},{},{}",pointTimeList.get(0),pointTimeAuxList.get(pointTimeAuxList.size()-1),pointTimeList.size(),pointList.size());
+        // 求pointList的重心
+        return super.resultB562(lastResult);
     }
 
     @Override

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

@@ -75,14 +75,14 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
 
         //todo 创建FocusCalculator对象需获取该测站的杆长度，上一小时的Tilt平均值，上一小时的测站相对坐标融合值ekfResult
         FocusCalculator focusCalculator;
-        /*if(groupCalc!=null && groupCalc.getVer() == 2){
+        if(groupCalc!=null && groupCalc.getVer() == 2){
             focusCalculator = calculatorMap.computeIfAbsent(deviceId,
                     s -> new FocusCalculator2(200,tiltMap.get(deviceId),positionMap.get(deviceId)));
         }
-        else {*/
+        else {
             focusCalculator = calculatorMap.computeIfAbsent(deviceId,
                     s -> new FocusCalculator1(200, tiltMap.get(deviceId),positionMap.get(deviceId)));
-        //}
+        }
 
         // 读取惯导
         Tilt tilt = message.getTilt();

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

@@ -83,6 +83,7 @@ CREATE TABLE IF NOT EXISTS `gnssgroupcalc` (
     `auto_filter` bit(1) DEFAULT 0,
     `filter_min_hour` int DEFAULT NULL COMMENT '最小平滑窗口',
     `auto_threshold` float DEFAULT NULL,
+    `ver` smallint DEFAULT 1 COMMENT '算法版本',
     `device_num` int DEFAULT 0,
     `auto_upload` bit(1) DEFAULT 0,
     PRIMARY KEY (`id`)
@@ -143,6 +144,9 @@ CREATE TABLE IF NOT EXISTS `gnssdevicelocationrecords` (
     `r9250e` double DEFAULT NULL,
     `r9250n` double DEFAULT NULL,
     `r9250d` double DEFAULT NULL,
+    /*`m9250e` double DEFAULT NULL,
+    `m9250n` double DEFAULT NULL,
+    `m9250d` double DEFAULT NULL,*/
     `rpose` double DEFAULT NULL COMMENT '相对位置东E',
     `rposn` double DEFAULT NULL COMMENT '相对位置北N',
     `rposd` double DEFAULT NULL COMMENT '相对位置天D',

sec-beidou/src/main/resources/templates/page/gnss_group_cfg.html

@@ -143,6 +143,7 @@
 		 **/
 		var cfg2_cols = [
 			{field: 'id', title: '组号', sort: true},
+			{field: 'ver', title: '算法版本'},
 			{field: 'filter_hour', title: '滤波周期(小时)'},
 			{field: 'xy_threshold', title: '水平异常门限(mm)'},
 			{field: 'z_threshold', title: '垂直异常门限(mm)'},

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

@@ -23,6 +23,15 @@
                 <input type="number" name="id" id="id" lay-verify="required" lay-reqtext="不能为空" placeholder="请输入组号" value="" class="layui-input">
             </div>
         </div>
+        <div class="layui-form-item">
+            <label class="layui-form-label">算法版本</label>
+            <div class="layui-input-inline">
+                <select name="ver" id="ver" lay-filter="ver">
+                    <option value="1">1</option>
+                    <option value="2">2</option>
+                </select>
+            </div>
+        </div>
         <div class="layui-form-item">
             <label class="layui-form-label required">滤波周期(小时)</label>
             <div class="layui-input-block">
@@ -133,6 +142,7 @@
         $('#filter_min_hour').val(data.filter_min_hour);
         $('#auto_threshold').val(data.auto_threshold);
         $('#auto_upload').val(data.auto_upload?'1':'0');
+        $('#ver').val(data.ver);
         form.render();
     }
 </script>

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

@@ -2,11 +2,15 @@ package com.imdroid.beidou.test_device.task;
 
 import com.imdroid.beidou.test_device.service.UDPClient;
 import com.imdroid.common.util.ByteUtil;
+import io.netty.buffer.ByteBuf;
+import io.netty.buffer.Unpooled;
 
 import java.io.BufferedReader;
 import java.io.FileReader;
 import java.io.IOException;
 import java.text.SimpleDateFormat;
+import java.time.LocalDateTime;
+import java.time.format.DateTimeFormatter;
 import java.util.Date;
 
 public class BeidouDevice {
@@ -22,23 +26,24 @@ public class BeidouDevice {
 
     public void run(){
         try{
+            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");
+            executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_4xx.log");
+            executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_5xx.log");
+            executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_6xx.log");
+
+            executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_1.log");
+            executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_2.log");
+            executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_1xx.log");
+            executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_2xx.log");
+            executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_3xx.log");
+            executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_4xx.log");
+            executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_5xx.log");
+            executeAsD342("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_6xx.log");
+
             //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412270_1xx.log");
             //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412270_2xx.log");
-            //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_1xx.log");
-            //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_2xx.log");
-            //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_3xx.log");
-            //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_4xx.log");
-            //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_5xx.log");
-            //execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_6xx.log");
-
-            execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_1.log");
-            execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_2.log");
-            execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_1xx.log");
-            execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_2xx.log");
-            execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_3xx.log");
-            execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_4xx.log");
-            execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_5xx.log");
-            execute("C:\\Users\\wd\\Desktop\\log\\b562_2412254_0416_6xx.log");
         }
         catch (Exception e){
 
@@ -66,9 +71,9 @@ public class BeidouDevice {
                 Date time = sdf.parse(arrs[0] + " " + arrs[1]);
 
                 if (lastTime!=0 && Math.abs(time.getTime() - lastTime) > 60 * 1000) {//超过1分钟为一个周期
+                    System.out.println(time+" cycle "+cycleCount+", b562 num "+b562Count);
                     Thread.sleep(30 * 1000);
                     cycleCount++;
-                    System.out.println(time+" cycle "+cycleCount+", b562 num "+b562Count);
                 }
                 udpClient.sendData(ByteUtil.hexStringTobyte(arrs[2]));
                 b562Count++;
@@ -84,4 +89,62 @@ public class BeidouDevice {
             if(fr!=null) fr.close();
         }
     }
+
+    public void executeAsD342(String dataFileName) throws IOException {
+        // d3 51 length device_id(4bytes) seq(2bytes) year mon day hour minute
+        FileReader fr=null;
+        BufferedReader br=null;
+        byte[] d342Head = new byte[]{(byte) 0xd3,(byte) 0x42};
+        ByteBuf dataBuffer = Unpooled.buffer();
+        ByteBuf sendBuffer = Unpooled.buffer();
+        try {
+            fr = new FileReader(dataFileName);
+            br = new BufferedReader(fr);
+            String line = "";
+            String[] arrs = null;
+
+            DateTimeFormatter formatter = DateTimeFormatter.ofPattern("yyyy-MM-dd HH:mm:ss.SSS");
+
+            LocalDateTime lastTime=null;
+            int b562Count = 0;
+
+            while ((line = br.readLine()) != null) {
+                arrs = line.split(" ");
+                if(arrs.length != 3) continue;
+                LocalDateTime time = LocalDateTime.parse(arrs[0] + " " + arrs[1],formatter);
+
+                dataBuffer.writeBytes(ByteUtil.hexStringTobyte(arrs[2]));
+                b562Count++;
+
+                if((b562Count%10)==0 ||
+                        (lastTime!=null && time.isAfter(lastTime.plusMinutes(1)))) {
+                    sendBuffer.writeBytes(d342Head);
+                    sendBuffer.writeShort(dataBuffer.readableBytes()+11);
+                    String deviceId = arrs[2].substring(4*2,8*2);
+                    sendBuffer.writeBytes(ByteUtil.hexStringTobyte(deviceId));
+                    sendBuffer.writeShort(b562Count);
+                    byte[] byteTime = new byte[]{(byte) (time.getYear() - 2000), (byte) time.getMonthValue(), (byte) time.getDayOfMonth(), (byte) time.getHour(), (byte) time.getMinute()};
+                    sendBuffer.writeBytes(byteTime);
+                    sendBuffer.writeBytes(dataBuffer);
+                    byte[] data = new byte[sendBuffer.readableBytes()];
+                    sendBuffer.getBytes(0,data);
+                    udpClient.sendData(data);
+                    Thread.sleep(100);
+                    sendBuffer.clear();
+                    dataBuffer.clear();
+                }
+                if ((b562Count%100)==0) {
+                    System.out.println(time+" b562 num "+b562Count);
+
+                }
+            }
+            br.close();
+            fr.close();
+        }
+        catch (Exception e){
+            e.printStackTrace();
+            if(br!=null) br.close();
+            if(fr!=null) fr.close();
+        }
+    }
 }