From b73ad2c5202bf4bb83f7fb252f393e0297f65aae Mon Sep 17 00:00:00 2001 From: weidong Date: Wed, 22 Nov 2023 09:49:00 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0b562=E5=9B=BA=E5=AE=9A?= =?UTF-8?q?=E8=A7=A3=E3=80=81=E9=9D=9E=E5=9B=BA=E5=AE=9A=E8=A7=A3=E7=9A=84?= =?UTF-8?q?=E7=BB=9F=E8=AE=A1=EF=BC=8C=E4=BB=A5=E5=8F=8A=E5=9B=BA=E5=AE=9A?= =?UTF-8?q?=E8=A7=A3=E5=B0=91=E4=BA=8E=E9=97=A8=E9=99=90=E6=97=B6=E4=BA=A7?= =?UTF-8?q?=E7=94=9F=E5=91=8A=E8=AD=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../imdroid/sideslope/bd/FocusCalculator.java | 8 ++-- .../sideslope/bd/FocusCalculator1.java | 41 +++++++++++-------- .../sideslope/bd/FocusCalculator2.java | 32 +++++++++------ .../com/imdroid/sideslope/bd/UBXUtil.java | 11 +++-- .../calc/SingleLineGNSSCalcService.java | 24 ++++------- .../sideslope/service/WarningService.java | 15 ++++--- .../sideslope/service/WarningServiceImpl.java | 31 +++++++++----- .../src/test/java/FocusCalculator3.java | 15 ++++--- .../src/test/java/FocusCalculatorTest.java | 4 +- 9 files changed, 102 insertions(+), 79 deletions(-) diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator.java index 80a57163..14a060fd 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator.java +++ b/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(); - public void addXyz(double[] xyz); - public int getB562Num(); - public double[] resultB562(double[] last); + void reset(); + void addXyz(double[] xyz); + int[] getB562Stat(); + double[] resultB562(double[] last); } diff --git 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 index 643c797d..95cf66c2 100644 --- 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 @@ -12,19 +12,23 @@ import java.util.*; */ public class FocusCalculator1 implements FocusCalculator{ - private List list = Collections.synchronizedList(new ArrayList<>()); - private List tilts = Collections.synchronizedList(new ArrayList<>()); + private final List list = Collections.synchronizedList(new ArrayList<>()); + private final List tilts = Collections.synchronizedList(new ArrayList<>()); private Tilt tilt0;//初始状态 private double[] position0; private double r = 10;//杆的长度cm 20230718 测试改为10 //private boolean flag = false; // 是否是第一次计算 - private List pointList = new ArrayList<>(); + private final List 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); - pointList.removeAll(pointList); + list.clear(); + pointList.clear(); } /** @@ -56,17 +60,22 @@ public class FocusCalculator1 implements FocusCalculator{ */ @Override public void addXyz(double[] xyz){ - if(filter(xyz)){ - list.add(xyz); - } - if(list.size() > 300){ - list.remove(0); + if(xyz[3] == UBXUtil.NO_B562) counterNoB562++; + else if(xyz[3] == UBXUtil.NO_FIX_RESULT) counterNoFixed++; + else { + counterFixedResult ++; + if (filter(xyz)) { + list.add(xyz); + } + if (list.size() > 300) { + list.remove(0); + } } } @Override - public int getB562Num(){ - return list.size(); + public int[] getB562Stat(){ + 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; + return focus(list); } 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 _9250_result; + return TiltUtil.toPosition(avg,tilt0,position0,r); } public Tilt getTilt0(){ diff --git 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 index 250b4f4e..6f9c0488 100644 --- 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 @@ -12,15 +12,18 @@ import java.util.*; */ public class FocusCalculator2 implements FocusCalculator{ - private List list = Collections.synchronizedList(new ArrayList<>()); + private final List list = Collections.synchronizedList(new ArrayList<>()); - private List pointList = new ArrayList<>(); + private final List pointList = new ArrayList<>(); + private int counterNoB562 = 0; + private int counterNoFixed = 0; + private int counterFixedResult = 0; @Override public void reset(){ - list.removeAll(list); - pointList.removeAll(pointList); + list.clear(); + pointList.clear(); } /** @@ -29,11 +32,16 @@ public class FocusCalculator2 implements FocusCalculator{ */ @Override public void addXyz(double[] xyz){ - if(filter(xyz)){ - list.add(xyz); - } - if(list.size() > 300){ - list.remove(0); + if(xyz[3] == UBXUtil.NO_B562) counterNoB562++; + else if(xyz[3] == UBXUtil.NO_FIX_RESULT) counterNoFixed++; + else { + counterFixedResult ++; + 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(){ - return list.size(); + public int[] getB562Stat(){ + return new int[]{counterFixedResult,counterNoFixed,counterNoB562}; } @@ -108,7 +116,7 @@ public class FocusCalculator2 implements FocusCalculator{ List 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]; } } diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/UBXUtil.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/UBXUtil.java index a63c4de6..6a42072f 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/UBXUtil.java +++ b/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}; } } } 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 75801577..b87e7824 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 @@ -47,10 +47,8 @@ public class SingleLineGNSSCalcService implements GNSSCalcService { @Override public double[] calcSingle(D341LocationMessage message) { String deviceId = message.getId(); - Device device = deviceService.findByDeviceId(deviceId); - if (device != null && device.getDeviceType() == Device.DEVICE_ROVER) { - resultOutputTimer(deviceId); - } + resultOutputTimer(deviceId, message.getTenantId()); + //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); - if (logger.isDebugEnabled()) { - logger.debug("测站" + deviceId + "的b562单次解析结果:{}", Arrays.toString(doubles)); - } - return doubles; - }else{ - return new double[0]; + focusCalculator.addXyz(doubles); + if (logger.isDebugEnabled()) { + logger.debug("测站" + deviceId + "的b562单次解析结果:{}", Arrays.toString(doubles)); } + 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; diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/service/WarningService.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/service/WarningService.java index 1298ba9f..7fa60aac 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/service/WarningService.java +++ b/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 - */ - void checkB562Num(int b562Num); - /*** - * 检查电压、RSSI等 - * @param msg + * @param */ + void checkB562Num(String deviceId,Integer tenantId, int[] numB562Stat); + + /*** + * 检查电压、RSSI等 + * @param msg + */ void checkDeviceStatus(GnssStatusMsg msg, GnssStatus curStatus); /*** * 检查未知报文是否较多 diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/service/WarningServiceImpl.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/service/WarningServiceImpl.java index bcaa46be..d8a38852 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/service/WarningServiceImpl.java +++ b/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 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); diff --git a/sec-beidou-rtcm/src/test/java/FocusCalculator3.java b/sec-beidou-rtcm/src/test/java/FocusCalculator3.java index 5a5a573e..5fb087f1 100644 --- a/sec-beidou-rtcm/src/test/java/FocusCalculator3.java +++ b/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 list = Collections.synchronizedList(new ArrayList<>()); + private final List list = Collections.synchronizedList(new ArrayList<>()); - private List listE = Collections.synchronizedList(new ArrayList<>()); + private final List listE = Collections.synchronizedList(new ArrayList<>()); - private List listN = Collections.synchronizedList(new ArrayList<>()); + private final List listN = Collections.synchronizedList(new ArrayList<>()); - private List listD = Collections.synchronizedList(new ArrayList<>()); + private final List listD = Collections.synchronizedList(new ArrayList<>()); - private List pointList = new ArrayList<>(); + private final List pointList = new ArrayList<>(); @Override @@ -53,8 +52,8 @@ public class FocusCalculator3 implements FocusCalculator { } @Override - public int getB562Num(){ - return list.size(); + public int[] getB562Stat(){ + return new int[]{0,0,0}; } @Override diff --git a/sec-beidou-rtcm/src/test/java/FocusCalculatorTest.java b/sec-beidou-rtcm/src/test/java/FocusCalculatorTest.java index dd917147..e155883e 100644 --- a/sec-beidou-rtcm/src/test/java/FocusCalculatorTest.java +++ b/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);