增加b562固定解、非固定解的统计，以及固定解少于门限时产生告警

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

@@ -1,8 +1,8 @@
 package com.imdroid.sideslope.bd;
 
 public interface FocusCalculator {
-    public void reset();
+    void reset();
-    public void addXyz(double[] xyz);
+    void addXyz(double[] xyz);
-    public int getB562Num();
+    int[] getB562Stat();
-    public double[] resultB562(double[] last);
+    double[] resultB562(double[] last);
 }

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

@@ -12,19 +12,23 @@ import java.util.*;
  */
 public class FocusCalculator1 implements FocusCalculator{
 
-    private List<double[]> list = Collections.synchronizedList(new ArrayList<>());
+    private final List<double[]> list = Collections.synchronizedList(new ArrayList<>());
-    private List<Tilt> tilts = Collections.synchronizedList(new ArrayList<>());
+    private final List<Tilt> tilts = Collections.synchronizedList(new ArrayList<>());
     private Tilt tilt0;//初始状态
     private double[] position0;
     private double r = 10;//杆的长度cm 20230718 测试改为10
     //private boolean flag = false;  // 是否是第一次计算
-    private List<Point> pointList = new ArrayList<>();
+    private final List<Point> pointList = new ArrayList<>();
     //public static double[] lastFocus = null;//
     private int delay_ms = 0;
     private int counter = 0;
     private boolean isShock = false;
 
-    private double shockThreshold = 1.5;
+    private final double shockThreshold = 1.5;
+
+    private int counterNoB562 = 0;
+    private int counterNoFixed = 0;
+    private int counterFixedResult = 0;
 
 
     /**
@@ -46,8 +50,8 @@ public class FocusCalculator1 implements FocusCalculator{
 
     @Override
     public void reset(){
-        list.removeAll(list);
+        list.clear();
-        pointList.removeAll(pointList);
+        pointList.clear();
     }
 
     /**
@@ -56,17 +60,22 @@ public class FocusCalculator1 implements FocusCalculator{
      */
     @Override
     public void addXyz(double[] xyz){
-        if(filter(xyz)){
+        if(xyz[3] == UBXUtil.NO_B562) counterNoB562++;
-            list.add(xyz);
+        else if(xyz[3] == UBXUtil.NO_FIX_RESULT) counterNoFixed++;
-        }
+        else {
-        if(list.size() > 300){
+            counterFixedResult ++;
-            list.remove(0);
+            if (filter(xyz)) {
+                list.add(xyz);
+            }
+            if (list.size() > 300) {
+                list.remove(0);
+            }
         }
     }
 
     @Override
-    public int getB562Num(){
+    public int[] getB562Stat(){
-        return list.size();
+        return new int[]{counterFixedResult,counterNoFixed,counterNoB562};
     }
 
 
@@ -147,8 +156,7 @@ public class FocusCalculator1 implements FocusCalculator{
     private double[] resultB562Sub(){
         if(list.size() > 0){
             //重心
-            double[] focus = focus(list);
+            return focus(list);
-            return focus;
         }
         return null;
     }
@@ -338,8 +346,7 @@ public class FocusCalculator1 implements FocusCalculator{
         if(tilt0 == null || position0 == null || r == 0 || avg == null){
             return null;
         }
-        double[] _9250_result = TiltUtil.toPosition(avg,tilt0,position0,r);
+        return TiltUtil.toPosition(avg,tilt0,position0,r);
-        return _9250_result;
     }
 
     public Tilt getTilt0(){

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

@@ -12,15 +12,18 @@ import java.util.*;
  */
 public class FocusCalculator2 implements FocusCalculator{
 
-    private List<double[]> list = Collections.synchronizedList(new ArrayList<>());
+    private final List<double[]> list = Collections.synchronizedList(new ArrayList<>());
 
-    private List<Point> pointList = new ArrayList<>();
+    private final List<Point> pointList = new ArrayList<>();
+    private int counterNoB562 = 0;
+    private int counterNoFixed = 0;
+    private int counterFixedResult = 0;
 
 
     @Override
     public void reset(){
-        list.removeAll(list);
+        list.clear();
-        pointList.removeAll(pointList);
+        pointList.clear();
     }
 
     /**
@@ -29,11 +32,16 @@ public class FocusCalculator2 implements FocusCalculator{
      */
     @Override
     public void addXyz(double[] xyz){
-        if(filter(xyz)){
+        if(xyz[3] == UBXUtil.NO_B562) counterNoB562++;
-            list.add(xyz);
+        else if(xyz[3] == UBXUtil.NO_FIX_RESULT) counterNoFixed++;
-        }
+        else {
-        if(list.size() > 300){
+            counterFixedResult ++;
-            list.remove(0);
+            if (filter(xyz)) {
+                list.add(xyz);
+            }
+            if (list.size() > 300) {
+                list.remove(0);
+            }
         }
     }
 
@@ -46,8 +54,8 @@ public class FocusCalculator2 implements FocusCalculator{
     }
 
     @Override
-    public int getB562Num(){
+    public int[] getB562Stat(){
-        return list.size();
+        return new int[]{counterFixedResult,counterNoFixed,counterNoB562};
     }
 
 
@@ -108,7 +116,7 @@ public class FocusCalculator2 implements FocusCalculator{
         List<Double> list2 = new ArrayList<>();
         for(double[] point: list){
             if(Math.abs(point[index]-g)<=r){
-                list2.add(new Double(point[index]));
+                list2.add(point[index]);
                 newG += point[index];
             }
         }

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

@@ -13,6 +13,11 @@ public class UBXUtil {
      * @param data UBX数据(b562开头)
      * @return x,y,z      东，北，竖直   东为正数，北为正数，朝上为正数
      */
+
+    public static int FIX_RESULT = 0;
+    public static int NO_FIX_RESULT = 1;
+    public static int NO_B562 = 2;
+
     public static double[] getLocation(ByteBuf data){
         boolean has_b562 = false;
         byte[] b562_flag = {(byte) 0xb5, (byte)0x62};
@@ -26,7 +31,7 @@ public class UBXUtil {
                 break;
             }
         }
-        if(!has_b562) return null;
+        if(!has_b562) return new double[]{0,0,0,NO_B562};
         pos+=6;
 
         double relPosN = data.getIntLE(pos+8) + data.getByte(pos+32)*0.01;
@@ -35,9 +40,9 @@ public class UBXUtil {
         relPosD = -relPosD;
         short status = (short) ((data.getUnsignedByte(pos+60) & 0x18)>>3); //mask: 00011000
         if(status == 2){
-            return new double[]{relPosE,relPosN,relPosD};
+            return new double[]{relPosE,relPosN,relPosD,FIX_RESULT};
         }else{
-            return null;
+            return new double[]{0,0,0,NO_FIX_RESULT};
         }
     }
 }

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

@@ -47,10 +47,8 @@ public class SingleLineGNSSCalcService implements GNSSCalcService {
     @Override
     public double[] calcSingle(D341LocationMessage message) {
         String deviceId = message.getId();
-        Device device = deviceService.findByDeviceId(deviceId);
+        resultOutputTimer(deviceId, message.getTenantId());
-        if (device != null && device.getDeviceType() == Device.DEVICE_ROVER) {
+
-            resultOutputTimer(deviceId);
-        }
         //todo 创建FocusCalculator对象需获取该测站的杆长度，上一小时的Tilt平均值，上一小时的测站相对坐标融合值ekfResult
         FocusCalculator1 focusCalculator = calculatorMap.computeIfAbsent(deviceId, s -> new FocusCalculator1(150,tiltMap.get(deviceId),
                 positionMap.get(deviceId)));
@@ -66,20 +64,15 @@ public class SingleLineGNSSCalcService implements GNSSCalcService {
         focusCalculator.addDelayMs(message.getPps());
 
         //计算到单次相对位置xyz并记录
-        //tilt共16个字节，从后边开始找b562
         double[] doubles = message.getB562_loc();
-        if(doubles != null){
+        focusCalculator.addXyz(doubles);
-            focusCalculator.addXyz(doubles);
+        if (logger.isDebugEnabled()) {
-            if (logger.isDebugEnabled()) {
+            logger.debug("测站" + deviceId + "的b562单次解析结果:{}", Arrays.toString(doubles));
-                logger.debug("测站" + deviceId + "的b562单次解析结果:{}", Arrays.toString(doubles));
-            }
-            return doubles;
-        }else{
-            return new double[0];
         }
+        return doubles;
     }
 
-    private void resultOutputTimer(String deviceId){
+    private void resultOutputTimer(String deviceId, Integer tenantId){
         //20秒没数据后输出结果
         ScheduledFuture<?> future = timerMap.get(deviceId);
         if (future != null && !future.isDone()) {
@@ -91,7 +84,8 @@ public class SingleLineGNSSCalcService implements GNSSCalcService {
             try {
                 FocusCalculator1 focusCalculator = calculatorMap.get(deviceId);
                 // 1.检查b562有效数，如果过少，产生告警
-                warningService.checkB562Num(focusCalculator.getB562Num());
+                warningService.checkB562Num(deviceId, tenantId,
+                        focusCalculator.getB562Stat());
 
                 // 2.数据处理：参考上次的位置，计算b562重心
                 double[] lastEfk = null;

sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/service/WarningService.java

@@ -3,20 +3,19 @@ package com.imdroid.sideslope.service;
 import com.imdroid.secapi.dto.GnssStatus;
 import com.imdroid.secapi.dto.GnssStatusMsg;
 import com.imdroid.secapi.dto.GnssTrxMsg;
-import com.imdroid.sideslope.message.D3F0SelfCheckMessage;
-import com.imdroid.sideslope.message.D3F2StopIndicationMessage;
 
 public interface WarningService {
     /***
      * 检查一个周期内的b562数量是否足够
      * 返回告警级别和告警码
-     * @param b562Num
+     * @param
-     */
-    void checkB562Num(int b562Num);
-    /***
-     * 检查电压、RSSI等
-     * @param msg
      */
+    void checkB562Num(String deviceId,Integer tenantId, int[] numB562Stat);
+
+        /***
+         * 检查电压、RSSI等
+         * @param msg
+         */
     void checkDeviceStatus(GnssStatusMsg msg, GnssStatus curStatus);
     /***
      * 检查未知报文是否较多

sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/service/WarningServiceImpl.java

@@ -2,8 +2,6 @@ package com.imdroid.sideslope.service;
 
 import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
 import com.imdroid.secapi.dto.*;
-import com.imdroid.sideslope.message.D3F0SelfCheckMessage;
-import com.imdroid.sideslope.message.D3F2StopIndicationMessage;
 import org.springframework.beans.factory.annotation.Autowired;
 import org.springframework.stereotype.Service;
 
@@ -18,7 +16,6 @@ public class WarningServiceImpl implements WarningService {
     WarningCfgMapper warningCfgMapper;
     @Autowired
     WarningMsgMapper warningMsgMapper;
-    private GnssStatusMapper statusMapper;
     // warning type <-> level & value
     Map<Integer, int[]> cfgMap = new HashMap<>();
     int warningLevel2Code = 0;
@@ -36,11 +33,25 @@ public class WarningServiceImpl implements WarningService {
     }
     /***
      * 检查一个周期内的b562数量是否足够
-     * @param b562Num
+     * @param
      */
     @Override
-    public void checkB562Num(int b562Num){
+    public void checkB562Num(String deviceId, Integer tenantId,
-
+                             int[] numB562Stat){
+        if(cfgMap.size() == 0) refreshCfg();
+        int[] threshold = cfgMap.get(WarningCfg.TYPE_LESS_B562);
+        if(threshold != null && (numB562Stat[0] < threshold[1])) {
+            // 检测电压和RSSI
+            WarningMsg warningMsg = new WarningMsg();
+            warningMsg.setDeviceid(deviceId);
+            warningMsg.setTenantid(tenantId);
+            warningMsg.setCreatetime(new Date());
+            warningMsg.setDevicetype(WarningCfg.TYPE_GNSS);
+            warningMsg.setCode(WarningCfg.TYPE_LESS_B562);
+            warningMsg.setLevel((short) threshold[0]);
+            warningMsg.setInfo("B562过少，固定解:" + numB562Stat[0] + "，非固定解:"+numB562Stat[1]+"，无B562:"+numB562Stat[2]);
+            warningMsgMapper.insert(warningMsg);
+        }
     }
     /***
      * 检查电压、RSSI等
@@ -60,10 +71,10 @@ public class WarningServiceImpl implements WarningService {
         warningMsg.setCreatetime(new Date());
         warningMsg.setDevicetype(WarningCfg.TYPE_GNSS);
 
-        int lowVoltage[] = cfgMap.get(WarningCfg.TYPE_LOW_VOLTAGE);
+        int[] lowVoltage = cfgMap.get(WarningCfg.TYPE_LOW_VOLTAGE);
         if(lowVoltage!=null){
             if(statusMsg.getVoltage() <= lowVoltage[1]) {
-                warningMsg.setCode((int) WarningCfg.TYPE_LOW_VOLTAGE);
+                warningMsg.setCode(WarningCfg.TYPE_LOW_VOLTAGE);
                 warningMsg.setLevel((short) lowVoltage[0]);
                 warningMsg.setInfo("低电压：" + statusMsg.getVoltage() + "mV");
                 warningMsgMapper.insert(warningMsg);
@@ -76,10 +87,10 @@ public class WarningServiceImpl implements WarningService {
             }
         }
 
-        int lowRSSI[] = cfgMap.get(WarningCfg.TYPE_LOW_RSSI);
+        int[] lowRSSI = cfgMap.get(WarningCfg.TYPE_LOW_RSSI);
         if(lowRSSI!=null){
             if(statusMsg.getRssi() <= lowRSSI[1]) {
-                warningMsg.setCode((int) WarningCfg.TYPE_LOW_RSSI);
+                warningMsg.setCode( WarningCfg.TYPE_LOW_RSSI);
                 warningMsg.setLevel((short) lowRSSI[0]);
                 warningMsg.setInfo("4G信号弱：" + statusMsg.getRssi());
                 warningMsgMapper.insert(warningMsg);

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

@@ -2,7 +2,6 @@ import com.imdroid.sideslope.bd.FocusCalculator;
 import com.imdroid.sideslope.bd.Point;
 import org.ejml.simple.SimpleMatrix;
 
-import java.io.IOException;
 import java.util.ArrayList;
 import java.util.Collections;
 import java.util.Comparator;
@@ -10,15 +9,15 @@ import java.util.List;
 
 public class FocusCalculator3 implements FocusCalculator {
 
-    private List<double[]> list = Collections.synchronizedList(new ArrayList<>());
+    private final List<double[]> list = Collections.synchronizedList(new ArrayList<>());
 
-    private List<Double> listE = Collections.synchronizedList(new ArrayList<>());
+    private final List<Double> listE = Collections.synchronizedList(new ArrayList<>());
 
-    private List<Double> listN = Collections.synchronizedList(new ArrayList<>());
+    private final List<Double> listN = Collections.synchronizedList(new ArrayList<>());
 
-    private List<Double> listD = Collections.synchronizedList(new ArrayList<>());
+    private final List<Double> listD = Collections.synchronizedList(new ArrayList<>());
 
-    private List<Point> pointList = new ArrayList<>();
+    private final List<Point> pointList = new ArrayList<>();
 
 
     @Override
@@ -53,8 +52,8 @@ public class FocusCalculator3 implements FocusCalculator {
     }
 
     @Override
-    public int getB562Num(){
+    public int[] getB562Stat(){
-        return list.size();
+        return new int[]{0,0,0};
     }
 
     @Override

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

@@ -86,9 +86,9 @@ public class FocusCalculatorTest {
         br.close();
         fr.close();
 
-        if(focusCalculator.getB562Num()>0) {
+        //if(focusCalculator.getB562Num()>0) {
             calcGravity(last, lastTime);
-        }
+        //}
 
         System.out.println("raw results statistic:");
         calcStd(calcResultList);