diff --git a/pom.xml b/pom.xml index 44e4eca6..6f7bb1ab 100644 --- a/pom.xml +++ b/pom.xml @@ -14,6 +14,7 @@ sec-beidou-rtcm sec-api sec-beidou-fwd + sec-test-device diff --git a/sec-api/src/main/java/com/imdroid/secapi/client/RtcmClient.java b/sec-api/src/main/java/com/imdroid/secapi/client/RtcmClient.java index 4246acca..04152067 100644 --- a/sec-api/src/main/java/com/imdroid/secapi/client/RtcmClient.java +++ b/sec-api/src/main/java/com/imdroid/secapi/client/RtcmClient.java @@ -12,6 +12,9 @@ public interface RtcmClient { @PostMapping("/device_param_changed") HttpResp deviceParamChanged(@RequestParam(name = "deviceId") String deviceId); + @PostMapping("/group_param_changed") + HttpResp groupParamChanged(); + @PostMapping("/warning_param_changed") HttpResp warningParamChanged(); } 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 14a060fd..e57de7ba 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 @@ -3,6 +3,18 @@ package com.imdroid.sideslope.bd; public interface FocusCalculator { void reset(); void addXyz(double[] xyz); + void addTilt(Tilt tilt); + void addDelayMs(int ms); int[] getB562Stat(); double[] resultB562(double[] last); + double[] result9250(); + double[] ekfResult(double[] b562Xyz, double[] tiltXyz); + Tilt avgTilt(); + int getAvgDelayMs(); + Tilt getTilt0(); + + double[] getPosition0(); + + double getR(); + boolean isShocked(); } 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 a9512a4a..e1190cb6 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 @@ -1,5 +1,8 @@ package com.imdroid.sideslope.bd; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + import java.util.*; /** @@ -11,34 +14,38 @@ import java.util.*; * 6.。。。。。。 */ public class FocusCalculator1 implements FocusCalculator{ + private final Logger logger = LoggerFactory.getLogger(FocusCalculator1.class); - 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 final List pointList = new ArrayList<>(); - //public static double[] lastFocus = null;// - private int delay_ms = 0; - private int counter = 0; - private boolean isShock = false; - - private final double shockThreshold = 1.5; - - private int counterNoB562 = 0; - private int counterNoFixed = 0; - private int counterFixedResult = 0; + // b562算法相关：b562固定解的点、计算球心初始半径、迭代步长、最少点数 + final List pointList = Collections.synchronizedList(new ArrayList<>()); + int gravityMinCount = 50; + int gravityMaxCount = 300; + double gravityInitR = 0.5; + double pointSelectedRate = 0.5; + double iterStep = 0.1; + // 惯导算法相关 + final List tilts = Collections.synchronizedList(new ArrayList<>()); + Tilt tilt0;//初始状态 + double[] position0; + double height = 200; + boolean isShock = false; + final double shockThreshold = 1.5; + //其他 + int delay_ms = 0; + int delay_counter = 0; + int counterNoB562 = 0; + int counterNoFixed = 0; + int counterFixedResult = 0; /** * 构建计算器，如果tilt0和position0是null，那么计算过程将不考虑与tilts数据融合 - * @param r 杆的长度，单位厘米 + * @param height 杆的长度，单位厘米 * @param tilt0 一个小时前的tilt平均值，用一小时的avgTilt计算得到，如果第一次，用null * @param position0 一个小时前的xyz三维坐标值，用一小时前的ekf计算得到的那个，如果第一次，用null */ - public FocusCalculator1(double r, Tilt tilt0, double[] position0){ - this.r = r; + public FocusCalculator1(double height, Tilt tilt0, double[] position0){ + this.height = height; this.tilt0 = tilt0; this.position0 = position0; this.isShock = false; @@ -53,15 +60,15 @@ public class FocusCalculator1 implements FocusCalculator{ @Override public void reset(){ - list.clear(); pointList.clear(); + tilts.clear(); counterNoB562 = 0; counterNoFixed = 0; counterFixedResult = 0; } /** - * 加入list + * 加入pointList * @param xyz */ @Override @@ -71,10 +78,10 @@ public class FocusCalculator1 implements FocusCalculator{ else { counterFixedResult ++; if (filter(xyz)) { - list.add(xyz); + pointList.add(xyz); } - if (list.size() > 300) { - list.remove(0); + if (pointList.size() > gravityMaxCount) { + pointList.remove(0); } } } @@ -86,70 +93,68 @@ public class FocusCalculator1 implements FocusCalculator{ /** - * 计算得到最终结果 + * 计算本轮固定解的重心： + * 1、如果本轮固定解数少于50个，则直接计算均值 + * 2、如果大于50个： + * a）找出离上次距离最近的50个点，计算初始重心 + * b）以初始重心为球心，初始半径为0.5画球，找出所有在球内的点 + * c）重新计算这个球的重心，半径增加0.1，再次找出所有在球内的点，直到筛选出不少于总数50%的点 + * d）计算这些点的均值 * @return */ @Override - public double[] resultB562(double[] r9250Result){ - int count = 50; - double rate = 0.5; + public double[] resultB562(double[] lastResult){ + double r = gravityInitR;//初始球半径 try { - double r = 0.5;//初始球半径 - if(list.size() >= count){ - List results = new ArrayList<>(); - if (r9250Result != null && r9250Result.length > 0){ + if(pointList.size() >= gravityMinCount){ + // 筛选出离上次解算结果最近的50个点计算初始球心 + // 如果没有上次位置，选最后50个点 + // 为便于排序，使用Point对象 + List selectPoints = new ArrayList<>(); + if (lastResult != null && lastResult.length > 0){ // 计算所有点位与变量之间的距离，并存入集合 - pointList.clear(); - for (double[] point : list) { - Point point1 = new Point(point[0],point[1],point[2]); - point1.setXDistance(disXY(point,r9250Result)); // 设置该点到变量的水平距离 - point1.setZDistance(Math.abs(point[2] - r9250Result[2])); // 设置该点到变量的垂直距离 - results.add(point1); // 将点位加入集合 + for (double[] point : pointList) { + Point pointObj = new Point(point[0],point[1],point[2]); + pointObj.setXDistance(disXY(point,lastResult)); // 设置该点到变量的水平距离 + pointObj.setZDistance(Math.abs(point[2] - lastResult[2])); // 设置该点到变量的垂直距离 + selectPoints.add(pointObj); // 将点位加入集合 } - Collections.sort(results, Comparator.comparing(Point::getXDistance));//排序 + Collections.sort(selectPoints, Comparator.comparing(Point::getXDistance));//排序 - results = results.subList(0, Math.min(50, results.size())); + selectPoints = selectPoints.subList(0, Math.min(50, selectPoints.size())); }else { - for (int i = list.size()-count; i < list.size(); i++) { - results.add(new Point(list.get(i)[0],list.get(i)[1],list.get(i)[2])); + for (int i = pointList.size()-gravityMinCount; i < pointList.size(); i++) { + selectPoints.add(new Point(pointList.get(i)[0],pointList.get(i)[1],pointList.get(i)[2])); } //flag = true; } // 求初始重心 - double[] focus = focusPoint(results); + double[] focus = focusPointObj(selectPoints); - //重心 + // 迭代计算重心，直到选出50%的点 int iterNum = 0; - if(globeProportion(focus,r) < rate){ + List pointList; + if(globeProportion(focus,r) < pointSelectedRate){ do{ iterNum++; - r += 0.1; + r += iterStep; //球附近点集个数有可能为0，继续计算会导致focus结果出现NaN - List doubles1 = globeFilter(focus, r); - if(doubles1.size() == 0){ - continue; - } - focus = focus(doubles1); - }while (globeProportion(focus,r) < rate); - System.out.println("calc focus iter num:"+iterNum); -// if (flag){ -// lastFocus = new double[3]; -// lastFocus = focus; -// } + pointList = globeFilter(focus, r); + if(pointList.size() == 0) continue; + focus = focusPoint(pointList); + }while (globeProportion(focus,r) < pointSelectedRate); + logger.info("calc focus iter num:{}, point num:{}",iterNum,pointList.size()); return focus; }else{ -// if (flag){ -// lastFocus = new double[3]; -// lastFocus = focus; -// } + logger.info("calc focus iter num:0, point num:{}",selectPoints.size()); return focus; } } }catch (Exception e){ - //如果计算过程中修改list，可能引发异常 + //如果计算过程中修改pointList，可能引发异常 e.printStackTrace(); } return resultB562Sub(); @@ -159,10 +164,10 @@ public class FocusCalculator1 implements FocusCalculator{ * resultB562替补方法，尽量不返回空 * @return x,y,z,有效点数 */ - private double[] resultB562Sub(){ - if(list.size() > 0){ + double[] resultB562Sub(){ + if(pointList.size() > 0){ //重心 - return focus(list); + return focusPoint(pointList); } return null; } @@ -172,7 +177,7 @@ public class FocusCalculator1 implements FocusCalculator{ * @param xyz * @return true表示数据正常 */ - private boolean filter(double[] xyz){ + boolean filter(double[] xyz){ double a = xyz[0]*xyz[1]*xyz[2]; if(a == 0 || Double.isInfinite(a) || Double.isNaN(a)){ return false; @@ -185,7 +190,7 @@ public class FocusCalculator1 implements FocusCalculator{ * @param list * @return */ - private double[] focus(List list){ + double[] focusPoint(List list){ double sumX = 0; double sumY = 0; double sumZ = 0; @@ -198,7 +203,7 @@ public class FocusCalculator1 implements FocusCalculator{ return new double[]{sumX/count,sumY/count,sumZ/count}; } - private double[] focusPoint(List list){ + double[] focusPointObj(List list){ double sumX = 0; double sumY = 0; double sumZ = 0; @@ -231,15 +236,15 @@ public class FocusCalculator1 implements FocusCalculator{ * @param r * @return */ - private double globeProportion(double[] focus, double r){ + double globeProportion(double[] focus, double r){ int in = 0;//在球内的个数 - for (double[] doubles : list) { + for (double[] doubles : pointList) { double dis = dis(doubles, focus); if(dis < r){ in += 1; } } - double proportion = 1.00*in/list.size(); + double proportion = 1.00*in/pointList.size(); return proportion; } @@ -249,9 +254,9 @@ public class FocusCalculator1 implements FocusCalculator{ * @param r * @return */ - private List globeFilter(double[] focus, double r){ + List globeFilter(double[] focus, double r){ ArrayList arrayList = new ArrayList<>(); - for (double[] doubles1 : list) { + for (double[] doubles1 : pointList) { if(dis(doubles1,focus) < r){ arrayList.add(doubles1); } @@ -266,6 +271,7 @@ public class FocusCalculator1 implements FocusCalculator{ * 3.可用点数无，用9250 * @return */ + @Override public double[] ekfResult(double[] b562Xyz, double[] tiltXyz){ if(tiltXyz == null){ return b562Xyz; @@ -275,15 +281,15 @@ public class FocusCalculator1 implements FocusCalculator{ } double b562Variance; double tiltVariance = 5; //Sherlock 230802 2-》5， 减小惯导的长周期影响 - if(list.size() >= 50){ + if(pointList.size() >= 50){ b562Variance = 1; - }else if(list.size() >= 40){ + }else if(pointList.size() >= 40){ b562Variance = 3; - }else if(list.size() >= 30){ + }else if(pointList.size() >= 30){ b562Variance = 9; - }else if(list.size() >= 20){ + }else if(pointList.size() >= 20){ b562Variance = 30; - }else if(list.size() >= 10){ + }else if(pointList.size() >= 10){ b562Variance = 60; }else{ b562Variance = 90; @@ -303,77 +309,72 @@ public class FocusCalculator1 implements FocusCalculator{ * 加入tilts * @param tilt */ + @Override public void addTilt(Tilt tilt){ tilts.add(tilt); if(tilt.getShock() > shockThreshold) isShock = true; - if(tilts.size() > 300){ + if(tilts.size() > gravityMaxCount){ tilts.remove(0); } } + @Override public boolean isShocked() { return isShock; } - /** - * 取平均 - * @return - */ - public Tilt avgTilt(){ - if(tilts.size() == 0){ + @Override + public Tilt avgTilt() { + if (tilts.size() == 0) { return null; } - try { - Iterator iterator = tilts.iterator(); - int size = tilts.size(); - double sumP = 0; - double sumR = 0; - double sumY = 0; - while (iterator.hasNext()){ - Tilt tilt = iterator.next(); - sumP += Math.sin(tilt.getPitch()*Math.PI/180); - sumR += Math.sin(tilt.getRoll()*Math.PI/180); - sumY += Math.sin(tilt.getYaw()*Math.PI/180); - } - double pitch = (Math.asin(sumP/size) * 180/Math.PI + 360)%360; - double roll = (Math.asin(sumR/size) * 180/Math.PI + 360)%360; - double yaw = (Math.asin(sumY/size) * 180/Math.PI + 360)%360; - - return new Tilt(pitch,roll,yaw); - }catch (Exception e){ - e.printStackTrace(); + Iterator iterator = tilts.iterator(); + double roll = 0; + double pitch = 0; + double yaw = 0; + while (iterator.hasNext()) { + Tilt tilt = iterator.next(); + roll += tilt.getRoll(); + pitch += tilt.getPitch(); + yaw += tilt.getYaw(); } - return null; + return new Tilt(pitch,roll,yaw); } + @Override public double[] result9250(){ Tilt avg = avgTilt(); - if(tilt0 == null || position0 == null || r == 0 || avg == null){ + if(tilt0 == null || position0 == null || height == 0 || avg == null){ return null; } - return TiltUtil.toPosition(avg,tilt0,position0,r); + return TiltUtil.toPosition(avg,tilt0,position0,height); } + @Override public Tilt getTilt0(){ return tilt0; } + @Override public double[] getPosition0(){ return position0; } + @Override public double getR(){ - return r; + return height; } + @Override public void addDelayMs(int ms){ delay_ms += ms; - counter ++; + delay_counter ++; } + @Override public int getAvgDelayMs(){ - return delay_ms/counter; + return delay_ms/delay_counter; } } 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 eb2010d9..b76d02e0 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 @@ -1,5 +1,6 @@ package com.imdroid.sideslope.bd; +import java.time.LocalDateTime; import java.util.*; /** @@ -10,243 +11,79 @@ import java.util.*; * 5.否则，扩大半径0.01再求重心，包容百分之68，此重心为所求 * 6.。。。。。。 */ -public class FocusCalculator2 implements FocusCalculator{ +public class FocusCalculator2 extends FocusCalculator1{ + final static int MAX_B562_NUM = 3600; + final static int B562_EXPIRED_HOUR = 2; + final List pointTimeList = Collections.synchronizedList(new ArrayList<>()); - private final List list = Collections.synchronizedList(new ArrayList<>()); - - private final List pointList = new ArrayList<>(); - private int counterNoB562 = 0; - private int counterNoFixed = 0; - private int counterFixedResult = 0; +/** + * 构建计算器，如果tilt0和position0是null，那么计算过程将不考虑与tilts数据融合 + * @param height 杆的长度，单位厘米 + * @param tilt0 一个小时前的tilt平均值，用一小时的avgTilt计算得到，如果第一次，用null + * @param position0 一个小时前的xyz三维坐标值，用一小时前的ekf计算得到的那个，如果第一次，用null + */ + public FocusCalculator2(double height, Tilt tilt0, double[] position0){ + super(height,tilt0,position0); + iterStep = 0.2; + gravityMaxCount = MAX_B562_NUM; + } + public FocusCalculator2(){ + } + @Override public void reset(){ - list.clear(); - pointList.clear(); + //pointList.clear(); counterNoB562 = 0; counterNoFixed = 0; counterFixedResult = 0; } /** - * 加入list + * 加入pointList * @param xyz */ @Override public void addXyz(double[] xyz){ + LocalDateTime now = LocalDateTime.now(); + Iterator iterPointTime = pointTimeList.iterator(); + Iterator iterPoint = pointList.iterator(); + while(iterPointTime.hasNext() && iterPoint.hasNext()){ + LocalDateTime pointTime = iterPointTime.next(); + if(now.isAfter(pointTime.plusHours(B562_EXPIRED_HOUR))){ + iterPointTime.remove(); + iterPoint.remove(); + } + else{ + break; + } + } + if((int)xyz[3] == UBXUtil.NO_B562) counterNoB562++; else if((int)xyz[3] == UBXUtil.NO_FIX_RESULT) counterNoFixed++; else { counterFixedResult ++; if (filter(xyz)) { - list.add(xyz); + pointList.add(xyz); + pointTimeList.add(now); } - if (list.size() > 300) { - list.remove(0); + if (pointList.size() > gravityMaxCount) { + pointList.remove(0); + pointTimeList.remove(0); } } } - private boolean filter(double[] xyz){ - double a = xyz[0]*xyz[1]*xyz[2]; - if(a == 0 || Double.isInfinite(a) || Double.isNaN(a)){ - return false; - } - return true; + public double[] resultB562(double[] lastResult){ + if(counterFixedResult<30) return null; + else return super.resultB562(lastResult); } @Override public int[] getB562Stat(){ - return new int[]{counterFixedResult,counterNoFixed,counterNoB562}; - } - - - /** - * 计算得到最终结果 - * @return - */ - public double[] resultB562_2() { - // 求全体样本的重心 - double e = 0; - double n = 0; - double d = 0; - for(double[] point: list){ - e += point[0]; - n += point[1]; - d += point[2]; - } - e /= list.size(); - n /= list.size(); - d /= list.size(); - - double g = 0; - int iterNum = 0; - while(Math.abs(g-e)>0.2){ - g = e; - e = gravity(g, 0.3, 0); - iterNum ++; - if(iterNum>=10) break; - } - //System.out.println("e iter num: "+iterNum); - - g = 0; - iterNum = 0; - while(Math.abs(g-n)>0.2){ - g = n; - n = gravity(g, 0.26, 0); - iterNum ++; - if(iterNum>=10) break; - } - //System.out.println("n iter num: "+iterNum); - - g = 0; - iterNum = 0; - while(Math.abs(g-d)>0.4){ - g = d; - d = gravity(g, 0.87, 0); - iterNum ++; - if(iterNum>=10) break; - } - //System.out.println("e iter num: "+iterNum); - - return new double[]{e,n,d}; - } - - double gravity(double g, double r, int index){ - //筛选出半径为r=0.144的圆内的点 - double newG = 0; - List list2 = new ArrayList<>(); - for(double[] point: list){ - if(Math.abs(point[index]-g)<=r){ - list2.add(point[index]); - newG += point[index]; - } - } - System.out.println("sub set: " + list2.size()); - if(list2.size()>0) newG /= list2.size(); - return newG; - } - - @Override - public double[] resultB562(double[] last) { - double[] g = {0, 0, 0}; - if(last != null) g=last; - else { - for (double[] xyz : list) { - g[0] += xyz[0]; - g[1] += xyz[1]; - g[2] += xyz[2]; - } - g[0] /= list.size(); - g[1] /= list.size(); - g[2] /= list.size(); - } - - double[] g2 = {0,0,0}; - int iterNum = 0; - while(isLarge(g,g2,2, 4)){ - g2 = g; - g = gravity3(g); - iterNum ++; - if(iterNum>=10) break; - } - System.out.println("e iter num: "+iterNum); - - return g; - } - - boolean isLarge(double[] p1, double[] p2, double xy, double z){ - //return true; - return (Math.sqrt((p1[0]-p2[0])*(p1[0]-p2[0])+(p1[1]-p2[1])*(p1[1]-p2[1]))>xy || - Math.abs(p1[2]-p2[2])>z); - } - - double[] gravity2(double[] g){ - //筛选出半径为r=0.144的圆内的点 - List points = new ArrayList<>(); - for(double[] xyz: list){ - Point point = new Point(xyz[0], xyz[1], xyz[2]); - point.setXDistance(Math.sqrt((xyz[0]-g[0])*(xyz[0]-g[0])+(xyz[1]-g[1])*(xyz[1]-g[1]))); - point.setZDistance(Math.abs(xyz[2]-g[2])); - points.add(point); - } - - double[] newG = {0,0,0}; - int i=0; - int calcNum = points.size()*5/10; - Collections.sort(points, Comparator.comparing(Point::getXDistance));//升序排序 - for(Point point:points){ - newG[0] += point.getX(); - newG[1] += point.getY(); - i++; - if(i>=calcNum) { - newG[0] /= calcNum; - newG[1] /= calcNum; - break; - } - } - - Collections.sort(points, Comparator.comparing(Point::getZDistance));//升序排序 - i=0; - for(Point point:points){ - newG[2] += point.getZ(); - i++; - if(i>=calcNum) { - newG[2] /= calcNum; - break; - } - } - - return newG; - } - - double[] gravity3(double[] g){ - //筛选出半径为r=0.144的圆内的点 - List points = new ArrayList<>(); - for(double[] xyz: list){ - Point point = new Point(xyz[0], xyz[1], xyz[2]); - point.setXDistance(Math.abs(xyz[0]-g[0])); - point.setYDistance(Math.abs(xyz[1]-g[1])); - point.setZDistance(Math.abs(xyz[2]-g[2])); - points.add(point); - } - - double[] newG = {0,0,0}; - int i=0; - int calcNum = points.size()*5/10; - Collections.sort(points, Comparator.comparing(Point::getXDistance));//升序排序 - for(Point point:points){ - newG[0] += point.getX(); - i++; - if(i>=calcNum) { - newG[0] /= calcNum; - break; - } - } - - Collections.sort(points, Comparator.comparing(Point::getYDistance));//升序排序 - i=0; - for(Point point:points){ - newG[1] += point.getY(); - i++; - if(i>=calcNum) { - newG[1] /= calcNum; - break; - } - } - - Collections.sort(points, Comparator.comparing(Point::getZDistance));//升序排序 - i=0; - for(Point point:points){ - newG[2] += point.getZ(); - i++; - if(i>=calcNum) { - newG[2] /= calcNum; - break; - } - } - - return newG; + int[] b562Stat = new int[]{counterFixedResult,counterNoFixed,counterNoB562}; + reset(); + return b562Stat; } } 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 5aa2f9d1..68fae736 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 @@ -20,8 +20,6 @@ import java.util.concurrent.ConcurrentHashMap; public class GNSSCalcFilterService { private final Logger logger = LoggerFactory.getLogger(GNSSCalcFilterService.class); - @Resource(name = "local") - DeviceService gnssDeviceRepository; @Autowired private GnssGroupCalcMapper groupCalcMapper; @Autowired @@ -44,18 +42,13 @@ public class GNSSCalcFilterService { final DateTimeFormatter dateFormatter = DateTimeFormatter.ofPattern("yyyy-MM-dd HH:mm:ss"); - public void calc(GnssCalcData locationRecord, boolean isExceed) { + public void calc(Device gnssDevice, GnssGroupCalc groupCalc, GnssCalcData locationRecord, boolean isExceed) { String deviceId = locationRecord.getDeviceid(); - Device gnssDevice = gnssDeviceRepository.findByDeviceId(deviceId); - if(gnssDevice == null) return; //补充解算记录的设备信息 locationRecord.setTenantid(gnssDevice.getTenantId()); locationRecord.setEnabled(true); - // 获取平滑参数 - GnssGroupCalc groupCalc = getCalcParams(gnssDevice.getCalcGroupId()); - // 计算平滑周期 int filterCycle = groupCalc.getFilter_hour(); VaryFilterCycle varyCycle = autoCycleDevices.get(deviceId); @@ -76,20 +69,6 @@ public class GNSSCalcFilterService { } - GnssGroupCalc getCalcParams(int calcGroupId){ - GnssGroupCalc calcParam = groupCalcMapper.selectById(calcGroupId); - if(calcParam == null){ - calcParam = new GnssGroupCalc(); - calcParam.setAuto_filter(false); - calcParam.setFilter_hour(FILTER_DEFAULT_CYCLE_HOUR); - calcParam.setFilter_min_hour(FILTER_MIN_CYCLE_HOUR); - calcParam.setXy_threshold(XY_THRESHOLD); - calcParam.setZ_threshold(Z_THRESHOLD); - calcParam.setAuto_threshold(AUTO_THRESHOLD); - } - return calcParam; - } - /** * 计算东北天的最近融合数据的加权平均, 刘畅20230725 copy 自 avgEND; id>20, 滤波参数6h 或 25h */ @@ -223,12 +202,8 @@ public class GNSSCalcFilterService { } } - public LocalDateTime updateRpos(String deviceId, LocalDateTime afterTime){ - // 获取平滑参数 - Device gnssDevice = gnssDeviceRepository.findByDeviceId(deviceId); - if(gnssDevice == null) return afterTime; - GnssGroupCalc groupCalc = getCalcParams(gnssDevice.getCalcGroupId()); - + public LocalDateTime updateRpos(Device gnssDevice, GnssGroupCalc groupCalc,LocalDateTime afterTime){ + String deviceId=gnssDevice.getDeviceId(); // 平滑处理 LocalDateTime beforTime = afterTime.plusHours(groupCalc.getFilter_hour()); QueryWrapper query = new QueryWrapper<>(); diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSDataCalcService.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSDataCalcService.java index 0fb588bc..e90a6381 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSDataCalcService.java +++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSDataCalcService.java @@ -1,7 +1,8 @@ package com.imdroid.sideslope.calc; -import com.imdroid.sideslope.bd.Tilt; +import com.imdroid.secapi.dto.GnssGroupCalc; import com.imdroid.sideslope.message.D341LocationMessage; +import com.imdroid.sideslope.sal.Device; import java.time.LocalDateTime; @@ -18,19 +19,6 @@ public interface GNSSDataCalcService { /** * 单轮解算结束，计算平滑值 */ - void calSingleDone(String deviceId, Integer tenantId, LocalDateTime resultTime); - /** - * 根据GNSS数据的中间结果，计算出最终结果 - * - * @param deviceId 设备id - * @return x,y,z三轴数据 - */ - //double[] calcResult(String deviceId,double[] b562Xyz, double[] tiltXyz); + void calSingleDone(Device device, GnssGroupCalc groupCalc, LocalDateTime resultTime); - /** - * 根据GNSS数据的中间结果，计算Tilt的平均值 - * @param deviceid 设备id - * @return Tilt - */ - Tilt calcAvgTilt(String deviceid); } diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/MultiLineGNSSCalcService.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/MultiLineGNSSCalcService.java index fbdff4d1..4af838ee 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/MultiLineGNSSCalcService.java +++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/MultiLineGNSSCalcService.java @@ -5,13 +5,17 @@ import com.imdroid.secapi.dto.*; import com.imdroid.sideslope.message.BaseMessage; import com.imdroid.sideslope.message.D341LocationMessage; import com.imdroid.sideslope.message.D342LocationMessage; +import com.imdroid.sideslope.sal.Device; +import com.imdroid.sideslope.sal.DeviceService; import com.imdroid.sideslope.service.DataPersistService; import org.slf4j.Logger; import org.slf4j.LoggerFactory; import org.springframework.beans.factory.annotation.Autowired; import org.springframework.stereotype.Service; +import javax.annotation.Resource; import java.time.LocalDateTime; +import java.util.List; import java.util.Map; import java.util.concurrent.ConcurrentHashMap; @@ -22,7 +26,7 @@ public class MultiLineGNSSCalcService { LocalDateTime createTime; LocalDateTime uploadTime; } - private static final Logger logger = LoggerFactory.getLogger(SingleLineGNSSCalcService.class); + private static final Logger logger = LoggerFactory.getLogger(MultiLineGNSSCalcService.class); private static final Map deviceMap = new ConcurrentHashMap<>(); private static final Map fwdRecordMap = new ConcurrentHashMap<>(); @@ -36,6 +40,24 @@ public class MultiLineGNSSCalcService { private BeidouClient beidouClient; @Autowired DataPersistService dataPersistService; + @Resource(name = "local") + private DeviceService deviceService; + + @Autowired + GnssGroupCalcMapper groupCalcMapper; + List groupCalcList; + + GnssGroupCalc getGroupCalc(int groupId){ + if(groupCalcList == null){ + groupCalcList = groupCalcMapper.selectList(null); + } + if(groupCalcList != null){ + for(GnssGroupCalc groupCalc:groupCalcList){ + if(groupCalc.getId() == groupId) return groupCalc; + } + } + return null; + } public void calc(D342LocationMessage d342Message){ // 如果时间跨度大于1分钟，或者不含d341，则计算平滑值 @@ -56,7 +78,9 @@ public class MultiLineGNSSCalcService { if (msgTime.isAfter(lastD342Time.createTime.plusMinutes(4))) { logger.info(deviceId + " d341 cycle done!"); // 计算上轮结果 - calcService.calSingleDone(deviceId, d342Message.getTenantId(), lastD342Time.createTime); + Device device = deviceService.findByDeviceId(deviceId); + GnssGroupCalc groupCalc = getGroupCalc(device.getCalcGroupId()); + calcService.calSingleDone(device, groupCalc, lastD342Time.createTime); } } else{ @@ -105,9 +129,11 @@ public class MultiLineGNSSCalcService { } // 计算上轮结果 - calcService.calSingleDone(deviceId, tenantId, lastDate); + Device device = deviceService.findByDeviceId(deviceId); + GnssGroupCalc groupCalc = getGroupCalc(device.getCalcGroupId()); + calcService.calSingleDone(device, groupCalc, lastDate); // 重算最近的 - lastDate = gnssCalcFilterService.updateRpos(deviceId,lastDate); + lastDate = gnssCalcFilterService.updateRpos(device,groupCalc,lastDate); // 记录转发表更新为upload done ResendRecord fwdRecord = fwdRecordMap.get(deviceId); if(fwdRecord != null){ @@ -159,4 +185,9 @@ public class MultiLineGNSSCalcService { } else return null; } + + public void refreshGroupCalc(){ + groupCalcList = groupCalcMapper.selectList(null); + logger.info("group paras changed"); + } } 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 7c51bf59..b4851fc0 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 @@ -2,6 +2,8 @@ package com.imdroid.sideslope.calc; import com.imdroid.common.util.ThreadManager; import com.imdroid.secapi.dto.GnssCalcData; +import com.imdroid.secapi.dto.GnssGroupCalc; +import com.imdroid.secapi.dto.GnssGroupCalcMapper; import com.imdroid.sideslope.bd.*; import com.imdroid.sideslope.message.D341LocationMessage; import com.imdroid.sideslope.sal.Device; @@ -15,6 +17,7 @@ import org.springframework.stereotype.Service; import javax.annotation.Resource; import java.time.LocalDateTime; import java.util.Arrays; +import java.util.List; import java.util.Map; import java.util.concurrent.*; /** @@ -26,7 +29,7 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { private static final Logger logger = LoggerFactory.getLogger(SingleLineGNSSCalcService.class); - private static final Map calculatorMap = new ConcurrentHashMap<>(); + private static final Map calculatorMap = new ConcurrentHashMap<>(); private static final Map> timerMap = new ConcurrentHashMap<>(); @@ -36,8 +39,6 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { private static final Map cleanTiltStatusMap = new ConcurrentHashMap<>(); - private static final Map cleanPositionStatusMap = new ConcurrentHashMap<>(); - @Autowired WarningService warningService; @@ -47,26 +48,53 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { @Resource(name = "local") private DeviceService deviceService; + @Autowired + GnssGroupCalcMapper groupCalcMapper; + List groupCalcList; + + GnssGroupCalc getGroupCalc(int groupId){ + if(groupCalcList == null){ + groupCalcList = groupCalcMapper.selectList(null); + } + if(groupCalcList != null){ + for(GnssGroupCalc groupCalc:groupCalcList){ + if(groupCalc.getId() == groupId) return groupCalc; + } + } + return null; + } + @Override public double[] calcSingle(D341LocationMessage message, boolean completeWhenIdle) { String deviceId = message.getId(); - if(completeWhenIdle) resultOutputTimer(deviceId, message.getTenantId(), message.getCreateTime()); + Device device = deviceService.findByDeviceId(deviceId); + if(device == null) return null; + GnssGroupCalc groupCalc = getGroupCalc(device.getCalcGroupId()); + + if(completeWhenIdle) resultOutputTimer(device, groupCalc, message.getCreateTime()); //todo 创建FocusCalculator对象需获取该测站的杆长度，上一小时的Tilt平均值，上一小时的测站相对坐标融合值ekfResult - FocusCalculator1 focusCalculator = calculatorMap.computeIfAbsent(deviceId, s -> new FocusCalculator1(150,tiltMap.get(deviceId), - positionMap.get(deviceId))); + FocusCalculator focusCalculator; + /*if(groupCalc!=null && groupCalc.getVer() == 2){ + focusCalculator = calculatorMap.computeIfAbsent(deviceId, + s -> new FocusCalculator2(200,tiltMap.get(deviceId),positionMap.get(deviceId))); + } + else {*/ + focusCalculator = calculatorMap.computeIfAbsent(deviceId, + s -> new FocusCalculator1(200, tiltMap.get(deviceId),positionMap.get(deviceId))); + //} + // 读取惯导 Tilt tilt = message.getTilt(); - focusCalculator.addTilt(tilt); if (logger.isDebugEnabled()) { - logger.debug("测站" + deviceId + "的9250单次解析结果:{}", tilt + "," + Arrays.toString(TiltUtil.toPosition(tilt, focusCalculator.getTilt0(), focusCalculator.getPosition0(), focusCalculator.getR()))); + logger.debug("测站" + deviceId + "惯导单次解析结果:{}", tilt); } - // 计算延迟 + // 延迟 focusCalculator.addDelayMs(message.getPps()); - //计算到单次相对位置xyz并记录 + // 单次b562 double[] doubles = message.getB562_loc(); focusCalculator.addXyz(doubles); if (logger.isDebugEnabled()) { @@ -76,10 +104,10 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { } @Override - public void calSingleDone(String deviceId, Integer tenantId, LocalDateTime resultTime) { + public void calSingleDone(Device device, GnssGroupCalc groupCalc, LocalDateTime resultTime) { ThreadManager.getScheduledThreadPool().schedule(() -> { try { - calCycleResult(deviceId, tenantId, resultTime); + calCycleResult(device, groupCalc, resultTime); } catch (Exception e) { logger.error(e.toString()); } @@ -87,9 +115,9 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { } - private void resultOutputTimer(String deviceId, Integer tenantId, LocalDateTime date){ + private void resultOutputTimer(Device device, GnssGroupCalc groupCalc,LocalDateTime date){ //40秒没数据后输出结果 - ScheduledFuture future = timerMap.get(deviceId); + ScheduledFuture future = timerMap.get(device.getDeviceId()); if (future != null && !future.isDone()) { future.cancel(true); future = null; @@ -97,41 +125,37 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { future = ThreadManager.getScheduledThreadPool().schedule(() -> { try { - calCycleResult(deviceId, tenantId, date); + calCycleResult(device, groupCalc,date); // 清除统计 - Device device = deviceService.findByDeviceId(deviceId); - if(device != null) device.clearStat(); + device.clearStat(); } catch (Exception e) { logger.error(e.toString()); } - },40, TimeUnit.SECONDS); - timerMap.put(deviceId, future); + },20, TimeUnit.SECONDS); + timerMap.put(device.getDeviceId(), future); } - private void calCycleResult(String deviceId, Integer tenantId, LocalDateTime resultTime) { - FocusCalculator1 focusCalculator = calculatorMap.get(deviceId); + private void calCycleResult(Device device, GnssGroupCalc groupCalc, LocalDateTime resultTime) { + String deviceId = device.getDeviceId(); + Integer tenantId = device.getTenantId(); + FocusCalculator focusCalculator = calculatorMap.get(deviceId); if(focusCalculator == null) return; - // 1.检查b562有效数，如果过少，产生告警 - warningService.checkB562Num(deviceId, tenantId, - focusCalculator.getB562Stat()); - // 2.数据处理：参考上次的位置，计算b562重心 - double[] lastEfk = null; - if(positionMap.containsKey(deviceId)){ - lastEfk = positionMap.get(deviceId); - } + // 数据处理：参考上次的位置，计算b562重心 + double[] lastEfk = positionMap.get(deviceId); double[] b562Result = focusCalculator.resultB562(lastEfk); - double[] r9250Result = null; - //判断 取到的b562 和上次融合坐标做判断，如果距离>100mm 不计算9250 - if (lastEfk != null && b562Result!=null && FocusCalculator1.disXY(b562Result,lastEfk)<100){ + double[] r9250Result = null; // 倾角转换成杆顶到杆底的位移 + double[] mergeResult = null; + //判断 取到的b562 和上次融合坐标做判断，如果距离>200mm 不计算融合值 + if (lastEfk != null && b562Result!=null && FocusCalculator1.disXY(b562Result,lastEfk)<200){ r9250Result = focusCalculator.result9250(); + mergeResult = focusCalculator.ekfResult(b562Result,r9250Result);//融合位置 } - double[] result = focusCalculator.ekfResult(b562Result,r9250Result); - Tilt tilt = calcAvgTilt(deviceId); + Tilt tilt = focusCalculator.avgTilt(); logger.info("测站 {} 的b562相对坐标重心:{}", deviceId, Arrays.toString(b562Result)); - logger.info("测站 {} 的9250相对坐标:{}", deviceId, Arrays.toString(r9250Result)); - logger.info("测站 {} 的相对坐标融合值:{}", deviceId, Arrays.toString(result)); + logger.info("测站 {} 的惯导相对坐标:{}", deviceId, Arrays.toString(r9250Result)); + logger.info("测站 {} 的相对坐标融合值:{}", deviceId, Arrays.toString(mergeResult)); logger.info("测站 {} 的Tilt平均值:{}", deviceId, tilt); logger.info("测站 {} 的平均延迟:{}ms", deviceId, focusCalculator.getAvgDelayMs()); @@ -143,68 +167,62 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService { } else { tiltMap.put(deviceId, tilt); } - } - if (result != null && b562Result != null) { + }/* + if (mergeResult != null && b562Result != null) { //这里检查一下result，过滤1千米外非正常数据 - if(focusCalculator.dis(result,b562Result) < 1000_00) { + if(focusCalculator.dis(mergeResult,b562Result) < 1000_00) { if (Boolean.TRUE.equals(cleanPositionStatusMap.get(deviceId))) { // 置为false，下一次计算时就可以正常设置值到positionMap了 cleanPositionStatusMap.put(deviceId, Boolean.FALSE); logger.info("置为false，下一次计算时就可以正常设置值到positionMap了"); } else { - positionMap.put(deviceId, result); + positionMap.put(deviceId, mergeResult); } - postLocationRecord(deviceId, b562Result, r9250Result, result, resultTime, + postLocationRecord(deviceId, b562Result, r9250Result, mergeResult, resultTime, focusCalculator.getAvgDelayMs(), focusCalculator.isShocked()); }else{ logger.error("融合值异常"); } + }*/ + if (b562Result != null) { + positionMap.put(deviceId, b562Result); + GnssCalcData locationRecord = new GnssCalcData(); + locationRecord.setCreatetime(resultTime); + locationRecord.setUpdatetime(LocalDateTime.now()); //通过这里可以区分补传记录 + locationRecord.setDeviceid(deviceId); + + // 调用这个函数之前已判断是否为null + locationRecord.setB562e(b562Result[0] * 10); //cm->mm + locationRecord.setB562n(b562Result[1] * 10); + locationRecord.setB562d(b562Result[2] * 10); + + if(r9250Result!=null) { + locationRecord.setR9250e(r9250Result[0]); + locationRecord.setR9250n(r9250Result[1]); + locationRecord.setR9250d(r9250Result[2]); + } +/* + if(mergeResult!=null){ + locationRecord.setMpose(mergeResult[0]); + locationRecord.setMposn(mergeResult[1]); + locationRecord.setMposd(mergeResult[2]); + } +*/ + locationRecord.setPps(focusCalculator.getAvgDelayMs()); + gnssCalcFilterService.calc(device, groupCalc, locationRecord, focusCalculator.isShocked()); } + + // 检查b562有效数，如果过少，产生告警 + warningService.checkB562Num(deviceId, tenantId, + focusCalculator.getB562Stat()); + focusCalculator.reset(); - calculatorMap.remove(deviceId); + //calculatorMap.remove(deviceId); } - private void postLocationRecord(String deviceId, double[] b562Result, double[] r9250Result, double[] result, - LocalDateTime resultTime, int delay, boolean isShocked) { - GnssCalcData locationRecord = new GnssCalcData(); - locationRecord.setCreatetime(resultTime); - locationRecord.setUpdatetime(LocalDateTime.now()); //通过这里可以区分补传记录 - locationRecord.setDeviceid(deviceId); - - // 调用这个函数之前已判断是否为null - locationRecord.setB562e(b562Result[0] * 10); //cm->mm - locationRecord.setB562n(b562Result[1] * 10); - locationRecord.setB562d(b562Result[2] * 10); - - if(r9250Result!=null) { - locationRecord.setR9250e(r9250Result[0]); - locationRecord.setR9250n(r9250Result[1]); - locationRecord.setR9250d(r9250Result[2]); - } - - locationRecord.setPps(delay); - gnssCalcFilterService.calc(locationRecord, isShocked); + public void refreshGroupCalc(){ + groupCalcList = groupCalcMapper.selectList(null); + logger.info("group paras changed"); } - - @Override - public Tilt calcAvgTilt(String deviceId) { - FocusCalculator1 focusCalculator = calculatorMap.get(deviceId); - if (focusCalculator != null) { - return focusCalculator.avgTilt(); - } - return null; - } - - /*** - * 清零：不需要，滤波窗口是按时间来选取历史数据的，只要超过4小时或一天，即使中间没有数据，也不会用 - * 超过filterCycle的数据 - - public void cleanTiltByDeviceId(String deviceId) { - logger.info("清零[{}]", deviceId); - tiltMap.remove(deviceId); - positionMap.remove(deviceId); - cleanTiltStatusMap.put(deviceId, Boolean.TRUE); - cleanPositionStatusMap.put(deviceId, Boolean.TRUE); - }*/ } diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/web/ApiController.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/web/ApiController.java index 46e879eb..d21d4b57 100644 --- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/web/ApiController.java +++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/web/ApiController.java @@ -1,6 +1,8 @@ package com.imdroid.sideslope.web; import com.imdroid.secapi.client.HttpResp; +import com.imdroid.sideslope.calc.MultiLineGNSSCalcService; +import com.imdroid.sideslope.calc.SingleLineGNSSCalcService; import com.imdroid.sideslope.sal.LocalDeviceServiceImpl; import com.imdroid.sideslope.server.DeviceChannel; import com.imdroid.sideslope.server.OnlineChannels; @@ -33,6 +35,10 @@ public class ApiController { @Autowired WarningService warningService; + @Autowired + SingleLineGNSSCalcService calcService; + @Autowired + MultiLineGNSSCalcService multiCalcService; @PostMapping(value = "/config") public HttpResp config(String deviceId, String configuration) { @@ -74,6 +80,16 @@ public class ApiController { return resp; } + @PostMapping("/group_param_changed") + public HttpResp groupParamChanged(){ + calcService.refreshGroupCalc(); + multiCalcService.refreshGroupCalc(); + HttpResp resp = new HttpResp(); + resp.setResponseMessage("succeed"); + return resp; + } + + @PostMapping("/warning_param_changed") public HttpResp warningParamChanged(){ warningService.refreshCfg(); diff --git a/sec-beidou-rtcm/src/test/java/FocusCalculator3.java b/sec-beidou-rtcm/src/test/java/FocusCalculator3.java deleted file mode 100644 index 5fb087f1..00000000 --- a/sec-beidou-rtcm/src/test/java/FocusCalculator3.java +++ /dev/null @@ -1,188 +0,0 @@ -import com.imdroid.sideslope.bd.FocusCalculator; -import com.imdroid.sideslope.bd.Point; -import org.ejml.simple.SimpleMatrix; - -import java.util.ArrayList; -import java.util.Collections; -import java.util.Comparator; -import java.util.List; - -public class FocusCalculator3 implements FocusCalculator { - - private final List list = Collections.synchronizedList(new ArrayList<>()); - - private final List listE = Collections.synchronizedList(new ArrayList<>()); - - private final List listN = Collections.synchronizedList(new ArrayList<>()); - - private final List listD = Collections.synchronizedList(new ArrayList<>()); - - private final List pointList = new ArrayList<>(); - - - @Override - public void reset(){ - list.removeAll(list); - pointList.removeAll(pointList); - listE.removeAll(listE); - listD.removeAll(listD); - listN.removeAll(listN); - } - - /** - * 加入list - * @param xyz - */ - @Override - public void addXyz(double[] xyz){ - if(filter(xyz)){ - list.add(xyz); - } - if(list.size() > 300){ - list.remove(0); - } - } - - private boolean filter(double[] xyz){ - double a = xyz[0]*xyz[1]*xyz[2]; - if(a == 0 || Double.isInfinite(a) || Double.isNaN(a)){ - return false; - } - return true; - } - - @Override - public int[] getB562Stat(){ - return new int[]{0,0,0}; - } - - @Override - public double[] resultB562(double[] last) { - double[] g = {0, 0, 0}; - if(last != null) g=last; - else { - for (double[] xyz : list) { - g[0] += xyz[0]; - g[1] += xyz[1]; - g[2] += xyz[2]; - } - g[0] /= list.size(); - g[1] /= list.size(); - g[2] /= list.size(); - } - - makeMatrixList(g); - - double[] l = listE.stream().mapToDouble(Double::doubleValue).toArray(); - double e = igg3(l); - l = listN.stream().mapToDouble(Double::doubleValue).toArray(); - double n = igg3(l); - l = listD.stream().mapToDouble(Double::doubleValue).toArray(); - double d = igg3(l); - - return new double[]{e,n,d}; - } - - void makeMatrixList(double[] g){ - //筛选出半径为r=0.144的圆内的点 - List points = new ArrayList<>(); - for(double[] xyz: list){ - Point point = new Point(xyz[0], xyz[1], xyz[2]); - point.setXDistance(Math.abs(xyz[0]-g[0])); - point.setYDistance(Math.abs(xyz[1]-g[1])); - point.setZDistance(Math.abs(xyz[2]-g[2])); - points.add(point); - } - - int calcNum = points.size()*5/10; - Collections.sort(points, Comparator.comparing(Point::getXDistance));//升序排序 - for(Point point:points){ - listE.add(point.getX()); - if(listE.size()>=calcNum) { - break; - } - } - - Collections.sort(points, Comparator.comparing(Point::getYDistance));//升序排序 - for(Point point:points){ - listN.add(point.getY()); - if(listN.size()>=calcNum) { - break; - } - } - - Collections.sort(points, Comparator.comparing(Point::getZDistance));//升序排序 - for(Point point:points){ - listD.add(point.getZ()); - if(listD.size()>=calcNum) { - break; - } - } - } - - public static double igg3(double[] l) { - SimpleMatrix L = new SimpleMatrix(l.length,1,false,l); - SimpleMatrix B = new SimpleMatrix(l.length,1); - B.fill(1); - SimpleMatrix P = new SimpleMatrix(l.length, l.length); - for(int i=0; i k1){ - k = 1e-8; - } - else{ - k=(k1-Math.abs(v))/(k1-k0); - k=k*k*(k0/Math.abs(v)); - } - P.set(i,i,P.get(i,i)*k); //P(i,i)=P(i,i)*k; - } - - if(x_prev==0) { - x_prev = X.get(0); - } - else if(Math.abs(x_prev-X.get(0))<0.1) { - break; - } - x_prev = X.get(0); - iterNum++; - } - - //x=x0+x_prev;%求出的最后长度 初值+改正数 - return (x0+x_prev); - } - -} diff --git a/sec-beidou-rtcm/src/test/java/FocusCalculatorTest.java b/sec-beidou-rtcm/src/test/java/FocusCalculatorTest.java index e155883e..385aba6a 100644 --- a/sec-beidou-rtcm/src/test/java/FocusCalculatorTest.java +++ b/sec-beidou-rtcm/src/test/java/FocusCalculatorTest.java @@ -22,8 +22,7 @@ import java.util.ListIterator; classes = { FocusCalculatorTest.class, FocusCalculator1.class, - FocusCalculator2.class, - FocusCalculator3.class + FocusCalculator2.class } ) diff --git a/sec-beidou/src/main/java/com/imdroid/beidou/controller/GnssGroupController.java b/sec-beidou/src/main/java/com/imdroid/beidou/controller/GnssGroupController.java index 4b183186..a4043dab 100644 --- a/sec-beidou/src/main/java/com/imdroid/beidou/controller/GnssGroupController.java +++ b/sec-beidou/src/main/java/com/imdroid/beidou/controller/GnssGroupController.java @@ -4,6 +4,7 @@ import com.alibaba.fastjson.JSONObject; import com.baomidou.mybatisplus.core.metadata.IPage; import com.baomidou.mybatisplus.extension.plugins.pagination.Page; import com.imdroid.beidou.common.HttpResult; +import com.imdroid.secapi.client.RtcmClient; import com.imdroid.secapi.dto.*; import org.springframework.beans.factory.annotation.Autowired; import org.springframework.stereotype.Controller; @@ -18,9 +19,10 @@ public class GnssGroupController extends BasicController { GnssGroupMapper gnssGroupMapper; @Autowired GnssGroupCalcMapper gnssGroupCalcMapper; - @Autowired GnssDeviceMapper deviceMapper; + @Autowired + RtcmClient rtcmClient; /********* 推送页面 *********/ @RequestMapping("/page/table/gnss_add_group") @@ -125,7 +127,10 @@ public class GnssGroupController extends BasicController { } if (num == 0) { return HttpResult.failed(); - } else return HttpResult.ok(); + } else{ + rtcmClient.groupParamChanged(); + return HttpResult.ok(); + } } @PostMapping("/gnss/group/delete_calc") @@ -141,7 +146,10 @@ public class GnssGroupController extends BasicController { int num = gnssGroupCalcMapper.deleteById(del_id); if (num == 0) { return HttpResult.failed(); - } else return HttpResult.ok(); + } else{ + rtcmClient.groupParamChanged(); + return HttpResult.ok(); + } } } diff --git a/sec-beidou/src/main/java/com/imdroid/beidou/task/DatasetCleaner.java b/sec-beidou/src/main/java/com/imdroid/beidou/task/DatasetCleaner.java index ec6902a1..70540dbf 100644 --- a/sec-beidou/src/main/java/com/imdroid/beidou/task/DatasetCleaner.java +++ b/sec-beidou/src/main/java/com/imdroid/beidou/task/DatasetCleaner.java @@ -76,7 +76,7 @@ public class DatasetCleaner { void checkFwdDataset(){ long before = System.currentTimeMillis() - - (long)365 * 24 * 3600 * 1000; + (long)180 * 24 * 3600 * 1000; Timestamp t = new Timestamp(before); int count = fwdRecordMapper.deleteTimeBefore(t); log.info("clean fwd dataset num: "+count); diff --git a/sec-beidou/src/main/resources/templates/page/gnss_data_calc.html b/sec-beidou/src/main/resources/templates/page/gnss_data_calc.html index 5f36c65f..4bb5262a 100644 --- a/sec-beidou/src/main/resources/templates/page/gnss_data_calc.html +++ b/sec-beidou/src/main/resources/templates/page/gnss_data_calc.html @@ -92,9 +92,9 @@ {field: 'rpose', title: '相对东', templet: "
{{d.rpose==null?'':d.rpose.toFixed(2)}}
"}, {field: 'rposn', title: '相对北', templet: "
{{d.rposn==null?'':d.rposn.toFixed(2)}}
"}, {field: 'rposd', title: '相对天', templet: "
{{d.rposd==null?'':d.rposd.toFixed(2)}}
"}, - {field: 'auxe', title: '辅助东', templet: "
{{d.auxe==null?'':d.auxe.toFixed(2)}}
"}, - {field: 'auxn', title: '辅助北', templet: "
{{d.auxn==null?'':d.auxn.toFixed(2)}}
"}, - {field: 'auxd', title: '辅助天', templet: "
{{d.auxd==null?'':d.auxd.toFixed(2)}}
"}, + //{field: 'auxe', title: '辅助东', templet: "
{{d.auxe==null?'':d.auxe.toFixed(2)}}
"}, + //{field: 'auxn', title: '辅助北', templet: "
{{d.auxn==null?'':d.auxn.toFixed(2)}}
"}, + //{field: 'auxd', title: '辅助天', templet: "
{{d.auxd==null?'':d.auxd.toFixed(2)}}
"}, {field: 'enabled', title: '有效',templet: '#enabledTrans'}, {field: 'pps', title: '平均延迟'} ]; diff --git a/sec-test-device/pom.xml b/sec-test-device/pom.xml new file mode 100644 index 00000000..271a8f75 --- /dev/null +++ b/sec-test-device/pom.xml @@ -0,0 +1,128 @@ + + + 4.0.0 + + com.imdroid + security-monitor + 1.0-SNAPSHOT + + + sec-test-device + + + 11 + 11 + UTF-8 + + + + + org.springframework.boot + spring-boot-starter-web + + + org.springframework.boot + spring-boot-starter-test + test + + + org.springframework.boot + spring-boot-devtools + true + + + + org.slf4j + slf4j-api + + + ch.qos.logback + logback-classic + + + io.netty + netty-all + 4.1.78.Final + + + com.google.code.gson + gson + 2.9.1 + + + org.apache.httpcomponents + httpclient + 4.5.13 + + + + com.imdroid + sec-api + 1.0-SNAPSHOT + + + com.imdroid + sec-common + 1.0-SNAPSHOT + + + + org.projectlombok + lombok + true + + + com.alibaba + fastjson + 1.2.76 + + + + org.eclipse.paho + org.eclipse.paho.client.mqttv3 + 1.2.5 + + + + + ${project.artifactId} + + + org.springframework.boot + spring-boot-maven-plugin + + + + repackage + + + + + + org.apache.maven.plugins + maven-compiler-plugin + + 11 + 11 + + + + + + + + central + ali-mirror + https://maven.aliyun.com/repository/central + + true + + + true + + + + + \ No newline at end of file diff --git a/sec-test-device/src/main/java/com/imdroid/beidou/test_device/BeidouTestApp.java b/sec-test-device/src/main/java/com/imdroid/beidou/test_device/BeidouTestApp.java new file mode 100644 index 00000000..fcf765b9 --- /dev/null +++ b/sec-test-device/src/main/java/com/imdroid/beidou/test_device/BeidouTestApp.java @@ -0,0 +1,24 @@ +package com.imdroid.beidou.test_device; + +import com.imdroid.beidou.test_device.task.BeidouDevice; +import org.springframework.boot.SpringApplication; +import org.springframework.boot.autoconfigure.SpringBootApplication; +import org.springframework.context.annotation.ComponentScan; + + +/** + * @author Layton + * @date 2023/1/31 20:33 + */ +@SpringBootApplication(scanBasePackages = {"com.imdroid"}) +@ComponentScan({"com.imdroid.*"}) +public class BeidouTestApp { + + public static void main(String[] args) { + + SpringApplication.run(BeidouTestApp.class, args); + BeidouDevice beidouDevice = new BeidouDevice(); + beidouDevice.connectServer(); + beidouDevice.run(); + } +} diff --git a/sec-test-device/src/main/java/com/imdroid/beidou/test_device/service/TCPClient.java b/sec-test-device/src/main/java/com/imdroid/beidou/test_device/service/TCPClient.java new file mode 100644 index 00000000..81ebc18e --- /dev/null +++ b/sec-test-device/src/main/java/com/imdroid/beidou/test_device/service/TCPClient.java @@ -0,0 +1,127 @@ +package com.imdroid.beidou.test_device.service; + +import com.imdroid.common.util.ThreadManager; +import io.netty.bootstrap.Bootstrap; +import io.netty.buffer.ByteBuf; +import io.netty.buffer.Unpooled; +import io.netty.channel.*; +import io.netty.channel.nio.NioEventLoopGroup; +import io.netty.channel.socket.SocketChannel; +import io.netty.channel.socket.nio.NioSocketChannel; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.nio.charset.StandardCharsets; +import java.time.LocalDateTime; +import java.util.concurrent.TimeUnit; + +public class TCPClient { + private final Logger logger = LoggerFactory.getLogger(this.getClass()); + + private String host; + + private int port; + + private Bootstrap bootstrap; + private EventLoopGroup group; + private Channel channel; + LocalDateTime connectTime = LocalDateTime.now(); + TCPListener listener; + + public void start() { + new Thread(this::connect, "forwarder tcp-client").start(); + } + + public void init(String dest_addr, int dest_port, TCPListener listener) { + this.host = dest_addr; + this.port = dest_port; + this.listener = listener; + + //客户端需要一个事件循环组 + group = new NioEventLoopGroup(); + //创建客户端启动对象 + // bootstrap 可重用, 只需在NettyClient实例化的时候初始化即可. + bootstrap = new Bootstrap(); + bootstrap.group(group) + .channel(NioSocketChannel.class) + .handler(new ChannelInitializer() { + @Override + protected void initChannel(SocketChannel ch) throws Exception { + //加入处理器 + ch.pipeline().addLast(new TcpMessageHandler(TCPClient.this)); + } + }); + } + + public void connect() { + logger.info("netty client starting"); + //启动客户端去连接服务器端 + try { + ChannelFuture cf = bootstrap.connect(host, port); + cf.addListener(new ChannelFutureListener() { + @Override + public void operationComplete(ChannelFuture future) throws Exception { + if (!future.isSuccess()) { + //重连交给后端线程执行 + future.channel().eventLoop().schedule(() -> { + logger.info("tcp client reconnect"); + try { + connect(); + } catch (Exception e) { + e.printStackTrace(); + } + }, 5000, TimeUnit.MILLISECONDS); + } else { + logger.info("tcp client start success!"); + } + } + }); + //对通道关闭进行监听 + this.channel = cf.channel(); + this.channel.closeFuture().sync(); + } catch (Exception e) { + logger.error("netty client error:", e); + } + } + + public void writeAndFlush(String json) { + ByteBuf sendBuffer = Unpooled.buffer(); + sendBuffer.writeBytes(json.getBytes(StandardCharsets.UTF_8)); + channel.writeAndFlush(sendBuffer).addListener(future -> { + if (future.isSuccess()) { + logger.info("send to tcp:"+host+" succeed."); + } else { + logger.info("send to tcp:"+host+" failed."); + if(listener!=null){ + listener.onMessage("failed"); + } + } + }); + } + + public void onConnected(){ + connectTime = LocalDateTime.now(); + } + + public void onDisconnect(){ + if(connectTime.isBefore(LocalDateTime.now().minusMinutes(1))) { + connect(); + } + else{ + ThreadManager.getScheduledThreadPool().schedule(() -> { + try { + connect(); + } catch (Exception e) { + logger.error(e.toString()); + } + },60, TimeUnit.SECONDS); + } + } + + public void onMessage(String msg){ + if(listener!=null){ + listener.onMessage(msg); + } + } + +} diff --git a/sec-test-device/src/main/java/com/imdroid/beidou/test_device/service/TCPListener.java b/sec-test-device/src/main/java/com/imdroid/beidou/test_device/service/TCPListener.java new file mode 100644 index 00000000..cf1e9d10 --- /dev/null +++ b/sec-test-device/src/main/java/com/imdroid/beidou/test_device/service/TCPListener.java @@ -0,0 +1,7 @@ +package com.imdroid.beidou.test_device.service; + +public interface TCPListener { + void onConnected(); + void onDisconnect(); + void onMessage(String msg); +} diff --git a/sec-test-device/src/main/java/com/imdroid/beidou/test_device/service/TcpMessageHandler.java b/sec-test-device/src/main/java/com/imdroid/beidou/test_device/service/TcpMessageHandler.java new file mode 100644 index 00000000..1158427c --- /dev/null +++ b/sec-test-device/src/main/java/com/imdroid/beidou/test_device/service/TcpMessageHandler.java @@ -0,0 +1,52 @@ +package com.imdroid.beidou.test_device.service; + +import io.netty.buffer.ByteBuf; +import io.netty.channel.ChannelHandlerContext; +import io.netty.channel.SimpleChannelInboundHandler; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.nio.charset.Charset; + +/** + * @author Layton + * @date 2023/2/18 20:36 + */ +public class TcpMessageHandler extends SimpleChannelInboundHandler { + + private final Logger logger = LoggerFactory.getLogger(this.getClass()); + + private final TCPClient tcpClient; + + public TcpMessageHandler(TCPClient tcpClient) { + this.tcpClient = tcpClient; + } + + @Override + protected void channelRead0(ChannelHandlerContext channelHandlerContext, ByteBuf buf) throws Exception { + String msg = buf.toString(Charset.defaultCharset()); + tcpClient.onMessage(msg); + if (logger.isDebugEnabled()) { + logger.debug("receive server message:" + msg); + } + } + + @Override + public void channelActive(ChannelHandlerContext ctx) throws Exception { + logger.info("tcp channel active"); + tcpClient.onConnected(); + } + + @Override + public void channelInactive(ChannelHandlerContext ctx) throws Exception { + logger.info("tcp channel inactive"); + tcpClient.onDisconnect(); + } + + @Override + public void exceptionCaught(ChannelHandlerContext ctx, Throwable cause) throws Exception { + logger.error("TcpMessageHandler error: {}", cause.toString()); + ctx.close(); + } + +} diff --git a/sec-test-device/src/main/java/com/imdroid/beidou/test_device/service/UDPClient.java b/sec-test-device/src/main/java/com/imdroid/beidou/test_device/service/UDPClient.java new file mode 100644 index 00000000..3ee125a8 --- /dev/null +++ b/sec-test-device/src/main/java/com/imdroid/beidou/test_device/service/UDPClient.java @@ -0,0 +1,41 @@ +package com.imdroid.beidou.test_device.service; + +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.net.DatagramPacket; +import java.net.DatagramSocket; +import java.net.InetAddress; + +public class UDPClient { + private final Logger logger = LoggerFactory.getLogger(this.getClass()); + + private DatagramSocket socket; + + private InetAddress inetAddress; + + private String host; + + private int port; + + public void init(String host, int port) { + this.host = host; + this.port = port; + try { + logger.info("UDP client init "+host + ":" + port); + this.socket = new DatagramSocket(); + this.inetAddress = InetAddress.getByName(host); + } catch (Exception e) { + logger.error("初始化udp客户端失败：", e); + } + } + + public void sendData(byte[] data) { + try { + DatagramPacket packet = new DatagramPacket(data, data.length, inetAddress, port); + socket.send(packet); + } catch (Exception e) { + logger.error("udp推送gnss数据异常:", e); + } + } +} diff --git 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 new file mode 100644 index 00000000..ed58de38 --- /dev/null +++ b/sec-test-device/src/main/java/com/imdroid/beidou/test_device/task/BeidouDevice.java @@ -0,0 +1,87 @@ +package com.imdroid.beidou.test_device.task; + +import com.imdroid.beidou.test_device.service.UDPClient; +import com.imdroid.common.util.ByteUtil; + +import java.io.BufferedReader; +import java.io.FileReader; +import java.io.IOException; +import java.text.SimpleDateFormat; +import java.util.Date; + +public class BeidouDevice { + private String host="127.0.0.1"; + + private int port=9903; + + UDPClient udpClient; + public void connectServer(){ + udpClient = new UDPClient(); + udpClient.init(host, port); + } + + public void run(){ + try{ + //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){ + + } + System.out.println("finish!"); + } + + public void execute(String dataFileName) throws IOException { + FileReader fr=null; + BufferedReader br=null; + try { + fr = new FileReader(dataFileName); + br = new BufferedReader(fr); + String line = ""; + String[] arrs = null; + SimpleDateFormat sdf = new SimpleDateFormat("yyyy-MM-dd HH:mm:ss.SSS"); + + long lastTime = 0; + int b562Count = 0; + int cycleCount = 0; + + while ((line = br.readLine()) != null) { + arrs = line.split(" "); + if(arrs.length != 3) continue; + Date time = sdf.parse(arrs[0] + " " + arrs[1]); + + if (lastTime!=0 && Math.abs(time.getTime() - lastTime) > 60 * 1000) {//超过1分钟为一个周期 + Thread.sleep(30 * 1000); + cycleCount++; + System.out.println(time+" cycle "+cycleCount+", b562 num "+b562Count); + } + udpClient.sendData(ByteUtil.hexStringTobyte(arrs[2])); + b562Count++; + lastTime = time.getTime(); + Thread.sleep(100); + } + br.close(); + fr.close(); + } + catch (Exception e){ + e.printStackTrace(); + if(br!=null) br.close(); + if(fr!=null) fr.close(); + } + } +} diff --git a/sec-test-device/src/main/resources/application.properties b/sec-test-device/src/main/resources/application.properties new file mode 100644 index 00000000..9ac62967 --- /dev/null +++ b/sec-test-device/src/main/resources/application.properties @@ -0,0 +1,22 @@ +server.port=9916 +server.servlet.context-path=/gnss + +spring.application.name=test-gnss +spring.application.build=20240106 + +spring.jpa.show-sql = true +spring.jpa.hibernate.ddl-auto = none +spring.jpa.database-platform = org.hibernate.dialect.MySQLDialect +spring.datasource.url = jdbc:mysql://localhost:3306/beidou?characterEncoding=utf8&useSSL=false&allowPublicKeyRetrieval=true&serverTimezone=Asia/Shanghai +spring.datasource.username = admin +spring.datasource.password = DBMgr_2022 +spring.datasource.driverClassName = com.mysql.cj.jdbc.Driver + +spring.jackson.dateFormat = yyyy-MM-dd HH:mm:ss +spring.jackson.time-zone = GMT+8 + +app.format.date = yyyy-MM-dd +app.format.time = HH:mm:ss +app.format.datetime = yyyy-MM-dd HH:mm:ss + +mybatis-plus.configuration.map-underscore-to-camel-case=false