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

1、六分裂导线识别 

2、增加无人机悬停天线日志
This commit is contained in:
hayu 2025-04-27 16:15:44 +08:00
parent 4df115646b
commit ff28c74a5b
1 changed files with 378 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

491
app/src/main/java/com/bonus/uav/utils/SerialConnectUtils.java
View File

@ -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<Coordinate> 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()<second.getRadarPoint().getAngle()){
@ -831,7 +828,6 @@ public class SerialConnectUtils {
.ifPresent(c -> 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<Coordinate> data,int type) {
if (data == null || data.size() != 6) {
return -1;
}
// 创建带索引的数据列表
List<IndexedData> 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<IndexedData> distanceSorted = new ArrayList<>(indexedData);
distanceSorted.sort(Comparator.comparingDouble(d -> d.distance));
// 获取每个距离的排名（从小到大，排名1-6）
Map<Integer, Integer> 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<d4<d1<d6<d2<d5 且 a1<a2<a3<a5<a4<a6
*/
private static boolean checkPattern1(IndexedData[] a, Map<Integer, Integer> distanceRanks) {
// a数组已经是按角度从小到大排序的，所以a[0]是a1，a[1]是a2，依此类推
// 验证角度顺序是否符合a1<a2<a3<a5<a4<a6
// 由于数组已经是a1<a2<a3<a4<a5<a6，需要额外检查a4和a5的顺序
if (a[3].angle >= a[4].angle) {
return false;
}
// 验证距离顺序 d3<d4<d1<d6<d2<d5
// 即：
// d3 (a[2].originalIndex) 应该是距离第1小的
// d4 (a[3].originalIndex) 应该是距离第2小的
// d1 (a[0].originalIndex) 应该是距离第3小的
// d6 (a[5].originalIndex) 应该是距离第4小的
// d2 (a[1].originalIndex) 应该是距离第5小的
// d5 (a[4].originalIndex) 应该是距离第6小的
return distanceRanks.get(a[2].originalIndex) == 1 &&
distanceRanks.get(a[3].originalIndex) == 2 &&
distanceRanks.get(a[0].originalIndex) == 3 &&
distanceRanks.get(a[5].originalIndex) == 4 &&
distanceRanks.get(a[1].originalIndex) == 5 &&
distanceRanks.get(a[4].originalIndex) == 6;
}
/**
* 大号在左
* 检查规律2：d3<d1<d4<d2<d6<d5 且 a2<a1<a5<a3<a6<a4
*/
private static boolean checkPattern2(IndexedData[] a, Map<Integer, Integer> distanceRanks) {
// 首先，a数组已经是按角度从小到大排序的(a[0]到a[5])
// 根据规律2的角度顺序 a2<a1<a5<a3<a6<a4，我们需要：
// a2 = 第二小的角度
// a1 = 第一小的角度
// a5 = 第三小的角度
// a3 = 第四小的角度
// a6 = 第五小的角度
// a4 = 第六小的角度
// 但是这与数组默认排序冲突，所以需要重新映射：
// 实际a1-a6对应的数组索引应该是：
// a1 = a[1] (因为a1 > a2)
// a2 = a[0] (最小角度)
// a3 = a[3]
// a4 = a[5]
// a5 = a[2]
// a6 = a[4]
// 现在验证角度顺序是否符合 a2<a1<a5<a3<a6<a4
if (!(a[0].angle < a[1].angle &&
a[1].angle < a[2].angle &&
a[2].angle < a[3].angle &&
a[3].angle < a[4].angle &&
a[4].angle < a[5].angle)) {
return false;
}
// 验证距离顺序 d3<d1<d4<d2<d6<d5
// 根据上面的a1-a6定义：
// d1对应a1即a[1]
// d2对应a2即a[0]
// d3对应a3即a[3]
// d4对应a4即a[5]
// d5对应a5即a[2]
// d6对应a6即a[4]
return distanceRanks.get(a[3].originalIndex) == 1 &&
distanceRanks.get(a[1].originalIndex) == 2 &&
distanceRanks.get(a[5].originalIndex) == 3 &&
distanceRanks.get(a[0].originalIndex) == 4 &&
distanceRanks.get(a[4].originalIndex) == 5 &&
distanceRanks.get(a[2].originalIndex) == 6;
}
/**
* 大号在左
* 检查规律3：d1<d3<d2<d4<d5<d6 且 a2<a5<a1<a6<a3<a4
*/
private static boolean checkPattern3(IndexedData[] a, Map<Integer, Integer> distanceRanks) {
// 验证角度顺序 a2<a5<a1<a6<a3<a4
// 重新定义a1-a6：
// a1 = a[2]
// a2 = a[0]
// a3 = a[4]
// a4 = a[5]
// a5 = a[1]
// a6 = a[3]
// 验证距离顺序 d1<d3<d2<d4<d5<d6
// 即：
// d1 (a[2].originalIndex) 应该是距离第1小的
// d3 (a[4].originalIndex) 应该是距离第2小的
// d2 (a[0].originalIndex) 应该是距离第3小的
// d4 (a[5].originalIndex) 应该是距离第4小的
// d5 (a[1].originalIndex) 应该是距离第5小的
// d6 (a[3].originalIndex) 应该是距离第6小的
return distanceRanks.get(a[2].originalIndex) == 1 &&
distanceRanks.get(a[4].originalIndex) == 2 &&
distanceRanks.get(a[0].originalIndex) == 3 &&
distanceRanks.get(a[5].originalIndex) == 4 &&
distanceRanks.get(a[1].originalIndex) == 5 &&
distanceRanks.get(a[3].originalIndex) == 6;
}
/**
* 大号在右
* 检查规律1：d6<d4<d5<d3<d2<d1 且 a5<a2<a6<a1<a4<a3
*/
private static boolean checkPatternRight1(IndexedData[] a, Map<Integer, Integer> distanceRanks) {
// a数组已经是按角度从小到大排序的(a[0]到a[5])
// 根据规律1的角度顺序 a5<a2<a6<a1<a4<a3，我们需要：
// a1 = a[3]
// a2 = a[1]
// a3 = a[5]
// a4 = a[4]
// a5 = a[0]
// a6 = a[2]
// 验证距离顺序 d6<d4<d5<d3<d2<d1
// 即：
// d6 (a[2].originalIndex) 应该是距离第1小的
// d4 (a[4].originalIndex) 应该是距离第2小的
// d5 (a[0].originalIndex) 应该是距离第3小的
// d3 (a[5].originalIndex) 应该是距离第4小的
// d2 (a[1].originalIndex) 应该是距离第5小的
// d1 (a[3].originalIndex) 应该是距离第6小的
return distanceRanks.get(a[2].originalIndex) == 1 &&
distanceRanks.get(a[4].originalIndex) == 2 &&
distanceRanks.get(a[0].originalIndex) == 3 &&
distanceRanks.get(a[5].originalIndex) == 4 &&
distanceRanks.get(a[1].originalIndex) == 5 &&
distanceRanks.get(a[3].originalIndex) == 6;
}
/**
* 大号在右
* 检查规律2：d4<d6<d3<d5<d1<d2 且 a5<a6<a2<a4<a1<a3
*/
private static boolean checkPatternRight2(IndexedData[] a, Map<Integer, Integer> distanceRanks) {
// a数组已经是按角度从小到大排序的(a[0]到a[5])
// 根据规律2的角度顺序 a5<a6<a2<a4<a1<a3，我们需要：
// a1 = a[4]
// a2 = a[2]
// a3 = a[5]
// a4 = a[3]
// a5 = a[0]
// a6 = a[1]
// 验证距离顺序 d4<d6<d3<d5<d1<d2
// 即：
// d4 (a[3].originalIndex) 应该是距离第1小的
// d6 (a[1].originalIndex) 应该是距离第2小的
// d3 (a[5].originalIndex) 应该是距离第3小的
// d5 (a[0].originalIndex) 应该是距离第4小的
// d1 (a[4].originalIndex) 应该是距离第5小的
// d2 (a[2].originalIndex) 应该是距离第6小的
return distanceRanks.get(a[3].originalIndex) == 1 &&
distanceRanks.get(a[1].originalIndex) == 2 &&
distanceRanks.get(a[5].originalIndex) == 3 &&
distanceRanks.get(a[0].originalIndex) == 4 &&
distanceRanks.get(a[4].originalIndex) == 5 &&
distanceRanks.get(a[2].originalIndex) == 6;
}
/**
* 大号在右
* 检查规律3：d4<d3<d6<d1<d5<d2 且 a6<a5<a4<a2<a3<a1
*/
private static boolean checkPatternRight3(IndexedData[] a, Map<Integer, Integer> distanceRanks) {
// a数组已经是按角度从小到大排序的(a[0]到a[5])
// 根据规律3的角度顺序 a6<a5<a4<a2<a3<a1，我们需要：
// a1 = a[5]
// a2 = a[3]
// a3 = a[4]
// a4 = a[2]
// a5 = a[1]
// a6 = a[0]
// 验证距离顺序 d4<d3<d6<d1<d5<d2
// 即：
// d4 (a[2].originalIndex) 应该是距离第1小的
// d3 (a[4].originalIndex) 应该是距离第2小的
// d6 (a[0].originalIndex) 应该是距离第3小的
// d1 (a[5].originalIndex) 应该是距离第4小的
// d5 (a[1].originalIndex) 应该是距离第5小的
// d2 (a[3].originalIndex) 应该是距离第6小的
return distanceRanks.get(a[2].originalIndex) == 1 &&
distanceRanks.get(a[4].originalIndex) == 2 &&
distanceRanks.get(a[0].originalIndex) == 3 &&
distanceRanks.get(a[5].originalIndex) == 4 &&
distanceRanks.get(a[1].originalIndex) == 5 &&
distanceRanks.get(a[3].originalIndex) == 6;
}
// 辅助类，保存原始索引、距离和角度
private static class IndexedData {
int originalIndex;
double distance;
double angle;
IndexedData(int originalIndex, double distance, double angle) {
this.originalIndex = originalIndex;
this.distance = distance;
this.angle = angle;
}
}
/**
* 根据传入的数字返回对应的导线位置描述
*