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

算法7改为ECEF+判断周跳

Browse Source
This commit is contained in:
weidong 2024-11-13 19:39:58 +08:00
parent 5c4fff393d
commit 9a95f3d0aa
1 changed files with 54 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -2,13 +2,16 @@ package com.imdroid.sideslope.bd;
import com.imdroid.sideslope.sal.Device;
import static com.imdroid.sideslope.bd.GeoCoordConverterUtil.ECEF2ENU;
import static com.imdroid.sideslope.bd.GeoCoordConverterUtil.LLA2ECEF;
/**
* 博通：用GGA绝对坐标代替相对位置
*/
public class FocusCalculator7 extends FocusCalculator3{
final static long scale = 100000000L;//地球1°：111km，放大到mm乘以100,000,000
//final static long scale = 100000000L;//地球1°：111km，放大到mm乘以100,000,000
final static int bad_change_mm = 500;//固定解跳变连续10次超过500mm，认为是周跳
final static int bad_duration = 10;
@ -18,7 +21,7 @@ public class FocusCalculator7 extends FocusCalculator3{
super(bsDevice);
}
@Override
/*@Override
public void addGGA(Gga gga) {
if(gga == null) return;
@ -41,6 +44,55 @@ public class FocusCalculator7 extends FocusCalculator3{
}
else if(gga.getQuality() == 5) counterNoFixed++;
else counterNoB562 ++;
}*/
@Override
public void addGGA(Gga gga) {
if(gga == null) return;
double[] end;
// 测站的GGA - 测站 LLA 数据
GeoCoordConverterUtil.LLA_Coordinate rover_lla = new GeoCoordConverterUtil.LLA_Coordinate(gga.getLatitude(),gga.getLongitude(),gga.getAltitude());
// 测站 LLA 坐标转 ECEF 坐标，单位米
GeoCoordConverterUtil.ECEF_Coordinate rover_ecef = LLA2ECEF(rover_lla);
if(bsDevice==null || bsDevice.getEcefx()==null){
end = new double[]{
rover_ecef.getECEF_X()*1000,
rover_ecef.getECEF_Y()*1000,
rover_ecef.getECEF_Z()*1000,
gga.getQuality()};
}
else {
// 查询测站的绑定的基站
GeoCoordConverterUtil.ECEF_Coordinate reference_ecef = new GeoCoordConverterUtil.ECEF_Coordinate(bsDevice.getEcefx(), bsDevice.getEcefy(), bsDevice.getEcefz());
// 以基站为站心的 ENU 坐标
GeoCoordConverterUtil.ENU_Coordinate difference_enu = ECEF2ENU(reference_ecef, rover_ecef);
end = new double[]{
difference_enu.ENU_E*1000,
difference_enu.ENU_N*1000,
difference_enu.ENU_U*1000,
gga.getQuality()};
}
if(gga.isFixed()) {
counterFixedResult++;
if(pointList.size()>0){
double[] lastXyz = pointList.get(pointList.size()-1);
if(Math.abs(end[0]-lastXyz[0])>bad_change_mm ||
Math.abs(end[1]-lastXyz[1])>bad_change_mm ||
Math.abs(end[2]-lastXyz[2])>bad_change_mm){
bad_count++;
return;
}
}
bad_count = 0;
pointList.add(end);
}
else if(gga.getQuality() == 5) counterNoFixed++;
else counterNoB562 ++;
}
@Override