From ff28c74a5b3210cd8dd6a7d1625b2118543ebb03 Mon Sep 17 00:00:00 2001 From: hayu <1604366271@qq.com> Date: Sun, 27 Apr 2025 16:15:44 +0800 Subject: [PATCH] =?UTF-8?q?1=E3=80=81=E5=85=AD=E5=88=86=E8=A3=82=E5=AF=BC?= =?UTF-8?q?=E7=BA=BF=E8=AF=86=E5=88=AB=202=E3=80=81=E5=A2=9E=E5=8A=A0?= =?UTF-8?q?=E6=97=A0=E4=BA=BA=E6=9C=BA=E6=82=AC=E5=81=9C=E5=A4=A9=E7=BA=BF?= =?UTF-8?q?=E6=97=A5=E5=BF=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../bonus/uav/utils/SerialConnectUtils.java | 491 ++++++++++++++---- 1 file changed, 378 insertions(+), 113 deletions(-) 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 5997134..13ac713 100644 --- a/app/src/main/java/com/bonus/uav/utils/SerialConnectUtils.java +++ b/app/src/main/java/com/bonus/uav/utils/SerialConnectUtils.java @@ -22,6 +22,7 @@ import com.bonus.uav.utils.sag.RadarMathUtil; import java.io.IOException; import java.nio.charset.StandardCharsets; import java.util.ArrayList; +import java.util.Arrays; import java.util.Collections; import java.util.Comparator; import java.util.HashMap; @@ -559,6 +560,10 @@ public class SerialConnectUtils { } else { //给导线编号 classifyAndSortCoordinates(coordinates, true, re); + if (coordinates.get(0).getWireNum() == 7){ + //不符合已知规律 + radarStatus = false; + } } } @@ -569,7 +574,7 @@ public class SerialConnectUtils { sb.append("03"); //雷达数据 if(radarStatus){ - for (int m = 0; m < (Math.min(coordinates.size(), 4)); m++) { + for (int m = 0; m < (Math.min(coordinates.size(), 6)); m++) { Coordinate coordinate = coordinates.get(m); int distance = (int) (ArithUtil.round(coordinate.getRadarPoint().getDistance(), 3) * 1000); int angle = (int) (ArithUtil.round(coordinate.getRadarPoint().getAngle(), 2) * 100); @@ -577,12 +582,12 @@ public class SerialConnectUtils { sb.append(ByteUtil.decIntToHexStr(angle, 2)); } // 如果 coordinates.size() 小于 4，填充 "0000" - int remaining = 4 - coordinates.size(); + int remaining = 6 - coordinates.size(); for (int i = 0; i < remaining; i++) { sb.append("00000000"); } }else{ - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 6; i++) { sb.append("00000000"); } } @@ -733,8 +738,8 @@ public class SerialConnectUtils { */ public static void classifyAndSortCoordinates(List coordinateList, boolean reverse,int type) { int size = coordinateList.size(); - if (size < 1 || size > 4) { - throw new IllegalArgumentException("列表中的元素数量必须为1到4"); + if (size < 1 || size > 6) { + throw new IllegalArgumentException("列表中的元素数量必须为1到6"); } // 如果只有1个或2个元素，按distance大小编号，并根据reverse决定是否反转 if (size == 1) { @@ -742,14 +747,6 @@ public class SerialConnectUtils { } else if (size == 2) { Coordinate first = coordinateList.get(0); Coordinate second = coordinateList.get(1); -// if (first.getRadarPoint().getDistance() > second.getRadarPoint().getDistance()) { -// first.setWireNum(1); -// second.setWireNum(2); -// } else { -// first.setWireNum(2); -// second.setWireNum(1); -// } - switch (uavPosition) { case "大号在左": if (first.getRadarPoint().getAngle() c.setWireNum(3)); } break; - case "大号在右": if (type == 2) { // 无人机在上面 @@ -872,104 +868,96 @@ public class SerialConnectUtils { } break; } + } else if (size == 6) { + //六分裂 + switch (uavPosition) { + case "大号在左": + // 判断数据符合哪一条规律 + int matchedPattern = matchPattern(coordinateList,0); + // 输出结果 + if (matchedPattern != -1) { + if (matchedPattern == 1) { + //符合第一条规律 + coordinateList.get(0).setWireNum(1); + coordinateList.get(1).setWireNum(2); + coordinateList.get(2).setWireNum(3); + coordinateList.get(3).setWireNum(5); + coordinateList.get(4).setWireNum(4); + coordinateList.get(5).setWireNum(6); + } else if (matchedPattern == 2){ + //符合第二条规律 + coordinateList.get(0).setWireNum(2); + coordinateList.get(1).setWireNum(1); + coordinateList.get(2).setWireNum(5); + coordinateList.get(3).setWireNum(3); + coordinateList.get(4).setWireNum(6); + coordinateList.get(5).setWireNum(4); + } else if (matchedPattern == 3){ + //符合第三条规律 + coordinateList.get(0).setWireNum(2); + coordinateList.get(1).setWireNum(5); + coordinateList.get(2).setWireNum(1); + coordinateList.get(3).setWireNum(6); + coordinateList.get(4).setWireNum(3); + coordinateList.get(5).setWireNum(4); + } + System.out.println("六分裂-输入数据符合第" + matchedPattern + "条规律"); + } else { + System.out.println("输入数据不符合任何已知规律"); + coordinateList.get(0).setWireNum(7); + coordinateList.get(1).setWireNum(8); + coordinateList.get(2).setWireNum(9); + coordinateList.get(3).setWireNum(10); + coordinateList.get(4).setWireNum(11); + coordinateList.get(5).setWireNum(12); + } + break; + case "大号在右": + // 判断数据符合哪一条规律 + int matchedPattern2 = matchPattern(coordinateList,1); + // 输出结果 + if (matchedPattern2 != -1) { + if (matchedPattern2 == 1) { + //符合第一条规律 + coordinateList.get(0).setWireNum(5); + coordinateList.get(1).setWireNum(2); + coordinateList.get(2).setWireNum(6); + coordinateList.get(3).setWireNum(1); + coordinateList.get(4).setWireNum(4); + coordinateList.get(5).setWireNum(3); + } else if (matchedPattern2 == 2){ + //符合第二条规律 + coordinateList.get(0).setWireNum(5); + coordinateList.get(1).setWireNum(6); + coordinateList.get(2).setWireNum(2); + coordinateList.get(3).setWireNum(4); + coordinateList.get(4).setWireNum(1); + coordinateList.get(5).setWireNum(3); + } else if (matchedPattern2 == 3){ + //符合第三条规律 + coordinateList.get(0).setWireNum(6); + coordinateList.get(1).setWireNum(5); + coordinateList.get(2).setWireNum(4); + coordinateList.get(3).setWireNum(2); + coordinateList.get(4).setWireNum(3); + coordinateList.get(5).setWireNum(1); + } + System.out.println("六分裂-输入数据符合第" + matchedPattern2 + "条规律"); + } else { + System.out.println("输入数据不符合任何已知规律"); + coordinateList.get(0).setWireNum(7); + coordinateList.get(1).setWireNum(8); + coordinateList.get(2).setWireNum(9); + coordinateList.get(3).setWireNum(10); + coordinateList.get(4).setWireNum(11); + coordinateList.get(5).setWireNum(12); + } + break; + default: + break; + } - //-----------2025-03-30,将以下代码注释----------- -// // 如果有4个元素，按要求编号 -// int maxDistanceIndex = 0, minDistanceIndex = 0; -// int remainingIndex1 = -1, remainingIndex2 = -1; -// // 找出distance最大的和最小的点 -// for (int i = 1; i < 4; i++) { -// if (coordinateList.get(i).getRadarPoint().getDistance() > coordinateList.get(maxDistanceIndex).getRadarPoint().getDistance()) { -// maxDistanceIndex = i; -// } -// if (coordinateList.get(i).getRadarPoint().getDistance() < coordinateList.get(minDistanceIndex).getRadarPoint().getDistance()) { -// minDistanceIndex = i; -// } -// } -// // 剩余的两个点 -// for (int i = 0; i < 4; i++) { -// if (i != maxDistanceIndex && i != minDistanceIndex) { -// if (remainingIndex1 == -1) { -// remainingIndex1 = i; -// } else { -// remainingIndex2 = i; -// } -// } -// } -// // 根据h值确定剩余两个元素的编号 -// int highHIndex = coordinateList.get(remainingIndex1).getH() > coordinateList.get(remainingIndex2).getH() ? remainingIndex1 : remainingIndex2; -// int lowHIndex = coordinateList.get(remainingIndex1).getH() > coordinateList.get(remainingIndex2).getH() ? remainingIndex2 : remainingIndex1; -// if(reverse) { -// //0-90度 -// coordinateList.get(highHIndex).setWireNum(4); -// coordinateList.get(maxDistanceIndex).setWireNum(1); -// coordinateList.get(lowHIndex).setWireNum(2); -// coordinateList.get(minDistanceIndex).setWireNum(3); -// }else{ -// coordinateList.get(highHIndex).setWireNum(1); -// coordinateList.get(maxDistanceIndex).setWireNum(2); -// coordinateList.get(lowHIndex).setWireNum(3); -// coordinateList.get(minDistanceIndex).setWireNum(4); -// } - //-----------结束----------- } - - - -// //右侧 -// if (size == 1) { -// coordinateList.get(0).setWireNum(1); -// } else if (size == 2) { -// Coordinate first = coordinateList.get(0); -// Coordinate second = coordinateList.get(1); -// if (first.getRadarPoint().getDistance() > second.getRadarPoint().getDistance()) { -// first.setWireNum(1); -// second.setWireNum(2); -// } else { -// first.setWireNum(2); -// second.setWireNum(1); -// } -// }else if (size == 4) { -// // 如果有4个元素，按要求编号 -// int maxDistanceIndex = 0, minDistanceIndex = 0; -// int remainingIndex1 = -1, remainingIndex2 = -1; -// // 找出distance最大的和最小的点 -// for (int i = 1; i < 4; i++) { -// if (coordinateList.get(i).getRadarPoint().getDistance() > coordinateList.get(maxDistanceIndex).getRadarPoint().getDistance()) { -// maxDistanceIndex = i; -// } -// if (coordinateList.get(i).getRadarPoint().getDistance() < coordinateList.get(minDistanceIndex).getRadarPoint().getDistance()) { -// minDistanceIndex = i; -// } -// } -// // 剩余的两个点 -// for (int i = 0; i < 4; i++) { -// if (i != maxDistanceIndex && i != minDistanceIndex) { -// if (remainingIndex1 == -1) { -// remainingIndex1 = i; -// } else { -// remainingIndex2 = i; -// } -// } -// } -// // 根据h值确定剩余两个元素的编号 -// int highHIndex = coordinateList.get(remainingIndex1).getH() > coordinateList.get(remainingIndex2).getH() ? remainingIndex1 : remainingIndex2; -// int lowHIndex = coordinateList.get(remainingIndex1).getH() > coordinateList.get(remainingIndex2).getH() ? remainingIndex2 : remainingIndex1; -// if(reverse) { -// //0-90度 -// coordinateList.get(highHIndex).setWireNum(1); -// coordinateList.get(maxDistanceIndex).setWireNum(4); -// coordinateList.get(lowHIndex).setWireNum(3); -// coordinateList.get(minDistanceIndex).setWireNum(2); -// }else{ -// coordinateList.get(highHIndex).setWireNum(4); -// coordinateList.get(maxDistanceIndex).setWireNum(3); -// coordinateList.get(lowHIndex).setWireNum(2); -// coordinateList.get(minDistanceIndex).setWireNum(1); -// } -// } - - // 按编号排序 Collections.sort(coordinateList, Comparator.comparingInt(Coordinate::getWireNum)); } @@ -1069,7 +1057,7 @@ public class SerialConnectUtils { public static Double getXyData(String result) { String str = ""; String firstChar = ByteUtil.dataInterception(result, 0, 1); - if (firstChar.equals("1")) { + if ("1".equals(firstChar)) { str += "-"; } str += ByteUtil.dataInterception(result, 1, 4) + "." + @@ -1134,10 +1122,10 @@ public class SerialConnectUtils { for (Coordinate coordinate : coordinates) { double angle = coordinate.getRadarPoint().getAngle(); if (angle >= 90) { - allAnglesLessThan90 = false; // 如果发现一个角度大于等于90，则不是所有角度都小于90 + allAnglesLessThan90 = false; } if (angle <= 90) { - allAnglesGreaterThan90 = false; // 如果发现一个角度小于等于90，则不是所有角度都大于90 + allAnglesGreaterThan90 = false; } } @@ -1154,6 +1142,283 @@ public class SerialConnectUtils { } } + /** + * 判断输入数据符合哪一条规律 + * @param data 输入的6组数据，每组包含距离和角度 + * @return 匹配的规律编号（1, 2, 或 3），如果都不匹配则返回 -1 + */ + public static int matchPattern(List data,int type) { + if (data == null || data.size() != 6) { + return -1; + } + + // 创建带索引的数据列表 + List indexedData = new ArrayList<>(); + for (int i = 0; i < data.size(); i++) { + indexedData.add(new IndexedData( + i, // 原始索引 + data.get(i).getRadarPoint().getDistance(), + data.get(i).getRadarPoint().getAngle() + )); + } + + // 按角度从小到大排序，确定a1-a6 + indexedData.sort(Comparator.comparingDouble(d -> d.angle)); + IndexedData[] a = indexedData.toArray(new IndexedData[6]); + + // 按距离从小到大排序，确定d1-d6的顺序 + List distanceSorted = new ArrayList<>(indexedData); + distanceSorted.sort(Comparator.comparingDouble(d -> d.distance)); + + // 获取每个距离的排名（从小到大，排名1-6） + Map distanceRanks = new HashMap<>(); + for (int i = 0; i < distanceSorted.size(); i++) { + distanceRanks.put(distanceSorted.get(i).originalIndex, i + 1); + } + + if (type == 0) { + //大号在左 + // 检查规律1 + if (checkPattern1(a, distanceRanks)) { + return 1; + } + // 检查规律2 + if (checkPattern2(a, distanceRanks)) { + return 2; + } + // 检查规律3 + if (checkPattern3(a, distanceRanks)) { + return 3; + } + } else { + //大号在右 + // 检查规律1 + if (checkPatternRight1(a, distanceRanks)) { + return 1; + } + // 检查规律2 + if (checkPatternRight2(a, distanceRanks)) { + return 2; + } + // 检查规律3 + if (checkPatternRight3(a, distanceRanks)) { + return 3; + } + } + return -1; + } + + /** + * 大号在左 + * 检查规律1：d3 distanceRanks) { + // a数组已经是按角度从小到大排序的，所以a[0]是a1，a[1]是a2，依此类推 + + // 验证角度顺序是否符合a1= a[4].angle) { + return false; + } + + // 验证距离顺序 d3 distanceRanks) { + // 首先，a数组已经是按角度从小到大排序的(a[0]到a[5]) + // 根据规律2的角度顺序 a2 a2) + // a2 = a[0] (最小角度) + // a3 = a[3] + // a4 = a[5] + // a5 = a[2] + // a6 = a[4] + + // 现在验证角度顺序是否符合 a2 distanceRanks) { + // 验证角度顺序 a2 distanceRanks) { + // a数组已经是按角度从小到大排序的(a[0]到a[5]) + // 根据规律1的角度顺序 a5 distanceRanks) { + // a数组已经是按角度从小到大排序的(a[0]到a[5]) + // 根据规律2的角度顺序 a5 distanceRanks) { + // a数组已经是按角度从小到大排序的(a[0]到a[5]) + // 根据规律3的角度顺序 a6