增加周跳判断和处理

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

@@ -27,4 +27,5 @@ public interface FocusCalculator {
 
     // 算法版本
     int getVer();
+    boolean isJump();
 }

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

@@ -349,6 +349,11 @@ public class FocusCalculator1 implements FocusCalculator{
         return 1;
     }
 
+    @Override
+    public boolean isJump() {
+        return false;
+    }
+
     @Override
     public Tilt avgTilt() {
         if (tilts.size() == 0) {

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

@@ -210,6 +210,11 @@ public class FocusCalculator5 implements FocusCalculator{
         return 5;
     }
 
+    @Override
+    public boolean isJump() {
+        return false;
+    }
+
     @Override
     public Tilt avgTilt() {
         if (tilts.size() == 0) {

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

@@ -215,6 +215,11 @@ public class FocusCalculator6 implements FocusCalculator{
         return 6;
     }
 
+    @Override
+    public boolean isJump() {
+        return false;
+    }
+
     @Override
     public Tilt avgTilt() {
         if (tilts.size() == 0) {

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

@@ -9,6 +9,10 @@ import com.imdroid.sideslope.sal.Device;
 public class FocusCalculator7 extends FocusCalculator3{
 
     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;
+
+    int bad_count = 0;
 
     public FocusCalculator7(Device bsDevice) {
         super(bsDevice);
@@ -22,15 +26,40 @@ public class FocusCalculator7 extends FocusCalculator3{
 
         if(gga.isFixed()) {
             counterFixedResult++;
+            if(pointList.size()>0){
+                double[] lastXyz = pointList.get(pointList.size()-1);
+                if(Math.abs(xyz[0]-lastXyz[0])>bad_change_mm ||
+                        Math.abs(xyz[1]-lastXyz[1])>bad_change_mm ||
+                        Math.abs(xyz[2]-lastXyz[2])>bad_change_mm){
+                    bad_count++;
+                    return;
+                }
+            }
+            bad_count = 0;
+
             pointList.add(xyz);
         }
         else if(gga.getQuality() == 5) counterNoFixed++;
         else counterNoB562 ++;
     }
 
+    @Override
+    public void reset(){
+        super.reset();
+        bad_count = 0;
+    }
+
     @Override
     public int getVer() {
         return 7;
     }
 
+    public boolean isJump(){
+        if (bad_count>bad_duration){
+            bad_count = 0;
+            return true;
+        }
+        else return false;
+    }
+
 }

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

@@ -1,6 +1,7 @@
 package com.imdroid.sideslope.calc;
 
 import com.baomidou.mybatisplus.core.conditions.query.QueryWrapper;
+import com.imdroid.common.util.DataTypeUtil;
 import com.imdroid.common.util.ThreadManager;
 import com.imdroid.secapi.dto.GnssCalcData;
 import com.imdroid.secapi.dto.GnssCalcDataMapper;
@@ -10,7 +11,11 @@ import com.imdroid.sideslope.bd.*;
 import com.imdroid.sideslope.message.D341LocationMessage;
 import com.imdroid.sideslope.sal.Device;
 import com.imdroid.sideslope.sal.DeviceService;
+import com.imdroid.sideslope.server.DeviceChannel;
+import com.imdroid.sideslope.server.OnlineChannels;
 import com.imdroid.sideslope.service.WarningService;
+import io.netty.buffer.ByteBuf;
+import io.netty.buffer.Unpooled;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
 import org.springframework.beans.factory.annotation.Autowired;
@@ -119,7 +124,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
         if(tilt != null) {
             focusCalculator.addTilt(tilt);
             if (logger.isDebugEnabled()) {
-                logger.debug("测站" + deviceId + "惯导单次解析结果:{}", tilt);
+                logger.debug("测站{}惯导单次解析结果:{}", deviceId,tilt);
             }
         }
 
@@ -130,15 +135,19 @@ 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.info("测站{}的b562单次解析结果:{}", deviceId,Arrays.toString(doubles));
         }
 
         // 单次GGA
         Gga gga = message.getGga();
         if(gga != null) {
-            focusCalculator.addGGA(message.getGga());
+            focusCalculator.addGGA(gga);
-            logger.info("测站" + deviceId + "的gga单次解析结果:{},{},{},{}",
+            logger.info("测站{}的gga单次解析结果:{},{},{},{}",deviceId,
                         gga.getLongitude(), gga.getLatitude(), gga.getAltitude(), gga.getQuality());
+            if(groupCalc.getVer() == 7 && focusCalculator.isJump()){
+                logger.info("{}发生周跳",deviceId);
+                hardResetDevice(deviceId);
+            }
         }
     }
 
@@ -350,4 +359,21 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
         }
         return null;
     }
+
+    void hardResetDevice(String deviceId){
+        String resetCmd = "42 4b cc 17 26 04 00 00";
+        DeviceChannel deviceChannel = OnlineChannels.INSTANCE.getConfigChannel(deviceId);
+        if(deviceChannel == null || !deviceChannel.isOnline()) {
+            deviceChannel = OnlineChannels.INSTANCE.getDataChannel(deviceId);
+        }
+        //if(deviceChannel!=null && deviceChannel.isOnline()){
+        if(deviceChannel!=null && deviceChannel.isOnline()){
+            // send command
+            ByteBuf buf = Unpooled.buffer();
+            byte[] data = DataTypeUtil.hexStringToBytes(resetCmd);
+            logger.info("send hard reset command:{}", resetCmd);
+            buf.writeBytes(data);
+            deviceChannel.writeAndFlush(buf);
+        }
+    }
 }