方位角计算优化

app/src/main/AndroidManifest.xml

@@ -14,6 +14,8 @@
     <uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE" />
     <uses-permission android:name="android.permission.INTERNET" />
     <uses-permission android:name="android.permission.ACCESS_NETWORK_STATE" />
+    <uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE"/>
+    <uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE"/>
 
     <uses-permission android:name="android.permission.BIND_JOB_SERVICE"
         tools:ignore="ProtectedPermissions" />

app/src/main/java/com/bonus/uav/service/RadarRunnable.java

@@ -198,8 +198,8 @@ public class RadarRunnable implements Runnable {
          */
 
         List<RadarPointBean> filteredList = scanDistanceData.stream()
-                .filter(it -> it.getDistance() != 0.0 && it.getDistance() > 0.05 && it.getDistance() < 4.0
+                .filter(it -> it.getDistance() != 0.0 && it.getDistance() > 1.0 && it.getDistance() < 6
-                        && ((it.getAngle() > 14.8 && it.getAngle() < 90.3)))
+                        && ((it.getAngle() > 14.8 && it.getAngle() < 80)))
                 .sorted(Comparator.comparing(RadarPointBean::getAngle))
                 .collect(Collectors.toList());
         // 输出结果

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;
+        double du = antenna1.bearingTo(antenna2)+369;
-
+        // 规范化处理，使结果在0到360度之间
-//        return du-276;
+//        double du = ((antenna1.bearingTo(antenna2) + 369) % 360);
-//        return du-290;
         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);
+    }
+
     /**
      * 计算两个三维点之间的方向向量并计算方位角和仰角
      */

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;

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 = 90 ;
-    private static final double radarAngle1 = 0 ;
+//    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度，修改角度
+        //2025-01-16,激光雷达扫描角度偏差90度，修改角度,2025-03-27换成cos了不需要换算补角了
-        if(radarAngle>90){
+//        if(radarAngle>90){
-            radarAngle = radarAngle - 90;
+//            radarAngle = radarAngle - 90;
-        }else if(radarAngle<90){
+//        }else if(radarAngle<90){
-            radarAngle = 90 - radarAngle;
+//            radarAngle = 90 - radarAngle;
-        }
+//        }
-        double h2 = hRadar + dis * MathUtils.sin(radarAngle - sensorDataBean.getRollAngle());
+        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();
+    }
+
 
 }