diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSCalcFilterService.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSCalcFilterService.java index 5d67de19..52a1e2f2 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSCalcFilterService.java +++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSCalcFilterService.java @@ -64,20 +64,23 @@ public class GNSSCalcFilterService { public boolean isGoodGnssData(GnssCalcData newRecord, double[] referPos, List gnssHistoryRecords, float xyThreshold, float zThreshold, boolean isAdvFilter) { boolean isGood = true; - if(referPos == null) { - logger.info("bad point"); - return false; - } + // 检查是不是坏点 GnssCalcData record0 = null; if(gnssHistoryRecords.size()>0) record0 = gnssHistoryRecords.get(0); if(record0 != null && Math.abs(record0.getB562e() - newRecord.getB562e())<0.001) { - isGood = false; + return false; } - else if (Math.abs(newRecord.getB562e() - referPos[0]) > xyThreshold || + if(referPos==null && record0!=null) { + referPos = new double[]{record0.getB562e(), record0.getB562n(), record0.getB562d()}; + } + if(referPos==null) return false; + + if (Math.abs(newRecord.getB562e() - referPos[0]) > xyThreshold || Math.abs(newRecord.getB562n() - referPos[1]) > xyThreshold || Math.abs(newRecord.getB562d() - referPos[2]) > zThreshold) { - logger.info("{},{},{} bad point",newRecord.getB562e()- referPos[0],newRecord.getB562n()- referPos[1],newRecord.getB562d()- referPos[2]); + logger.debug("{} bad point,delta: {},{},{} ",newRecord.getDeviceid(), + newRecord.getB562e()- referPos[0],newRecord.getB562n()- referPos[1],newRecord.getB562d()- referPos[2]); isGood = false; //记录为坏点,下次不参与滤波 } else if(isAdvFilter){ @@ -87,7 +90,7 @@ public class GNSSCalcFilterService { Math.abs(newRecord.getB562e() - record1.getB562e()) > xyThreshold*2 || Math.abs(newRecord.getB562n() - record1.getB562n()) > xyThreshold*2 || Math.abs(newRecord.getB562d() - record1.getB562d()) > zThreshold*2) { - logger.info("bad point"); + logger.debug("{} adv filter bad point",newRecord.getDeviceid()); isGood = false; //记录为坏点,下次不参与滤波 } } diff --git 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 index b9223069..78551853 100644 --- 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 @@ -210,21 +210,23 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { if(focusCalculator2.isReferPointFiltered()) { if (locationRecord.getEnabled() && locationRecord.getRpose() != null) { focusCalculator2.setReferPoint(new double[]{locationRecord.getRpose(), locationRecord.getRposn(), locationRecord.getRposd()}); - logger.info("set reference pos to filter result:{},{},{}", locationRecord.getRpose(), locationRecord.getRposn(), locationRecord.getRposd()); + logger.info("{} set reference pos to filter result:{},{},{}", + deviceId, locationRecord.getRpose(), locationRecord.getRposn(), locationRecord.getRposd()); } else{ - logger.info("reference pos not changed"); + logger.info("{} reference pos not changed",deviceId); } } else{ // 查询设备有没有配置初始位置 if(device.getIPose()!=null){ focusCalculator2.setInitReferPoint(new double[]{device.getIPose(),device.getIPosn(),device.getIPosd()}); - logger.info("set reference pos to init pos:{},{},{}",device.getIPose(),device.getIPosn(),device.getIPosd()); + logger.info("{} set reference pos to init pos:{},{},{}", + deviceId,device.getIPose(),device.getIPosn(),device.getIPosd()); } else{ focusCalculator.setReferPoint(b562Result); - logger.info("set reference pos to b562 result"); + logger.info("{} set reference pos to b562 result",deviceId); } } }