diff --git a/app/src/main/AndroidManifest.xml b/app/src/main/AndroidManifest.xml
index ffe79bd..f395248 100644
--- a/app/src/main/AndroidManifest.xml
+++ b/app/src/main/AndroidManifest.xml
@@ -14,6 +14,8 @@
+
+
diff --git a/app/src/main/java/com/bonus/uav/service/RadarRunnable.java b/app/src/main/java/com/bonus/uav/service/RadarRunnable.java
index 71877e8..33789f7 100644
--- a/app/src/main/java/com/bonus/uav/service/RadarRunnable.java
+++ b/app/src/main/java/com/bonus/uav/service/RadarRunnable.java
@@ -198,8 +198,8 @@ public class RadarRunnable implements Runnable {
*/
List filteredList = scanDistanceData.stream()
- .filter(it -> it.getDistance() != 0.0 && it.getDistance() > 0.05 && it.getDistance() < 4.0
- && ((it.getAngle() > 14.8 && it.getAngle() < 90.3)))
+ .filter(it -> it.getDistance() != 0.0 && it.getDistance() > 1.0 && it.getDistance() < 6
+ && ((it.getAngle() > 14.8 && it.getAngle() < 80)))
.sorted(Comparator.comparing(RadarPointBean::getAngle))
.collect(Collectors.toList());
// 输出结果
diff --git a/app/src/main/java/com/bonus/uav/utils/RTKUtils.java b/app/src/main/java/com/bonus/uav/utils/RTKUtils.java
index 9c368a2..d39aeb2 100644
--- a/app/src/main/java/com/bonus/uav/utils/RTKUtils.java
+++ b/app/src/main/java/com/bonus/uav/utils/RTKUtils.java
@@ -81,9 +81,9 @@ public class RTKUtils {
//测点经度
//bearing 不转换的方位角
//天线1的经纬度
- double lon2 = lon1 + 0.389 * Math.sin(((bearing-10)+0)/180*Math.PI)/Rq/Math.cos(lat1/180*Math.PI)*180/Math.PI;
+ double lon2 = lon1 + 0.409 * Math.sin(((bearing-14.3)+0)/180*Math.PI)/Rq/Math.cos(lat1/180*Math.PI)*180/Math.PI;
//测点纬度
- double lat2 = lat1 + (0.389*Math.cos(((bearing-10)+0)/180*Math.PI)/Rq)*180/Math.PI;
+ double lat2 = lat1 + (0.409 * Math.cos(((bearing-14.3)+0)/180*Math.PI)/Rq)*180/Math.PI;
double h2 = alt1;
System.out.printf("雷达点经纬度：lat：%.8f; lon：%.8f; 高度：%.2f; 方位角：%.2f\n", lat2, lon2, h2,bearing);
return new double[]{lat2,lon2,h2};
@@ -93,7 +93,7 @@ public class RTKUtils {
* 根据两个点的经纬度，计算两点之间某点的经纬度
*/
public static double[] calculatePoint(double lat1, double lon1, double alt1,
- double lat2, double lon2, double alt2,float bearing) {
+ double lat2, double lon2, double alt2,double bearing) {
double[] ecef1 = convertToECEF(lat1, lon1, alt1);
double[] ecef2 = convertToECEF(lat2, lon2, alt2);
@@ -126,7 +126,7 @@ public class RTKUtils {
}
// 计算方位角
- public static float calculateBearing(double lat1, double lon1, double lat2, double lon2) {
+ public static double calculateBearing(double lat1, double lon1, double lat2, double lon2) {
Location antenna1 = new Location("antenna1");
antenna1.setLatitude(lat1);
antenna1.setLongitude(lon1);
@@ -135,13 +135,57 @@ public class RTKUtils {
antenna2.setLatitude(lat2);
antenna2.setLongitude(lon2);
- float du = antenna1.bearingTo(antenna2)+369;
-
-// return du-276;
-// return du-290;
+ double du = antenna1.bearingTo(antenna2)+369;
+ // 规范化处理，使结果在0到360度之间
+// double du = ((antenna1.bearingTo(antenna2) + 369) % 360);
return du;
}
+ // 计算方位角
+ public static double calculateBearing2(double lat1, double lon1, double lat2, double lon2) {
+ double d1 = lon1/180*Math.PI;
+ double d2 = lat1/180*Math.PI;
+ double d3 = lon2/180*Math.PI;
+ double d4 = lat2/180*Math.PI;
+ double y = Math.sin(-d1+d3) * Math.cos(d4);
+ double x = Math.cos(d2)*Math.sin(d4) - Math.sin(d2)*Math.cos(d4)*Math.cos(-d1+d3);
+ double du = Math.atan2(y,x) * 180/Math.PI;
+ return normalizeAngle(du);
+ }
+
+ /**
+ * 调整航向角在0-360度之间
+ * @param x
+ * @return
+ */
+ public static double normalizeAngle(double x) {
+ x = x % 360; // 先对360取模
+ if (x < 0) {
+ x += 360; // 如果结果为负，则加360度调整
+ }
+ return x;
+ }
+
+ public static double calculateBearing3(double lat1, double lon1, double lat2, double lon2) {
+ // 将纬度和经度从度数转换为弧度
+ double φ1 = Math.toRadians(lat1);
+ double φ2 = Math.toRadians(lat2);
+ double Δλ = Math.toRadians(lon2 - lon1);
+
+ // 计算Δφ和Δλ的差值
+ double y = Math.sin(Δλ) * Math.cos(φ2);
+ double x = Math.cos(φ1) * Math.sin(φ2) - Math.sin(φ1) * Math.cos(φ2) * Math.cos(Δλ);
+
+ // 计算初始方位角（以弧度为单位）
+ double bearing = Math.atan2(y, x);
+
+ // 转换为度数
+ bearing = Math.toDegrees(bearing);
+
+ // 规范化处理，使结果在0到360度之间
+ return (double) ((bearing + 360) % 360);
+ }
+
/**
* 计算两个三维点之间的方向向量并计算方位角和仰角
*/
diff --git a/app/src/main/java/com/bonus/uav/utils/SerialConnectUtils.java b/app/src/main/java/com/bonus/uav/utils/SerialConnectUtils.java
index 1d7ae94..ed17832 100644
--- a/app/src/main/java/com/bonus/uav/utils/SerialConnectUtils.java
+++ b/app/src/main/java/com/bonus/uav/utils/SerialConnectUtils.java
@@ -177,6 +177,35 @@ public class SerialConnectUtils {
});
}
+ public static void main(String[] args) {
+ double lonC=117.16341925;
+ double latC=31.78579748;
+ double altC=0.0;
+ double lon=117.16341193;
+ double lat=31.78579944;
+ double alt=0.0;
+ // 计算方位角
+ double bearing = RTKUtils.calculateBearing2(latC, lonC, lat, lon);
+ System.out.println("BBB雷达方位角: " + bearing + "°");
+// double[] midPoint = RTKUtils.calculatePoint(lat, lon, alt, latC, lonC, altC, bearing);
+ double[] midPoint = RTKUtils.calculateMidPoint2(latC, lonC, altC, bearing);
+ System.out.println("雷达点纬度: " + midPoint[0]);
+ System.out.println("雷达点经度: " + midPoint[1]);
+ double midPointHight = midPoint[2];
+ System.out.println("雷达点高度: " + midPoint[2]);
+ //所以传感器结果
+ SensorDataBean sensorDataBean = new SensorDataBean();
+ sensorDataBean.setLon(midPoint[1]);
+ sensorDataBean.setLat(midPoint[0]);
+ sensorDataBean.setH(midPointHight);
+ sensorDataBean.setAzimuthAngle(bearing);
+ System.out.printf("RTK：lat：%.8f; lon：%.8f; 高度：%.2f\n", sensorDataBean.getLat(), sensorDataBean.getLon(), sensorDataBean.getH());
+ RadarPointBean radarPointBean = new RadarPointBean();
+ radarPointBean.setDistance(1.53);
+ radarPointBean.setAngle(52.85);
+ Coordinate coordinate = RadarMathUtil.transferPoint2(radarPointBean, sensorDataBean);
+ }
+
/**
* 传感器返回值回调
*
@@ -277,21 +306,25 @@ public class SerialConnectUtils {
}
if (latC != 0.0 && lonC != 0.0 && lat != 0.0 && lon != 0.0){
//主天线、从天线都有数据 先计算中点坐标
- // 计算中心点
-// double[] midPoint = RTKUtils.calculateMidPoint(lat, lon, alt, latC, lonC, altC);
-// System.out.println("中心点纬度: " + midPoint[0]);
-// System.out.println("中心点经度: " + midPoint[1]);
-// System.out.println("中心点高度: " + midPoint[2]);
// 计算方位角
- float bearing = RTKUtils.calculateBearing(lat, lon, latC, lonC);
+ double bearing = RTKUtils.calculateBearing2(lat, lon, latC, lonC);
System.out.println("BBB雷达方位角: " + bearing + "°");
logger.log("ABCD雷达方位角：", bearing+ "°");
- double[] midPoint = RTKUtils.calculatePoint(lat, lon, alt, latC, lonC, altC, bearing);
- RTKUtils.calculateMidPoint2(lat, lon, alt,bearing);
+ // 算法一，计算中心点在偏移高差计算雷达点。
+// double[] midPoint = RTKUtils.calculateMidPoint(lat, lon, alt, latC, lonC, altC);
+// System.out.println("中心点纬度: " + midPoint[0]);
+// System.out.println("中心点经度: " + midPoint[1]);
+// double midPointHight = midPoint[2] - 0.3;
+// System.out.println("中心点高度: " + midPointHight);
+
+ // 算法二，以天线一与雷达方位计算雷达点。
+// double[] midPoint = RTKUtils.calculatePoint(lat, lon, alt, latC, lonC, altC, bearing);
+ double[] midPoint = RTKUtils.calculateMidPoint2(lat, lon, alt,bearing);
System.out.println("雷达点纬度: " + midPoint[0]);
System.out.println("雷达点经度: " + midPoint[1]);
+ double midPointHight = midPoint[2];
System.out.println("雷达点高度: " + midPoint[2]);
logger.log("ABCD雷达点经纬度：", "纬度：" + midPoint[0] + ";经度:" + midPoint[1] + ";海拔:" + midPoint[2]);
@@ -301,7 +334,7 @@ public class SerialConnectUtils {
sensorDataBean.setLon(midPoint[1]);
sensorDataBean.setLat(midPoint[0]);
- sensorDataBean.setH(midPoint[2]);
+ sensorDataBean.setH(midPointHight);
sensorDataBean.setPositioningMode(qual);
sensorDataBean.setAzimuthAngle(bearing);
}
@@ -515,8 +548,6 @@ public class SerialConnectUtils {
assert serialHelper != null;
Log.e(TAG, "2.测量数据返回：" + sb.toString());
serialHelper.sendHex(sb.toString());
-
-
// else {
// SerialHelper serialHelper = serialHelperMap.get("数传");
// assert serialHelper != null;
diff --git a/app/src/main/java/com/bonus/uav/utils/sag/RadarMathUtil.java b/app/src/main/java/com/bonus/uav/utils/sag/RadarMathUtil.java
index 9c1e043..9d32c1f 100644
--- a/app/src/main/java/com/bonus/uav/utils/sag/RadarMathUtil.java
+++ b/app/src/main/java/com/bonus/uav/utils/sag/RadarMathUtil.java
@@ -5,6 +5,9 @@ import com.bonus.uav.entity.RadarPointBean;
import com.bonus.uav.entity.SensorDataBean;
import com.bonus.uav.utils.math.MathUtils;
+import java.math.BigDecimal;
+import java.math.RoundingMode;
+
public class RadarMathUtil {
/**
@@ -22,8 +25,8 @@ public class RadarMathUtil {
/**
* 激光雷达与测点连线与正北方向偏角 顺时针为正
*/
-// private static final double radarAngle1 = -90 ;
- private static final double radarAngle1 = 0 ;
+ private static final double radarAngle1 = 90 ;
+// private static final double radarAngle1 = 0 ;
/**
* 3）、测点经纬度计算(pdf)
@@ -86,12 +89,12 @@ public class RadarMathUtil {
public static Coordinate transferPoint2(RadarPointBean radarPointBean, SensorDataBean sensorDataBean) {
double radarAngle = radarPointBean.getAngle();
double dis = radarPointBean.getDistance();
-
+ dis = dis * Math.sin(Math.toRadians(radarAngle));
double lonGps = sensorDataBean.getLon();
double latGps = sensorDataBean.getLat();
double hGps = sensorDataBean.getH();
// double azimuthAngle = sensorDataBean.getAzimuthAngle() - 15.0;
- double azimuthAngle = sensorDataBean.getAzimuthAngle()+90;
+ double azimuthAngle = sensorDataBean.getAzimuthAngle();
// //1.RTK天线到激光雷达点转换
// //激光雷达经度
// double lonRadar = lonGps + RTKDistance * Math.sin((azimuthAngle+RTKAngle)/180*Math.PI)/Rq/Math.cos(latGps/180*Math.PI)*180/Math.PI;
@@ -100,8 +103,6 @@ public class RadarMathUtil {
// //激光雷达高程
// double hRadar = hGps;
// System.out.printf("雷达点：lat：%.8f; lon：%.8f; 高度：%.2f; 方位角：%.2f\n", latRadar, lonRadar, hRadar,azimuthAngle);
-
-
double lonRadar=lonGps;
double latRadar=latGps;
double hRadar = hGps;
@@ -112,13 +113,13 @@ public class RadarMathUtil {
//测点纬度
double lat2 = latRadar + (dis*Math.cos((azimuthAngle+radarAngle1)/180*Math.PI)/Rq)*180/Math.PI;
//测点高程 可能为减
- //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());
+ //2025-01-16,激光雷达扫描角度偏差90度，修改角度,2025-03-27换成cos了不需要换算补角了
+// if(radarAngle>90){
+// radarAngle = radarAngle - 90;
+// }else if(radarAngle<90){
+// radarAngle = 90 - radarAngle;
+// }
+ double h2 = hRadar + dis * MathUtils.cos(radarAngle - sensorDataBean.getRollAngle());
System.out.printf("ABC中心雷达点：lat：%.8f; lon：%.8f; 高度：%.2f; 方位角：%.2f\n", latRadar, lonRadar, hRadar,azimuthAngle);
System.out.printf("ABC测量点：lat：%.8f; lon：%.8f; 高度：%.2f\n", lat2, lon2, h2);
return new Coordinate(lon2,lat2,h2);
@@ -206,5 +207,11 @@ public class RadarMathUtil {
return Math.pow(l,2)* Math.abs(k)/4;
}
+ public static double keepDouble(double number,int num) {
+ BigDecimal bd = new BigDecimal(Double.toString(number));
+ bd = bd.setScale(num, RoundingMode.HALF_UP);
+ return bd.doubleValue();
+ }
+
}