bonus
/
wrj_control
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

雷达点转经纬度优化

This commit is contained in:
fl 2025-03-11 10:08:44 +08:00
parent 94ae32e70d
commit 5d88c394c9
3 changed files with 17 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
app/src/main/java/com/bonus/wrjtest/MainActivity.kt
View File

@ -217,7 +217,6 @@ class MainActivity : AppCompatActivity() {
Toast.LENGTH_SHORT
).show()
}
"F0000001" -> {
resultDataSb.clear()
Toast.makeText(
@ -226,7 +225,6 @@ class MainActivity : AppCompatActivity() {
Toast.LENGTH_SHORT
).show()
}
"F0000002" -> {
resultDataSb.clear()
returnData.setText("激光雷达解析数据与分裂类型不匹配")
@ -1042,6 +1040,8 @@ class MainActivity : AppCompatActivity() {
sb.append("01")
sb.append("02")
sb.append("01")
val sum16 = ByteUtil.getSum16(sb.toString())
sb.append(sum16)
//保存基础数据

2
app/src/main/java/com/bonus/wrjtest/utils/MultipointSagMeasure.java
View File

@ -14,7 +14,7 @@ public class MultipointSagMeasure {
/**
* 计算测点弧垂
*
* @param span 档距
* @param list 坐标角度等
* @param pointNumber 运用几点测量公式
* @return

37
app/src/main/java/com/bonus/wrjtest/utils/RadarMathUtil.java
View File

@ -93,37 +93,28 @@ public class RadarMathUtil {
double lonGps = sensorDataBean.getLon();
double latGps = sensorDataBean.getLat();
double hGps = sensorDataBean.getH();
double azimuthAngle = sensorDataBean.getAzimuthAngle();
//2025-01-16,激光雷达角度偏差90度，修改角度
if(radarAngle>90){
radarAngle = radarAngle - 90;
}if(radarAngle<90){
radarAngle = 90 - radarAngle;
}
double azimuthAngle = sensorDataBean.getAzimuthAngle() - 15.0;
//1.RTK天线到激光雷达点转换
double disLonPoint = RTKDistance * MathUtils.cos(azimuthAngle + RTKAngle);
//激光雷达经度
double lonRadar = lonGps + disLonPoint/Rq;
//距离
double disLatPoint = RTKDistance * MathUtils.sin(azimuthAngle + RTKAngle);
double lonRadar = lonGps + RTKDistance * Math.sin((azimuthAngle+RTKAngle)/180*Math.PI)/Rq/Math.cos(latGps/180*Math.PI)*180/Math.PI;
//激光雷达纬度
double latRadar = latGps + disLatPoint/(Rq * MathUtils.cos(Math.toRadians(lonGps)));
double latRadar = latGps + (RTKDistance * Math.cos((azimuthAngle+RTKAngle)/180*Math.PI)/Rq)*180/Math.PI;
//激光雷达高程
double hRadar = hGps;
//经纬度属于弧度值，使用util需要将弧度转成角度
//距离
double disLonPoint2 = dis * MathUtils.cos(radarAngle) * MathUtils.cos(azimuthAngle + radarAngle1);
System.out.printf("雷达点：lat：%.8f; lon：%.8f; 高度：%.2f; 方位角：%.2f\n", latRadar, lonRadar, hRadar,azimuthAngle);
//1.激光雷达点到扫描点转换
//测点经度
double lon2 = lonRadar + disLonPoint2/Rq;
//距离
double disLatPoint2 = dis * MathUtils.cos(radarAngle) * MathUtils.sin(azimuthAngle + radarAngle1);
double lon2 = lonRadar + dis * Math.sin((azimuthAngle+radarAngle1)/180*Math.PI)/Rq/Math.cos(latRadar/180*Math.PI)*180/Math.PI;
//测点纬度
double lat2 = latRadar + disLatPoint2/(Rq * MathUtils.cos(Math.toRadians(lonRadar)));
double lat2 = latRadar + (dis*Math.cos((azimuthAngle+radarAngle1)/180*Math.PI)/Rq)*180/Math.PI;
//测点高程 可能为减
double h2 = hRadar + dis * MathUtils.sin(radarAngle);
//2025-01-16,激光雷达扫描角度偏差90度，修改角度
if(radarAngle>90){
radarAngle = radarAngle - 90;
}else if(radarAngle<90){
radarAngle = 90 - radarAngle;
}
double h2 = hRadar + dis * MathUtils.sin(radarAngle - sensorDataBean.getRollAngle());
return new Coordinate(lon2,lat2,h2);
}