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 33789f7..991cb0b 100644 --- a/app/src/main/java/com/bonus/uav/service/RadarRunnable.java +++ b/app/src/main/java/com/bonus/uav/service/RadarRunnable.java @@ -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 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()); // 输出结果 diff --git a/app/src/main/java/com/bonus/uav/utils/FileLogger.java b/app/src/main/java/com/bonus/uav/utils/FileLogger.java index ece2a74..53137e1 100644 --- a/app/src/main/java/com/bonus/uav/utils/FileLogger.java +++ b/app/src/main/java/com/bonus/uav/utils/FileLogger.java @@ -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); } } \ No newline at end of file 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 d39aeb2..4dc0785 100644 --- a/app/src/main/java/com/bonus/uav/utils/RTKUtils.java +++ b/app/src/main/java/com/bonus/uav/utils/RTKUtils.java @@ -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; 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 ed17832..1e356ff 100644 --- a/app/src/main/java/com/bonus/uav/utils/SerialConnectUtils.java +++ b/app/src/main/java/com/bonus/uav/utils/SerialConnectUtils.java @@ -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 serialHelperMap = new HashMap<>(); Map serialHelperMap = new HashMap<>(); - //每个传感器当前所发指令 + /** 每个传感器当前所发指令 */ Map outInstruction = new HashMap<>(); - //所以传感器结果 + /** 所以传感器结果 */ SensorDataBean sensorDataBean = new SensorDataBean(); - //线程管理类 + /** 线程管理类 */ private ScheduledThreadManager threadManager; private ConfigDao configDao; private TowerBasicDataDao towerDao; - //传感器信息 + /** 传感器信息 */ List list; - //RTK输出数据多段，组合一下 + /** RTK输出数据多段，组合一下 */ StringBuffer rtkSb = new StringBuffer(); String GNGGA=""; String GNGGAH=""; Context context; + /** 是否记录日志 */ + private int isPrint=0; + public SerialConnectUtils(ScheduledThreadManager threadManager, List 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 radarPointBeanList = FastJsonHelper.jsonArrStrToBeanList(radarData, RadarPointBean.class); //算法过滤雷达数据，返回指定数据，跟杆塔分裂类型对比，数据不匹配直接返回失败，重新找角度测量 + //获取设定的基础数据，杆塔分裂类型等 TowerDataBean towerDataBean = towerDao.selectValue(); //2.1处理雷达数据，返回符合条件的结果集 List> groupedPoints = dealWithRadarData(radarPointBeanList, distanceDiff, pointNum); //2.2计算平均数据存储结果 List calStandardDeviation = new ArrayList<>(); for (List 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 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 coordinateList, boolean reverse) { + public static void classifyAndSortCoordinates(List 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 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; + } + } + + } 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 9d32c1f..6fc7234 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 @@ -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); }