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

2024-04-18 23:09:55 +08:00 · 2024-04-18 23:09:55 +08:00 · 2f8d570d19
commit 2f8d570d19
parent 40ae325fe6
8 changed files with 146 additions and 25 deletions
--- a/sec-api/src/main/java/com/imdroid/secapi/dto/GnssGroupCalc.java
+++ b/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;
 }
--- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator1.java
+++ b/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<>());
--- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator2.java
+++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator2.java
@ -15,8 +15,10 @@ 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数据融合
     * @param height 杆的长度，单位厘米
     * @param tilt0 一个小时前的tilt平均值，用一小时的avgTilt计算得到，如果第一次，用null
@ -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;
-        else return super.resultB562(lastResult);
+        if(counterFixedResult<50) return null;
+        // 把本轮的固定解添加到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
--- 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
@ -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();
--- a/sec-beidou/src/main/resources/db/schema.sql
+++ b/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',
--- a/sec-beidou/src/main/resources/templates/page/gnss_group_cfg.html
+++ b/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)'},
--- a/sec-beidou/src/main/resources/templates/page/table/gnss_add_group_calc.html
+++ b/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>
--- a/sec-test-device/src/main/java/com/imdroid/beidou/test_device/task/BeidouDevice.java
+++ b/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();
+        }
+    }
 }