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

算法调整

This commit is contained in:
hayu 2025-04-10 13:39:41 +08:00
parent c75a78b0da
commit b9e54e290f
5 changed files with 445 additions and 213 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
app/src/main/java/com/bonus/uav/service/RadarRunnable.java
View File

@ -122,7 +122,7 @@ public class RadarRunnable implements Runnable {
byte[] buf = new byte[s.available()];
s.read(buf);
str = ByteUtil.bytesToHex(buf);
Log.e(TAG + "-雷达", "雷达原始数据：" + str);
// Log.e(TAG + "-雷达", "雷达原始数据：" + str);
analysisRadarData(str);
} catch (IOException | InterruptedException e) {
Log.e(TAG, e.getMessage());
@ -183,7 +183,7 @@ public class RadarRunnable implements Runnable {
// System.out.println("过滤后数据-->距离：" + item.getDistance() + ";角度:" + item.getAngle() );
// });
configDao.setValue("radarData", FastJsonHelper.beanListToJsonArrStr(filterData));
Log.e(TAG, "ABC雷达过滤后数据：" + FastJsonHelper.beanListToJsonArrStr(filterData));
// Log.e(TAG, "ABC雷达过滤后数据：" + FastJsonHelper.beanListToJsonArrStr(filterData));
}
long endMillis = System.currentTimeMillis();
// Log.e(TAG, (endMillis-timeMillis)/1000+"");
@ -199,7 +199,7 @@ public class RadarRunnable implements Runnable {
List<RadarPointBean> filteredList = scanDistanceData.stream()
.filter(it -> it.getDistance() != 0.0 && it.getDistance() > 1.0 && it.getDistance() < 6
&& ((it.getAngle() > 14.8 && it.getAngle() < 80)))
&& ((it.getAngle() > 14.8 && it.getAngle() < 90)))
.sorted(Comparator.comparing(RadarPointBean::getAngle))
.collect(Collectors.toList());
// 输出结果

172
app/src/main/java/com/bonus/uav/utils/FileLogger.java
View File

@ -4,87 +4,121 @@ import android.content.Context;
import java.io.File;
import java.io.FileOutputStream;
import java.io.IOException;
import java.io.OutputStreamWriter;
import java.text.SimpleDateFormat;
import java.util.Calendar;
import java.util.Date;
import java.util.Locale;
public class FileLogger {
private static final String LOG_DIR = "app_logs";
private static final String LOG_FILE = "app_log.txt";
private static final int MAX_LOG_SIZE = 1024 * 1024;
private static final int MAX_LOG_FILES = 5;
private Context context;
private SimpleDateFormat dateFormat;
private final Context appContext;
private File currentLogFile;
private long logStartTime;
// 一个小时的时间间隔
private static final long LOG_INTERVAL_MILLIS = 60 * 60 * 1000;
private static volatile FileLogger instance;
public FileLogger(Context context) {
this.context = context.getApplicationContext();
this.dateFormat = new SimpleDateFormat("yyyy-MM-dd HH:mm:ss", Locale.getDefault());
this.appContext = context.getApplicationContext();
}
public void log(String tag, String message) {
String logMessage = String.format("%s [%s] %s\n",
dateFormat.format(new Date()),
tag,
message);
writeToFile(logMessage);
}
private void writeToFile(String message) {
File logDir = new File(context.getExternalFilesDir(null), LOG_DIR);
if (!logDir.exists()) {
logDir.mkdirs();
}
File logFile = new File(logDir, LOG_FILE);
try {
// 检查文件大小，如果超过限制则轮转
if (logFile.exists() && logFile.length() > MAX_LOG_SIZE) {
rotateLogs(logDir);
}
FileOutputStream fos = new FileOutputStream(logFile, true);
OutputStreamWriter osw = new OutputStreamWriter(fos);
osw.append(message);
osw.close();
fos.close();
} catch (IOException e) {
e.printStackTrace();
}
}
private void rotateLogs(File logDir) {
// 删除最旧的日志文件
File oldestFile = new File(logDir, LOG_FILE + "." + MAX_LOG_FILES);
if (oldestFile.exists()) {
oldestFile.delete();
}
// 重命名现有日志文件
for (int i = MAX_LOG_FILES - 1; i >= 0; i--) {
File oldFile = i == 0 ?
new File(logDir, LOG_FILE) :
new File(logDir, LOG_FILE + "." + i);
if (oldFile.exists()) {
File newFile = new File(logDir, LOG_FILE + "." + (i + 1));
oldFile.renameTo(newFile);
}
}
}
public void clearLogs() {
File logDir = new File(context.getExternalFilesDir(null), LOG_DIR);
if (logDir.exists()) {
File[] files = logDir.listFiles();
if (files != null) {
for (File file : files) {
file.delete();
public static FileLogger getInstance(Context context) {
if (instance == null) {
synchronized (FileLogger.class) {
if (instance == null) {
instance = new FileLogger(context.getApplicationContext());
}
}
}
return instance;
}
/**
* 打印并保存日志。
*
* @param message 日志内容。
* @param type 控制是否保存日志。1 表示保存，0 表示不保存。
*/
public void log(String message, int type) {
if (type == 1) {
saveLogToFile(message);
} else {
// System.out.println("日志保存已禁用：" + message);
}
}
/**
* 将日志保存到文件。
*
* @param message 日志内容。
*/
private void saveLogToFile(String message) {
long currentTime = System.currentTimeMillis();
// 如果当前时间超过了日志文件的有效期，或者没有日志文件，则创建新的日志文件
if (currentTime - logStartTime >= LOG_INTERVAL_MILLIS || currentLogFile == null) {
createNewLogFile();
logStartTime = currentTime;
}
try {
if (currentLogFile != null) {
FileOutputStream output = new FileOutputStream(currentLogFile, true);
output.write((getCurrentDateTimeString() + " - " + message + "\n").getBytes());
output.close();
System.out.println("日志文件已保存至：" + currentLogFile.getAbsolutePath());
}
} catch (IOException e) {
e.printStackTrace();
System.out.println("写入日志文件时出错: " + e.getMessage());
}
}
/**
* 创建一个新的日志文件。
*/
private void createNewLogFile() {
File storageDir = new File(appContext.getExternalFilesDir(null), "Logs");
if (!storageDir.exists()) {
storageDir.mkdirs();
}
String fileName = "app_log" + getCurrentTimeAsString() + ".txt";
currentLogFile = new File(storageDir, fileName);
System.out.println("新日志文件已创建：" + currentLogFile.getAbsolutePath());
}
/**
* 获取当前时间的字符串表示（格式为 xxxx年xx月xx日xx时xx分xx秒）。
*
* @return 当前时间的字符串表示。
*/
private String getCurrentTimeAsString() {
Calendar calendar = Calendar.getInstance();
int year = calendar.get(Calendar.YEAR);
// 注意：月份从0开始，需加1
int month = calendar.get(Calendar.MONTH) + 1;
int day = calendar.get(Calendar.DAY_OF_MONTH);
int hour = calendar.get(Calendar.HOUR_OF_DAY);
int minute = calendar.get(Calendar.MINUTE);
int second = calendar.get(Calendar.SECOND);
return String.format(
Locale.getDefault(),
"%d年%02d月%02d日%02d时%02d分%02d秒",
year, month, day, hour, minute, second
);
}
/**
* 获取当前日期和时间的字符串表示（格式为 yyyy-MM-dd HH:mm:ss）。
*
* @return 当前日期和时间的字符串表示。
*/
private String getCurrentDateTimeString() {
Date currentDateTime = new Date();
SimpleDateFormat dateFormat = new SimpleDateFormat("yyyy-MM-dd HH:mm:ss", Locale.getDefault());
return dateFormat.format(currentDateTime);
}
}

37
app/src/main/java/com/bonus/uav/utils/RTKUtils.java
View File

@ -10,16 +10,24 @@ import com.bonus.uav.utils.math.MathUtils;
* @date 2025/3/13 9:15
*/
public class RTKUtils {
private static final double a = 6378137.0; // WGS84椭球长半轴
private static final double f = 1.0 / 298.257223563; // 扁率
private static final double eSquared = 2 * f - f * f; // 第一偏心率平方
private static final double RADIUS_OF_EARTH_KM = 6371.0; // 地球平均半径，单位：公里
/** WGS84椭球长半轴 */
private static final double a = 6378137.0;
/** 扁率 */
private static final double f = 1.0 / 298.257223563;
/** 第一偏心率平方 */
private static final double eSquared = 2 * f - f * f;
/** 地球平均半径，单位：公里 */
private static final double RADIUS_OF_EARTH_KM = 6371.0;
private static final double Rq = (6371.393)*1000 ;
/** RTK天线与雷达的高度 m */
private static final double RTK_ANTENNA_HEIGHT = 0.3;
// 转换经纬度到ECEF坐标系
/**
* 转换经纬度到ECEF坐标系
*/
public static double[] convertToECEF(double lat, double lon, double alt) {
double latRad = Math.toRadians(lat);
double lonRad = Math.toRadians(lon);
@ -33,7 +41,9 @@ public class RTKUtils {
};
}
// 从ECEF转换回经纬度
/**
* 从ECEF转换回经纬度
*/
public static double[] ecefToLatLonAlt(double x, double y, double z) {
double lonRad = Math.atan2(y, x);
double p = Math.sqrt(x * x + y * y);
@ -60,7 +70,9 @@ public class RTKUtils {
};
}
// 计算中心点坐标
/**
* 计算中心点坐标
*/
public static double[] calculateMidPoint(double lat1, double lon1, double alt1,
double lat2, double lon2, double alt2) {
double[] ecef1 = convertToECEF(lat1, lon1, alt1);
@ -76,7 +88,7 @@ public class RTKUtils {
/**
* 天线1到雷达坐标的转换
*/
public static double[] calculateMidPoint2(double lat1, double lon1, double alt1,double bearing) {
public static double[] calculateMidPoint2(double lat1, double lon1, double alt1,double bearing,double rollAngle) {
//天线1到雷达坐标的转换
//测点经度
//bearing 不转换的方位角
@ -84,8 +96,7 @@ public class RTKUtils {
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.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);
double h2 = alt1-RTK_ANTENNA_HEIGHT*Math.cos(rollAngle/180*Math.PI);
return new double[]{lat2,lon2,h2};
}
@ -141,7 +152,9 @@ public class RTKUtils {
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;

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

@ -39,17 +39,17 @@ public class SerialConnectUtils {
private static final String TAG = "SerialConnectUtils";
// 误差范围，用于比较浮点数
/** 误差范围，用于比较浮点数 */
private static final double EPSILON = 1e-6;
// 雷达数据的最小差角
/** 雷达数据的最小差角 */
private static final double radarMinAngleDiff = 0.1;
// 相邻角度扫描距离小于多少算作一组数据
/** 相邻角度扫描距离小于多少算作一组数据 */
double distanceDiff = 0.1;
// 最少扫到多少个点算作扫到一条导线
/** 最少扫到多少个点算作扫到一条导线 */
int pointNum = 3;
// 导线中心扫描误差值（导线半径补充客户提供）
double lineRadius = 0.01;
/** 导线中心扫描误差值（导线半径补充客户提供） */
double lineRadius = 0.0;
private static final String ANTENNA_TYPE = "双天线";
@ -58,26 +58,34 @@ public class SerialConnectUtils {
FileLogger logger;
/**
* 无人机的位置：大号在左，大号在右
*/
public static String uavPosition="大号在右";
// Map<String, SerialHelper> serialHelperMap = new HashMap<>();
Map<String, SerialHelpers> serialHelperMap = new HashMap<>();
//每个传感器当前所发指令
/** 每个传感器当前所发指令 */
Map<String, String> outInstruction = new HashMap<>();
//所以传感器结果
/** 所以传感器结果 */
SensorDataBean sensorDataBean = new SensorDataBean();
//线程管理类
/** 线程管理类 */
private ScheduledThreadManager threadManager;
private ConfigDao configDao;
private TowerBasicDataDao towerDao;
//传感器信息
/** 传感器信息 */
List<MapBean> list;
//RTK输出数据多段，组合一下
/** RTK输出数据多段，组合一下 */
StringBuffer rtkSb = new StringBuffer();
String GNGGA="";
String GNGGAH="";
Context context;
/** 是否记录日志 */
private int isPrint=0;
public SerialConnectUtils(ScheduledThreadManager threadManager, List<MapBean> list, DBHelper dbHelper, Context context) {
this.threadManager = threadManager;
this.list = list;
@ -89,6 +97,7 @@ public class SerialConnectUtils {
/**
* 批量打开ttys串口
* 接收串口返回数据，并进行解析
*/
public void connect() {
list.forEach(c -> {
@ -106,20 +115,23 @@ public class SerialConnectUtils {
if ("双天线".equals(ANTENNA_TYPE)){
//处理双天线
result=paramComBean.data;
Log.e(TAG, "串口返回的数据---："+result + "");
if (result.startsWith("$GNGGA") && result.contains("$GNGGAH")){
GNGGAH=extractValidGNGGAOrGNGGAH(result, "$GNGGAH");
Log.e(TAG, "串口返回的数据---GNGGAH："+GNGGAH + "");
GNGGA=extractValidGNGGAOrGNGGAH(result, "$GNGGA");
Log.e(TAG, "串口返回的数据---GNGGA："+GNGGA + "");
} else if (result.contains("$GNGGAH")){
GNGGAH=extractValidGNGGAOrGNGGAH(result, "$GNGGAH");
Log.e(TAG, "串口返回的数据---GNGGAH："+GNGGAH + "");
} else if (result.contains("$GNGGA")){
//匹配，并取出符合条件的数据
GNGGA=extractValidGNGGAOrGNGGAH(result, "$GNGGA");
Log.e(TAG, "串口返回的数据---GNGGA："+GNGGA + "");
}
if (!StringHelper.isEmptyAndNull(GNGGA) && !StringHelper.isEmptyAndNull(GNGGAH)){
//将两个字符串拼接
String results = GNGGA +"@"+ GNGGAH;
handlerData(results, c.getName());
GNGGA="";
GNGGAH="";
}
} else {
result = new String(bRec, StandardCharsets.UTF_8);
handlerData(result, c.getName());
@ -128,17 +140,6 @@ public class SerialConnectUtils {
result = ByteUtil.bytesToHex(bRec);
handlerData(result, c.getName());
}
// Log.e(TAG + c.getName(), result + "");
if ("双天线".equals(ANTENNA_TYPE) && !StringHelper.isEmptyAndNull(GNGGA) && !StringHelper.isEmptyAndNull(GNGGAH)){
if (GNGGA.contains("$GNGGA") && GNGGAH.contains("$GNGGAH")){
//将两个字符串拼接
String results = GNGGA +"@"+ GNGGAH;
handlerData(results, c.getName());
GNGGA="";
GNGGAH="";
}
}
}
};
//打开串口发送数据
@ -188,7 +189,7 @@ public class SerialConnectUtils {
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);
double[] midPoint = RTKUtils.calculateMidPoint2(latC, lonC, altC, bearing, 0.0);
System.out.println("雷达点纬度: " + midPoint[0]);
System.out.println("雷达点经度: " + midPoint[1]);
double midPointHight = midPoint[2];
@ -213,7 +214,6 @@ public class SerialConnectUtils {
* @param type 传感器类型
*/
private void handlerData(String result, String type) {
// Log.e("", type+": "+result );
try {
if (result.length() > 2) {
String substring = result.substring(result.length() - 2);
@ -227,7 +227,6 @@ public class SerialConnectUtils {
String y = ByteUtil.dataInterception(newResult, 14, 22);
Double xValue = getXyData(x);
Double yValue = getXyData(y);
// Log.e(TAG + "-倾角", "X轴：" + xValue + ",Y轴：" + yValue);
sensorDataBean.setxAngle(xValue);
sensorDataBean.setyAngle(yValue);
} else {
@ -243,12 +242,13 @@ public class SerialConnectUtils {
if (result.length() == 28) {
String pitch = ByteUtil.dataInterception(result, 8, 14);
String roll = ByteUtil.dataInterception(result, 14, 20);
String heading = ByteUtil.dataInterception(result, 20, 26);
// String heading = ByteUtil.dataInterception(result, 20, 26);
Double pitchValue = getCompassData(pitch);
Double rollValue = getCompassData(roll);
Double headingValue = getCompassData(heading);
headingValue+=118;
Log.e(TAG + "-罗盘", "罗盘传感器数据--俯仰角：" + pitchValue + ",横滚角：" + rollValue + ",方位角：" + headingValue);
// Double headingValue = getCompassData(heading);
// headingValue+=118;
Log.e("罗盘传感器数据：", "俯仰角：" + pitchValue + ",横滚角：" + rollValue);
logger.log("罗盘传感器数据：俯仰角：" + pitchValue + ",横滚角：" + rollValue,isPrint);
sensorDataBean.setPitchAngle(pitchValue);
sensorDataBean.setRollAngle(rollValue);
// sensorDataBean.setAzimuthAngle(headingValue);
@ -258,7 +258,6 @@ public class SerialConnectUtils {
}
}
} else if ("RTK".equals(type)) {
Log.e(TAG , "-RTK简单处理后的数据："+ result);
if ("双天线".equals(ANTENNA_TYPE)){
double latC=0.0;
double lonC=0.0;
@ -271,7 +270,9 @@ public class SerialConnectUtils {
if (result.contains("$GNGGA") && result.contains("$GNGGAH")){
//将字符串根据@符号分割
String[] parts = result.split("@");
//天线1数据
String[] GNGGA = parts[0].split(",");
//天线2数据
String[] GNGGAH = parts[1].split(",");
if (GNGGA.length >= 9) {
//纬度
@ -282,11 +283,11 @@ public class SerialConnectUtils {
qual = GNGGA[6];
//海拔高度
alt = 0.0;
if (!"".equals(GNGGA[9])) {
if (!StringHelper.isEmptyAndNull(GNGGA[9])) {
alt = Double.parseDouble(GNGGA[9]);
}
Log.e(TAG + "-RTK", "RTK传感器-1数据--经度：" + lon + ";纬度:" + lat + ";海拔:" + alt + ";定位模式:" + qual);
logger.log("ABCD天线1数据：", "经度：" + lon + ";纬度:" + lat + ";海拔:" + alt + ";定位模式:" + qual);
// Log.e("RTK传感器：", "天线1数据：--经度：" + lon + ";纬度:" + lat + ";海拔:" + alt + ";定位模式:" + qual);
// logger.log("ABCD天线1数据：", "经度：" + lon + ";纬度:" + lat + ";海拔:" + alt + ";定位模式:" + qual);
}
if (GNGGAH.length >= 9) {
//纬度
@ -300,45 +301,62 @@ public class SerialConnectUtils {
if (!"".equals(GNGGAH[9])) {
altC = Double.parseDouble(GNGGAH[9]);
}
Log.e(TAG + "-RTK", "RTK传感器-2数据--经度：" + lonC + ";纬度:" + latC + ";海拔:" + altC + ";定位模式:" + qualC);
logger.log("ABCD天线2数据：", "经度：" + lonC + ";纬度:" + latC + ";海拔:" + altC + ";定位模式:" + qualC);
// Log.e("RTK传感器：", "天线2数据--经度：" + lonC + ";纬度:" + latC + ";海拔:" + altC + ";定位模式:" + qualC);
// logger.log("ABCD天线2数据：", "经度：" + lonC + ";纬度:" + latC + ";海拔:" + altC + ";定位模式:" + qualC);
}
}
if (latC != 0.0 && lonC != 0.0 && lat != 0.0 && lon != 0.0){
//主天线、从天线都有数据 先计算中点坐标
// 计算方位角
double bearing = RTKUtils.calculateBearing2(lat, lon, latC, lonC);
System.out.println("BBB雷达方位角: " + bearing + "°");
logger.log("ABCD雷达方位角：", bearing+ "°");
if (latC != 0.0 && lonC != 0.0 && lat != 0.0 && lon != 0.0){
//过滤数据，天线1和天线2经纬度一样的情况
if (lat == latC || lon == lonC || alt - altC>0.1 || altC - alt>0.1){
return;
}
// 算法一，计算中心点在偏移高差计算雷达点。
logger.log("RTK天线1数据：--经度：" + lon + ";纬度:" + lat + ";海拔:" + alt + ";定位模式:" + qual,isPrint);
logger.log("RTK天线2数据：--经度：" + lonC + ";纬度:" + latC + ";海拔:" + altC + ";定位模式:" + qualC,isPrint);
//高度为两个天线高度的平均值
// double alt1=(alt+altC)/2+0.7*Math.sin(Math.toRadians(sensorDataBean.getRollAngle()));
double alt1=altC-0.235*Math.sin(Math.toRadians(sensorDataBean.getRollAngle()));
alt=alt1;
altC=alt1;
// 计算方位角
double bearing = RTKUtils.calculateBearing2(lat, lon, latC, lonC);
// 算法一，计算中心点在偏移高差计算雷达点。
// 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]);
double[] midPoint = RTKUtils.calculateMidPoint2(lat, lon, alt,bearing,sensorDataBean.getRollAngle());
double midPointHight = midPoint[2];
// double[] results = RTKUtils.calculateBearingAndElevation(lat, lon, alt, latC, lonC, altC);
// System.out.println("方位角2："+ results[0]);
// System.out.println("仰角2："+ results[1]);
sensorDataBean.setLon(midPoint[1]);
sensorDataBean.setLat(midPoint[0]);
sensorDataBean.setH(midPointHight);
sensorDataBean.setPositioningMode(qual);
sensorDataBean.setAzimuthAngle(bearing);
//打印数据，天线1和天线2、雷达点经纬度、雷达方位角
Log.e("RTK天线1数据：", "经度：" + lon + ";纬度:" + lat + ";海拔:" + alt + ";定位模式:" + qual);
Log.e("RTK天线2数据：", "经度：" + lonC + ";纬度:" + latC + ";海拔:" + altC + ";定位模式:" + qualC);
Log.e("雷达点经纬度：", "纬度：" + midPoint[0] + ";经度:" + midPoint[1] + ";海拔:" + midPoint[2]+";雷达方位角："+bearing+ "°");
logger.log("雷达点经纬度：--纬度：" + midPoint[0] + ";经度:" + midPoint[1] + ";海拔:" + midPoint[2]+";雷达方位角："+bearing+ "°",isPrint);
sensorDataBean.setLon(midPoint[1]);
sensorDataBean.setLat(midPoint[0]);
sensorDataBean.setH(midPointHight);
sensorDataBean.setPositioningMode(qual);
sensorDataBean.setAzimuthAngle(bearing);
}
}
} else {
if (result.startsWith("$GNGGA")) {
// Log.e(TAG + "-RTK", "完整数据：" + rtkSb);
@ -395,6 +413,7 @@ public class SerialConnectUtils {
String sum16 = ByteUtil.getSum16(data);
if (check.equals(sum16)) {
String orderType = result.substring(2, 4);
//保存基础数据命令
if ("01".equals(orderType)) {
if (result.length() == 26) {
//传入将要测量的杆塔基础信息(协议传入基础数据11个字节)
@ -429,27 +448,31 @@ public class SerialConnectUtils {
}
}
} else if ("02".equals(orderType)) {
//开始测量,返回传感器数据
String sensorData = configDao.getValue("sensorData");
isPrint=1;
//开始测量命令,返回传感器数据
// String sensorData = configDao.getValue("sensorData");
// SensorDataBean sensorDataBean = FastJsonHelper.jsonStrToBean(sensorData, SensorDataBean.class);
Log.e(TAG + "-数传", "传感器数据Bean：" + sensorDataBean);
//获取雷达数据
String radarData = configDao.getValue("radarData");
List<RadarPointBean> radarPointBeanList = FastJsonHelper.jsonArrStrToBeanList(radarData, RadarPointBean.class);
//算法过滤雷达数据，返回指定数据，跟杆塔分裂类型对比，数据不匹配直接返回失败，重新找角度测量
//获取设定的基础数据，杆塔分裂类型等
TowerDataBean towerDataBean = towerDao.selectValue();
//2.1处理雷达数据，返回符合条件的结果集
List<List<RadarPointBean>> groupedPoints = dealWithRadarData(radarPointBeanList, distanceDiff, pointNum);
//2.2计算平均数据存储结果
List<RadarPointBean> calStandardDeviation = new ArrayList<>();
for (List<RadarPointBean> group : groupedPoints) {
System.out.print("Group: ");
System.out.print("雷达测到一点的数据Group: ");
for (RadarPointBean point : group) {
System.out.print("(Angle: " + point.getAngle() + ", Distance: " + point.getDistance() + ") ");
}
System.out.println();
// 计算 《根据算法去调用不同的计算方法》
RadarPointBean stdDevPoint = getMiddlePoint(group);
System.out.println("Standard Deviation - Angle: " + stdDevPoint.getAngle() + ", Distance: " + stdDevPoint.getDistance());
System.out.println("处理后雷达的数据 - Angle: " + stdDevPoint.getAngle() + ", Distance: " + (stdDevPoint.getDistance() + lineRadius));
logger.log("处理后雷达的数据 - Angle: " + stdDevPoint.getAngle() + ", Distance: " + (stdDevPoint.getDistance() + lineRadius), isPrint);
stdDevPoint.setDistance(stdDevPoint.getDistance() + lineRadius);
calStandardDeviation.add(stdDevPoint);
}
@ -461,25 +484,35 @@ public class SerialConnectUtils {
List<Coordinate> coordinates = new ArrayList<>();
if (calStandardDeviation.size() >= towerDataBean.getSplitType()) {
radarStatus = true;
//根据设定的分裂数量进行截取数据条数
calStandardDeviation = calStandardDeviation.subList(0, towerDataBean.getSplitType());
for (RadarPointBean radarPointBean : calStandardDeviation) {
System.out.printf("RTK：lat：%.8f; lon：%.8f; 高度：%.2f\n", sensorDataBean.getLat(), sensorDataBean.getLon(), sensorDataBean.getH());
// System.out.printf("RTK：lat：%.8f; lon：%.8f; 高度：%.2f\n", sensorDataBean.getLat(), sensorDataBean.getLon(), sensorDataBean.getH());
//计算测点经纬度
Coordinate coordinate = RadarMathUtil.transferPoint2(radarPointBean, sensorDataBean);
coordinate.setRadarPoint(radarPointBean);
coordinates.add(coordinate);
System.out.printf("测点：lat：%.8f; lon：%.8f; 高度：%.2f\n", coordinate.getLat(), coordinate.getLon(), coordinate.getH());
logger.log("ABCD测点经纬度：", "经度："+coordinate.getLon()+"纬度："+coordinate.getLat()+"高度："+coordinate.getH());
// System.out.printf("测点：lat：%.8f; lon：%.8f; 高度：%.2f\n", coordinate.getLat(), coordinate.getLon(), coordinate.getH());
logger.log("雷达点经纬度：--经度："+sensorDataBean.getLon()+"纬度："+sensorDataBean.getLat()+"高度："+sensorDataBean.getH(),isPrint);
logger.log("测点经纬度：--经度："+coordinate.getLon()+"纬度："+coordinate.getLat()+"高度："+coordinate.getH(),isPrint);
}
//2.5导线编号
//先以0-90度为标准的导线if
if (coordinates.get(0).getRadarPoint().getAngle() < 90) {
classifyAndSortCoordinates(coordinates, false);
} else {
classifyAndSortCoordinates(coordinates, true);
}
//判断无人机位置，上或下
int re = checkAngleDistribution(coordinates);
if (re == 0) {
//提示该位置不合适
SerialHelpers serialHelper = serialHelperMap.get("数传");
assert serialHelper != null;
serialHelper.sendHex("F0000003");
return;
} else {
//给导线编号
classifyAndSortCoordinates(coordinates, true, re);
}
}
//2.6 已编号结束的雷达数据 coordinates,传感器测量数据 sensorDataBean,根据编成指令返回遥控器端
@ -508,11 +541,11 @@ public class SerialConnectUtils {
}
//传感器数据
//俯仰角
int pitchAngle = (int) (ArithUtil.round(Math.abs(sensorDataBean.getPitchAngle()), 2) * 100);
sb.append(ByteUtil.decIntToHexStr(pitchAngle, 2));
int pitchAngle = (int) (ArithUtil.round(sensorDataBean.getPitchAngle(), 2) * 100);
sb.append(ByteUtil.decIntToHexStr(pitchAngle, 4));
//横滚角
int rollAngle = (int) (ArithUtil.round(Math.abs(sensorDataBean.getRollAngle()), 2) * 100);
sb.append(ByteUtil.decIntToHexStr(rollAngle, 2));
int rollAngle = (int) (ArithUtil.round(sensorDataBean.getRollAngle(), 2) * 100);
sb.append(ByteUtil.decIntToHexStr(rollAngle, 4));
//方位角(从参考点（通常是正北方向）开始，顺时针方向测量的一个角度)
int azimuthAngle = (int) (ArithUtil.round(sensorDataBean.getAzimuthAngle(), 2) * 100);
sb.append(ByteUtil.decIntToHexStr(azimuthAngle, 2));
@ -547,6 +580,7 @@ public class SerialConnectUtils {
SerialHelpers serialHelper = serialHelperMap.get("数传");
assert serialHelper != null;
Log.e(TAG, "2.测量数据返回：" + sb.toString());
logger.log("传给手柄的数据：--"+sb.toString(),isPrint);
serialHelper.sendHex(sb.toString());
// else {
// SerialHelper serialHelper = serialHelperMap.get("数传");
@ -554,6 +588,21 @@ public class SerialConnectUtils {
// serialHelper.sendHex("F0000002");
// Log.e(TAG, "激光雷达解析数据与分裂类型不匹配：" + calStandardDeviation);
// }
} else if ("03".equals(orderType)) {
isPrint=1;
logger.log("遥控器端保存数据",isPrint);
isPrint=0;
} else if ("04".equals(orderType)) {
String position = result.substring(4, 6);
if ("01".equals(position)){
//大号在右
uavPosition="大号在右";
System.out.println("无人机测量位置："+uavPosition);
} else if ("02".equals(position)){
//大号在左
uavPosition="大号在左";
System.out.println("无人机测量位置："+uavPosition);
}
}
}
}
@ -619,8 +668,11 @@ public class SerialConnectUtils {
* 目前全部是按左侧算的如果右侧需要小改一下
* @param coordinateList
* @param reverse 返回的数据 1号就在list第一个，2号就是第二个
*
* 2025-03-30修改：修改四分裂编号逻辑
* @param type 1：无人机在导线上方，2：无人机在导线下方
*/
public static void classifyAndSortCoordinates(List<Coordinate> coordinateList, boolean reverse) {
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");
@ -639,44 +691,131 @@ public class SerialConnectUtils {
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;
switch (uavPosition) {
case "大号在左":
if (type == 2) {
// 无人机在上面
// 距离最短为1号线
coordinateList.stream()
.min(Comparator.comparing(c -> c.getRadarPoint().getDistance()))
.ifPresent(c -> c.setWireNum(1));
// 距离最长为3号线
coordinateList.stream()
.max(Comparator.comparing(c -> c.getRadarPoint().getDistance()))
.ifPresent(c -> c.setWireNum(3));
// 角度最小为4号线
coordinateList.stream()
.min(Comparator.comparing(c -> c.getRadarPoint().getAngle()))
.ifPresent(c -> c.setWireNum(4));
// 角度最大为2号线
coordinateList.stream()
.max(Comparator.comparing(c -> c.getRadarPoint().getAngle()))
.ifPresent(c -> c.setWireNum(2));
} else {
remainingIndex2 = i;
// 无人机在下面
// 距离最短为2号线
coordinateList.stream()
.min(Comparator.comparing(c -> c.getRadarPoint().getDistance()))
.ifPresent(c -> c.setWireNum(2));
// 距离最长为4号线
coordinateList.stream()
.max(Comparator.comparing(c -> c.getRadarPoint().getDistance()))
.ifPresent(c -> c.setWireNum(4));
// 角度最小为1号线
coordinateList.stream()
.min(Comparator.comparing(c -> c.getRadarPoint().getAngle()))
.ifPresent(c -> c.setWireNum(1));
// 角度最大为3号线
coordinateList.stream()
.max(Comparator.comparing(c -> c.getRadarPoint().getAngle()))
.ifPresent(c -> c.setWireNum(3));
}
}
}
// 根据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);
break;
case "大号在右":
if (type == 2) {
// 无人机在上面
// 距离最短为4号线
coordinateList.stream()
.min(Comparator.comparing(c -> c.getRadarPoint().getDistance()))
.ifPresent(c -> c.setWireNum(4));
// 距离最长为2号线
coordinateList.stream()
.max(Comparator.comparing(c -> c.getRadarPoint().getDistance()))
.ifPresent(c -> c.setWireNum(2));
// 角度最小为1号线
coordinateList.stream()
.min(Comparator.comparing(c -> c.getRadarPoint().getAngle()))
.ifPresent(c -> c.setWireNum(1));
// 角度最大为3号线
coordinateList.stream()
.max(Comparator.comparing(c -> c.getRadarPoint().getAngle()))
.ifPresent(c -> c.setWireNum(3));
} else {
// 无人机在下面
// 距离最短为3号线
coordinateList.stream()
.min(Comparator.comparing(c -> c.getRadarPoint().getDistance()))
.ifPresent(c -> c.setWireNum(3));
// 距离最长为1号线
coordinateList.stream()
.max(Comparator.comparing(c -> c.getRadarPoint().getDistance()))
.ifPresent(c -> c.setWireNum(1));
// 角度最小为4号线
coordinateList.stream()
.min(Comparator.comparing(c -> c.getRadarPoint().getAngle()))
.ifPresent(c -> c.setWireNum(4));
// 角度最大为2号线
coordinateList.stream()
.max(Comparator.comparing(c -> c.getRadarPoint().getAngle()))
.ifPresent(c -> c.setWireNum(2));
}
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);
@ -808,7 +947,7 @@ public class SerialConnectUtils {
* @return
*/
public double convertToDegrees(String coordinate, int len) {
if(!"".equals(coordinate)){
if(!StringHelper.isEmptyAndNull(coordinate)){
// 提取度的部分（前3位）
int degrees = Integer.parseInt(coordinate.substring(0, len));
// 提取分的部分（后4位）
@ -847,7 +986,7 @@ public class SerialConnectUtils {
public static Double getCompassData(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) + "." +
@ -856,9 +995,14 @@ public class SerialConnectUtils {
}
/**
* 截取双天线数据
* GNGGA--天线1
* GNGGAH--天线2
*/
public static String extractValidGNGGAOrGNGGAH(String data,String type) {
Log.e(TAG,"-----111-----");
String[] lines = data.split("\\r?\\n"); // 处理不同系统的换行符
// 处理不同系统的换行符
String[] lines = data.split("\\r?\\n");
for (String line : lines) {
// 去除前后空格并处理前缀
String strippedLine = line.trim();
@ -867,7 +1011,6 @@ public class SerialConnectUtils {
// 按逗号分割并保留空字段
String[] parts = strippedLine.split(",", -1);
if (parts.length >= 10) {
// result.add(strippedLine);
return strippedLine;
}
}
@ -875,4 +1018,41 @@ public class SerialConnectUtils {
return "";
}
/**
* 根据雷达数据角度，判断无人机位置：
* 1、角度全部大于90，无人机在上面 返回2
* 2、角度全部小于90，无人机在下面 返回1
* 3、角度既有大于90，又有小于90，提示该位置不合适 返回0
*/
public int checkAngleDistribution(List<Coordinate> coordinates) {
// 定义两个标志变量，分别表示是否所有角度都小于90和是否所有角度都大于90
boolean allAnglesLessThan90 = true;
boolean allAnglesGreaterThan90 = true;
// 遍历 coordinates 列表
for (Coordinate coordinate : coordinates) {
double angle = coordinate.getRadarPoint().getAngle();
if (angle >= 90) {
allAnglesLessThan90 = false; // 如果发现一个角度大于等于90，则不是所有角度都小于90
}
if (angle <= 90) {
allAnglesGreaterThan90 = false; // 如果发现一个角度小于等于90，则不是所有角度都大于90
}
}
// 根据判断结果返回对应的值
if (allAnglesLessThan90) {
// 所有角度都小于90
return 1;
} else if (allAnglesGreaterThan90) {
// 所有角度都大于90
return 2;
} else {
// 角度既有大于90的也有小于90的
return 0;
}
}
}

29
app/src/main/java/com/bonus/uav/utils/sag/RadarMathUtil.java
View File

@ -87,22 +87,26 @@ public class RadarMathUtil {
* @return
*/
public static Coordinate transferPoint2(RadarPointBean radarPointBean, SensorDataBean sensorDataBean) {
double radarAngle = radarPointBean.getAngle();
// double radarAngles = 0;
// if (radarPointBean.getAngle()>90){
// //无人机在上面，扫描角度加上0.05
// radarAngles = radarPointBean.getAngle()+0.05;
// radarPointBean.setAngle(radarAngles);
// }else if (radarPointBean.getAngle()<90){
// //无人机在下面，扫描角度减去0.05
// radarAngles = radarPointBean.getAngle()-0.05;
// radarPointBean.setAngle(radarAngles);
// }
//雷达点到测点的角度
double radarAngle = radarPointBean.getAngle()- sensorDataBean.getPitchAngle();
//雷达点到测点的距离
double dis = radarPointBean.getDistance();
double dis2=dis;
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();
// //1.RTK天线到激光雷达点转换
// //激光雷达经度
// double lonRadar = lonGps + RTKDistance * Math.sin((azimuthAngle+RTKAngle)/180*Math.PI)/Rq/Math.cos(latGps/180*Math.PI)*180/Math.PI;
// //激光雷达纬度
// double latRadar = latGps + (RTKDistance * Math.cos((azimuthAngle+RTKAngle)/180*Math.PI)/Rq)*180/Math.PI;
// //激光雷达高程
// 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;
@ -119,8 +123,9 @@ public class RadarMathUtil {
// }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);
// double h2 = hRadar + dis2 * MathUtils.cos(radarAngle - sensorDataBean.getRollAngle());
double h2 = hRadar + dis2 * MathUtils.cos(radarAngle - 0);
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);
}