Imdroid/security-monitor
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

1、针对遮挡严重的场景做了优化

Browse Source 
2、增加模拟终端子模块，便于算法验证
This commit is contained in:
weidong 2024-04-17 15:31:33 +08:00
parent c88d37c072
commit 40ae325fe6
23 changed files with 836 additions and 647 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
pom.xml
View File

@ -14,6 +14,7 @@
<module>sec-beidou-rtcm</module>
<module>sec-api</module>
<module>sec-beidou-fwd</module>
<module>sec-test-device</module>
</modules>
<properties>

3
sec-api/src/main/java/com/imdroid/secapi/client/RtcmClient.java
View File

@ -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();
}

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

@ -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();
}

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

@ -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<double[]> list = 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 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 final double shockThreshold = 1.5;
private int counterNoB562 = 0;
private int counterNoFixed = 0;
private int counterFixedResult = 0;
// b562算法相关：b562固定解的点、计算球心初始半径、迭代步长、最少点数
final List<double[]> 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<Tilt> 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<Point> results = new ArrayList<>();
if (r9250Result != null && r9250Result.length > 0){
if(pointList.size() >= gravityMinCount){
// 筛选出离上次解算结果最近的50个点计算初始球心
// 如果没有上次位置，选最后50个点
// 为便于排序，使用Point对象
List<Point> 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<double[]> pointList;
if(globeProportion(focus,r) < pointSelectedRate){
do{
iterNum++;
r += 0.1;
r += iterStep;
//球附近点集个数有可能为0，继续计算会导致focus结果出现NaN
List<double[]> 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<double[]> list){
double[] focusPoint(List<double[]> 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<Point> list){
double[] focusPointObj(List<Point> 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<double[]> globeFilter(double[] focus, double r){
List<double[]> globeFilter(double[] focus, double r){
ArrayList<double[]> 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<Tilt> iterator = tilts.iterator();
int size = tilts.size();
double sumP = 0;
double sumR = 0;
double sumY = 0;
while (iterator.hasNext()){
double roll = 0;
double pitch = 0;
double yaw = 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);
roll += tilt.getRoll();
pitch += tilt.getPitch();
yaw += tilt.getYaw();
}
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();
}
return null;
}
@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;
}
}

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

@ -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<LocalDateTime> pointTimeList = Collections.synchronizedList(new ArrayList<>());
private final List<double[]> list = Collections.synchronizedList(new ArrayList<>());
/**
* 构建计算器，如果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;
}
private final List<Point> pointList = new ArrayList<>();
private int counterNoB562 = 0;
private int counterNoFixed = 0;
private int counterFixedResult = 0;
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<LocalDateTime> iterPointTime = pointTimeList.iterator();
Iterator<double[]> 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<Double> 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<Point> 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<Point> 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;
}
}

31
sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSCalcFilterService.java
View File

@ -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<GnssCalcData> query = new QueryWrapper<>();

18
sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/GNSSDataCalcService.java
View File

@ -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);
}

39
sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/calc/MultiLineGNSSCalcService.java
View File

@ -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<String, D342Time> deviceMap = new ConcurrentHashMap<>();
private static final Map<String, ResendRecord> 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<GnssGroupCalc> 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");
}
}

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

@ -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<String, FocusCalculator1> calculatorMap = new ConcurrentHashMap<>();
private static final Map<String, FocusCalculator> calculatorMap = new ConcurrentHashMap<>();
private static final Map<String, ScheduledFuture<?>> timerMap = new ConcurrentHashMap<>();
@ -36,8 +39,6 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
private static final Map<String, Boolean> cleanTiltStatusMap = new ConcurrentHashMap<>();
private static final Map<String, Boolean> 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<GnssGroupCalc> 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,30 +167,25 @@ 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("融合值异常");
}
}
focusCalculator.reset();
calculatorMap.remove(deviceId);
}
private void postLocationRecord(String deviceId, double[] b562Result, double[] r9250Result, double[] result,
LocalDateTime resultTime, int delay, boolean isShocked) {
}*/
if (b562Result != null) {
positionMap.put(deviceId, b562Result);
GnssCalcData locationRecord = new GnssCalcData();
locationRecord.setCreatetime(resultTime);
locationRecord.setUpdatetime(LocalDateTime.now()); //通过这里可以区分补传记录
@ -182,29 +201,28 @@ public class SingleLineGNSSCalcService implements GNSSDataCalcService {
locationRecord.setR9250n(r9250Result[1]);
locationRecord.setR9250d(r9250Result[2]);
}
locationRecord.setPps(delay);
gnssCalcFilterService.calc(locationRecord, isShocked);
/*
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());
}
@Override
public Tilt calcAvgTilt(String deviceId) {
FocusCalculator1 focusCalculator = calculatorMap.get(deviceId);
if (focusCalculator != null) {
return focusCalculator.avgTilt();
}
return null;
// 检查b562有效数，如果过少，产生告警
warningService.checkB562Num(deviceId, tenantId,
focusCalculator.getB562Stat());
focusCalculator.reset();
//calculatorMap.remove(deviceId);
}
/***
* 清零：不需要，滤波窗口是按时间来选取历史数据的，只要超过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);
}*/
public void refreshGroupCalc(){
groupCalcList = groupCalcMapper.selectList(null);
logger.info("group paras changed");
}
}

16
sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/web/ApiController.java
View File

@ -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();

188
sec-beidou-rtcm/src/test/java/FocusCalculator3.java
View File

@ -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<double[]> list = Collections.synchronizedList(new ArrayList<>());
private final List<Double> listE = Collections.synchronizedList(new ArrayList<>());
private final List<Double> listN = Collections.synchronizedList(new ArrayList<>());
private final List<Double> listD = Collections.synchronizedList(new ArrayList<>());
private final List<Point> 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<Point> 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<l.length;i++){
//P.set(i,i,1/l[i]);
P.set(i,i,1);
}
double x_prev=0;
double x0=0;
for(int i=0;i<l.length;i++){
x0 += l[i];
}
x0 /= l.length;
L = L.minus(x0);
SimpleMatrix W,N,X,V,Q,Qvv;
SimpleMatrix sigma0;
double s0;
double k0=1.0, k1=5, k=1.0;
int iterNum = 0;
while(iterNum<10){
W = B.transpose().mult(P).mult(L); //W=B'*P*l;
N = B.transpose().mult(P).mult(B); //N=B'*P*B;
X = N.invert().mult(W); //x=inv(N)*W;% 待估参数向量
V = B.mult(X).minus(L); //v=B*x-l;% 残差向量
sigma0 = V.transpose().mult(P).mult(V).divide((l.length-1));
s0 = Math.sqrt(sigma0.get(0,0));//sigma0=sqrt(v'*P*v/(10-1));% 单位权中误差
//System.out.println(X);
//System.out.println(s0);
Q = P.invert();//Q=inv(P);
Qvv = Q.minus(B.mult(N.invert().mult(B.transpose())));//Qvv=Q-B*inv(N)*B';
for(int i=0; i<l.length; i++){
double v = V.get(i)/(s0*Math.sqrt(Qvv.get(i,i))); //v_=v(i)/(sigma0*sqrt(Qvv(i,i)));
if(Math.abs(v) < k0){
k = 1.0;
}
else if(Math.abs(v) > 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);
}
}

3
sec-beidou-rtcm/src/test/java/FocusCalculatorTest.java
View File

@ -22,8 +22,7 @@ import java.util.ListIterator;
classes = {
FocusCalculatorTest.class,
FocusCalculator1.class,
FocusCalculator2.class,
FocusCalculator3.class
FocusCalculator2.class
}
)

14
sec-beidou/src/main/java/com/imdroid/beidou/controller/GnssGroupController.java
View File

@ -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();
}
}
}

2
sec-beidou/src/main/java/com/imdroid/beidou/task/DatasetCleaner.java
View File

@ -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);

6
sec-beidou/src/main/resources/templates/page/gnss_data_calc.html
View File

@ -92,9 +92,9 @@
{field: 'rpose', title: '相对东', templet: "<div>{{d.rpose==null?'':d.rpose.toFixed(2)}}</div>"},
{field: 'rposn', title: '相对北', templet: "<div>{{d.rposn==null?'':d.rposn.toFixed(2)}}</div>"},
{field: 'rposd', title: '相对天', templet: "<div>{{d.rposd==null?'':d.rposd.toFixed(2)}}</div>"},
{field: 'auxe', title: '辅助东', templet: "<div>{{d.auxe==null?'':d.auxe.toFixed(2)}}</div>"},
{field: 'auxn', title: '辅助北', templet: "<div>{{d.auxn==null?'':d.auxn.toFixed(2)}}</div>"},
{field: 'auxd', title: '辅助天', templet: "<div>{{d.auxd==null?'':d.auxd.toFixed(2)}}</div>"},
//{field: 'auxe', title: '辅助东', templet: "<div>{{d.auxe==null?'':d.auxe.toFixed(2)}}</div>"},
//{field: 'auxn', title: '辅助北', templet: "<div>{{d.auxn==null?'':d.auxn.toFixed(2)}}</div>"},
//{field: 'auxd', title: '辅助天', templet: "<div>{{d.auxd==null?'':d.auxd.toFixed(2)}}</div>"},
{field: 'enabled', title: '有效',templet: '#enabledTrans'},
{field: 'pps', title: '平均延迟'}
];

128
sec-test-device/pom.xml Normal file
View File

@ -0,0 +1,128 @@
<?xml version="1.0" encoding="UTF-8"?>
<project xmlns="http://maven.apache.org/POM/4.0.0"
xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:schemaLocation="http://maven.apache.org/POM/4.0.0 http://maven.apache.org/xsd/maven-4.0.0.xsd">
<modelVersion>4.0.0</modelVersion>
<parent>
<groupId>com.imdroid</groupId>
<artifactId>security-monitor</artifactId>
<version>1.0-SNAPSHOT</version>
</parent>
<artifactId>sec-test-device</artifactId>
<properties>
<maven.compiler.source>11</maven.compiler.source>
<maven.compiler.target>11</maven.compiler.target>
<project.build.sourceEncoding>UTF-8</project.build.sourceEncoding>
</properties>
<dependencies>
<dependency>
<groupId>org.springframework.boot</groupId>
<artifactId>spring-boot-starter-web</artifactId>
</dependency>
<dependency>
<groupId>org.springframework.boot</groupId>
<artifactId>spring-boot-starter-test</artifactId>
<scope>test</scope>
</dependency>
<dependency>
<groupId>org.springframework.boot</groupId>
<artifactId>spring-boot-devtools</artifactId>
<optional>true</optional>
</dependency>
<dependency>
<groupId>org.slf4j</groupId>
<artifactId>slf4j-api</artifactId>
</dependency>
<dependency>
<groupId>ch.qos.logback</groupId>
<artifactId>logback-classic</artifactId>
</dependency>
<dependency>
<groupId>io.netty</groupId>
<artifactId>netty-all</artifactId>
<version>4.1.78.Final</version>
</dependency>
<dependency>
<groupId>com.google.code.gson</groupId>
<artifactId>gson</artifactId>
<version>2.9.1</version>
</dependency>
<dependency>
<groupId>org.apache.httpcomponents</groupId>
<artifactId>httpclient</artifactId>
<version>4.5.13</version>
</dependency>
<dependency>
<groupId>com.imdroid</groupId>
<artifactId>sec-api</artifactId>
<version>1.0-SNAPSHOT</version>
</dependency>
<dependency>
<groupId>com.imdroid</groupId>
<artifactId>sec-common</artifactId>
<version>1.0-SNAPSHOT</version>
</dependency>
<dependency>
<groupId>org.projectlombok</groupId>
<artifactId>lombok</artifactId>
<optional>true</optional>
</dependency>
<dependency>
<groupId>com.alibaba</groupId>
<artifactId>fastjson</artifactId>
<version>1.2.76</version>
</dependency>
<!--mqtt相关依赖-->
<dependency>
<groupId>org.eclipse.paho</groupId>
<artifactId>org.eclipse.paho.client.mqttv3</artifactId>
<version>1.2.5</version>
</dependency>
</dependencies>
<build>
<finalName>${project.artifactId}</finalName>
<plugins>
<plugin>
<groupId>org.springframework.boot</groupId>
<artifactId>spring-boot-maven-plugin</artifactId>
<executions>
<execution>
<goals>
<goal>repackage</goal>
</goals>
</execution>
</executions>
</plugin>
<plugin>
<groupId>org.apache.maven.plugins</groupId>
<artifactId>maven-compiler-plugin</artifactId>
<configuration>
<source>11</source>
<target>11</target>
</configuration>
</plugin>
</plugins>
</build>
<repositories>
<repository>
<id>central</id>
<name>ali-mirror</name>
<url>https://maven.aliyun.com/repository/central</url>
<releases>
<enabled>true</enabled>
</releases>
<snapshots>
<enabled>true</enabled>
</snapshots>
</repository>
</repositories>
</project>

24
sec-test-device/src/main/java/com/imdroid/beidou/test_device/BeidouTestApp.java Normal file
View File

@ -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();
}
}

127
sec-test-device/src/main/java/com/imdroid/beidou/test_device/service/TCPClient.java Normal file
View File

@ -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<SocketChannel>() {
@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);
}
}
}

7
sec-test-device/src/main/java/com/imdroid/beidou/test_device/service/TCPListener.java Normal file
View File

@ -0,0 +1,7 @@
package com.imdroid.beidou.test_device.service;
public interface TCPListener {
void onConnected();
void onDisconnect();
void onMessage(String msg);
}

52
sec-test-device/src/main/java/com/imdroid/beidou/test_device/service/TcpMessageHandler.java Normal file
View File

@ -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<ByteBuf> {
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();
}
}

41
sec-test-device/src/main/java/com/imdroid/beidou/test_device/service/UDPClient.java Normal file
View File

@ -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);
}
}
}

87
sec-test-device/src/main/java/com/imdroid/beidou/test_device/task/BeidouDevice.java Normal file
View File

@ -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();
}
}
}

22
sec-test-device/src/main/resources/application.properties Normal file
View File

@ -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