diff --git a/app/src/main/java/com/bonus/uav/db/MeasurementDataDao.java b/app/src/main/java/com/bonus/uav/db/MeasurementDataDao.java new file mode 100644 index 0000000..5d9f4e4 --- /dev/null +++ b/app/src/main/java/com/bonus/uav/db/MeasurementDataDao.java @@ -0,0 +1,139 @@ +package com.bonus.uav.db; + +import android.database.Cursor; +import android.util.Log; + +import com.bonus.uav.entity.MeasurementDataBean; +import com.bonus.uav.utils.DBHelper; + +import java.util.ArrayList; +import java.util.List; + +public class MeasurementDataDao { + private static final String TAG = "MeasurementDataDao"; + + private DBHelper dbHelper; + + public MeasurementDataDao(DBHelper dbHelper) { + this.dbHelper = dbHelper; + } + + // 创建测量数据表的SQL语句 + public static final String CREATE_TABLE = "CREATE TABLE `bm_measurement_data` (" + + "`id` INTEGER PRIMARY KEY AUTOINCREMENT," + + "`loop_type` INTEGER," + + "`wire_position_int` INTEGER," + + "`wire_position_str` TEXT," + + "`roll_angle` REAL," + + "`pitch_angle` REAL," + + "`yaw_angle` REAL," + + "`radar_longitude` REAL," + + "`radar_latitude` REAL," + + "`radar_elevation` REAL," + + "`wire1_distance` REAL," + + "`wire1_angle` REAL," + + "`wire2_distance` REAL," + + "`wire2_angle` REAL," + + "`wire3_distance` REAL," + + "`wire3_angle` REAL," + + "`wire4_distance` REAL," + + "`wire4_angle` REAL," + + "`sag_value` REAL" + + ")"; + + /** + * 新增或更新测量数据 + * + * @param measurementData 测量数据对象 + * @return 是否成功 + */ + public boolean insert(MeasurementDataBean measurementData) { + String sql = "INSERT OR REPLACE INTO `bm_measurement_data` (" + + "`loop_type`, `wire_position_int`, `wire_position_str`, `roll_angle`, `pitch_angle`, `yaw_angle`, " + + "`radar_longitude`, `radar_latitude`, `radar_elevation`, `wire1_distance`, `wire1_angle`, `wire2_distance`, " + + "`wire2_angle`, `wire3_distance`, `wire3_angle`, `wire4_distance`, `wire4_angle`, `sag_value`) " + + "VALUES (?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?)"; + try { + this.dbHelper.execSQL(sql, new Object[]{ + measurementData.getLoopType(), + measurementData.getWirePositionInt(), + measurementData.getWirePositionStr(), + measurementData.getRollAngle(), + measurementData.getPitchAngle(), + measurementData.getYawAngle(), + measurementData.getRadarLongitude(), + measurementData.getRadarLatitude(), + measurementData.getRadarElevation(), + measurementData.getWire1Distance(), + measurementData.getWire1Angle(), + measurementData.getWire2Distance(), + measurementData.getWire2Angle(), + measurementData.getWire3Distance(), + measurementData.getWire3Angle(), + measurementData.getWire4Distance(), + measurementData.getWire4Angle(), + measurementData.getSagValue() + }); + return true; + } catch (Exception e) { + Log.e(TAG, "Insert or update failed: " + e.toString()); + return false; + } + } + + /** + * 查询所有测量数据 + * + * @return 测量数据列表 + */ + public List selectAll() { + String sql = "SELECT * FROM bm_measurement_data"; + Cursor cr = null; + List dataList = new ArrayList<>(); + try { + cr = this.dbHelper.query(sql, new String[]{}); + while (cr.moveToNext()) { + MeasurementDataBean data = new MeasurementDataBean(); + data.setId(cr.getInt(cr.getColumnIndex("id"))); + data.setLoopType(cr.getInt(cr.getColumnIndex("loop_type"))); + data.setWirePositionInt(cr.getInt(cr.getColumnIndex("wire_position_int"))); + data.setWirePositionStr(cr.getString(cr.getColumnIndex("wire_position_str"))); + data.setRollAngle(cr.getDouble(cr.getColumnIndex("roll_angle"))); + data.setPitchAngle(cr.getDouble(cr.getColumnIndex("pitch_angle"))); + data.setYawAngle(cr.getDouble(cr.getColumnIndex("yaw_angle"))); + data.setRadarLongitude(cr.getDouble(cr.getColumnIndex("radar_longitude"))); + data.setRadarLatitude(cr.getDouble(cr.getColumnIndex("radar_latitude"))); + data.setRadarElevation(cr.getDouble(cr.getColumnIndex("radar_elevation"))); + data.setWire1Distance(cr.getDouble(cr.getColumnIndex("wire1_distance"))); + data.setWire1Angle(cr.getDouble(cr.getColumnIndex("wire1_angle"))); + data.setWire2Distance(cr.getDouble(cr.getColumnIndex("wire2_distance"))); + data.setWire2Angle(cr.getDouble(cr.getColumnIndex("wire2_angle"))); + data.setWire3Distance(cr.getDouble(cr.getColumnIndex("wire3_distance"))); + data.setWire3Angle(cr.getDouble(cr.getColumnIndex("wire3_angle"))); + data.setWire4Distance(cr.getDouble(cr.getColumnIndex("wire4_distance"))); + data.setWire4Angle(cr.getDouble(cr.getColumnIndex("wire4_angle"))); + data.setSagValue(cr.getDouble(cr.getColumnIndex("sag_value"))); + dataList.add(data); + } + } catch (Exception e) { + Log.e(TAG, "Select all failed: " + e.toString()); + } finally { + if (cr != null) { + cr.close(); + } + } + return dataList; + } + + /** + * 清空测量数据表 + */ + public void clearTable() { + String sql = "DELETE FROM bm_measurement_data"; + try { + this.dbHelper.execSQL(sql, new Object[]{}); + } catch (Exception e) { + Log.e(TAG, "Clear table failed: " + e.toString()); + } + } +} \ No newline at end of file diff --git a/app/src/main/java/com/bonus/uav/entity/MeasurementDataBean.java b/app/src/main/java/com/bonus/uav/entity/MeasurementDataBean.java new file mode 100644 index 0000000..3630f03 --- /dev/null +++ b/app/src/main/java/com/bonus/uav/entity/MeasurementDataBean.java @@ -0,0 +1,63 @@ +package com.bonus.uav.entity; + +public class MeasurementDataBean { + private int id; + private int loopType; + private int wirePositionInt; + private String wirePositionStr; + private double rollAngle; + private double pitchAngle; + private double yawAngle; + private double radarLongitude; + private double radarLatitude; + private double radarElevation; + private double wire1Distance; + private double wire1Angle; + private double wire2Distance; + private double wire2Angle; + private double wire3Distance; + private double wire3Angle; + private double wire4Distance; + private double wire4Angle; + private double sagValue; + + // Getters and Setters + public int getId() { return id; } + public void setId(int id) { this.id = id; } + public int getLoopType() { return loopType; } + public void setLoopType(int loopType) { this.loopType = loopType; } + public int getWirePositionInt() { return wirePositionInt; } + public void setWirePositionInt(int wirePositionInt) { this.wirePositionInt = wirePositionInt; } + public String getWirePositionStr() { return wirePositionStr; } + public void setWirePositionStr(String wirePositionStr) { this.wirePositionStr = wirePositionStr; } + public double getRollAngle() { return rollAngle; } + public void setRollAngle(double rollAngle) { this.rollAngle = rollAngle; } + public double getPitchAngle() { return pitchAngle; } + public void setPitchAngle(double pitchAngle) { this.pitchAngle = pitchAngle; } + public double getYawAngle() { return yawAngle; } + public void setYawAngle(double yawAngle) { this.yawAngle = yawAngle; } + public double getRadarLongitude() { return radarLongitude; } + public void setRadarLongitude(double radarLongitude) { this.radarLongitude = radarLongitude; } + public double getRadarLatitude() { return radarLatitude; } + public void setRadarLatitude(double radarLatitude) { this.radarLatitude = radarLatitude; } + public double getRadarElevation() { return radarElevation; } + public void setRadarElevation(double radarElevation) { this.radarElevation = radarElevation; } + public double getWire1Distance() { return wire1Distance; } + public void setWire1Distance(double wire1Distance) { this.wire1Distance = wire1Distance; } + public double getWire1Angle() { return wire1Angle; } + public void setWire1Angle(double wire1Angle) { this.wire1Angle = wire1Angle; } + public double getWire2Distance() { return wire2Distance; } + public void setWire2Distance(double wire2Distance) { this.wire2Distance = wire2Distance; } + public double getWire2Angle() { return wire2Angle; } + public void setWire2Angle(double wire2Angle) { this.wire2Angle = wire2Angle; } + public double getWire3Distance() { return wire3Distance; } + public void setWire3Distance(double wire3Distance) { this.wire3Distance = wire3Distance; } + public double getWire3Angle() { return wire3Angle; } + public void setWire3Angle(double wire3Angle) { this.wire3Angle = wire3Angle; } + public double getWire4Distance() { return wire4Distance; } + public void setWire4Distance(double wire4Distance) { this.wire4Distance = wire4Distance; } + public double getWire4Angle() { return wire4Angle; } + public void setWire4Angle(double wire4Angle) { this.wire4Angle = wire4Angle; } + public double getSagValue() { return sagValue; } + public void setSagValue(double sagValue) { this.sagValue = sagValue; } +} diff --git a/app/src/main/java/com/bonus/uav/utils/DBHelper.java b/app/src/main/java/com/bonus/uav/utils/DBHelper.java index 62cad96..29700f3 100644 --- a/app/src/main/java/com/bonus/uav/utils/DBHelper.java +++ b/app/src/main/java/com/bonus/uav/utils/DBHelper.java @@ -8,6 +8,7 @@ import android.database.sqlite.SQLiteOpenHelper; import android.util.Log; import com.bonus.uav.db.ConfigDao; +import com.bonus.uav.db.MeasurementDataDao; import com.bonus.uav.db.SensorDataDao; import com.bonus.uav.db.TowerBasicDataDao; @@ -18,7 +19,7 @@ public class DBHelper extends SQLiteOpenHelper { private static final int version = 2; // 数据库版本 private final String[] CREATABLE = new String[]{ConfigDao.CREATE_TABLE, SensorDataDao.CREATE_TABLE, - TowerBasicDataDao.CREATE_TABLE}; + TowerBasicDataDao.CREATE_TABLE, MeasurementDataDao.CREATE_TABLE}; public DBHelper(Context context) { super(context, DB_NAME, null, version); diff --git a/app/src/main/java/com/bonus/uav/utils/FileHoverLogger.java b/app/src/main/java/com/bonus/uav/utils/FileHoverLogger.java new file mode 100644 index 0000000..f3d2a23 --- /dev/null +++ b/app/src/main/java/com/bonus/uav/utils/FileHoverLogger.java @@ -0,0 +1,125 @@ +package com.bonus.uav.utils; + +import android.content.Context; + +import java.io.File; +import java.io.FileOutputStream; +import java.io.IOException; +import java.text.SimpleDateFormat; +import java.util.Calendar; +import java.util.Date; +import java.util.Locale; + +public class FileHoverLogger { + + private final Context appContext; + private File currentLogFile; + private long logStartTime; + // 一个小时的时间间隔 + private static final long LOG_INTERVAL_MILLIS = 60 * 60 * 1000; + + private static volatile FileHoverLogger instance; + + public FileHoverLogger(Context context) { + this.appContext = context.getApplicationContext(); + } + + public static FileHoverLogger getInstance(Context context) { + if (instance == null) { + synchronized (FileHoverLogger.class) { + if (instance == null) { + instance = new FileHoverLogger(context.getApplicationContext()); + } + } + } + return instance; + } + + /** + * 打印并保存日志。 + * + * @param message 日志内容。 + * @param type 控制是否保存日志。1 表示保存，0 表示不保存。 + */ + public void recordLog(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), "HoverLogs"); + if (!storageDir.exists()) { + storageDir.mkdirs(); + } + + String fileName = "uav_hover_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 4dc0785..c6ac712 100644 --- a/app/src/main/java/com/bonus/uav/utils/RTKUtils.java +++ b/app/src/main/java/com/bonus/uav/utils/RTKUtils.java @@ -24,6 +24,23 @@ public class RTKUtils { /** RTK天线与雷达的高度 m */ private static final double RTK_ANTENNA_HEIGHT = 0.3; + /** + * 物理距离 + * 两个天线的物理距离--水平距离 0.735m + * 天线1到雷达点的距离--水平距离 0.415m + * 天线2到雷达点的距离--水平距离 0.320m + * 天线1到雷达点的距离--斜距 0.435m + */ + +// private static final double PHYSICAL_DISTANCE = 0.409; + private static final double PHYSICAL_DISTANCE = 0.435; + + /** + * 角度 + */ +// private static final double PHYSICAL_PERSPECTIVE = 14.3; + private static final double PHYSICAL_PERSPECTIVE = 15.0; + /** * 转换经纬度到ECEF坐标系 @@ -93,9 +110,9 @@ public class RTKUtils { //测点经度 //bearing 不转换的方位角 //天线1的经纬度 - 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 lon2 = lon1 + PHYSICAL_DISTANCE * Math.sin(((bearing-PHYSICAL_PERSPECTIVE)+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 lat2 = lat1 + (PHYSICAL_DISTANCE * Math.cos(((bearing-PHYSICAL_PERSPECTIVE)+0)/180*Math.PI)/Rq)*180/Math.PI; double h2 = alt1-RTK_ANTENNA_HEIGHT*Math.cos(rollAngle/180*Math.PI); return new double[]{lat2,lon2,h2}; } 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 1e356ff..5997134 100644 --- a/app/src/main/java/com/bonus/uav/utils/SerialConnectUtils.java +++ b/app/src/main/java/com/bonus/uav/utils/SerialConnectUtils.java @@ -57,12 +57,29 @@ public class SerialConnectUtils { private static final double DU=276; FileLogger logger; + FileHoverLogger hoverLogger; /** * 无人机的位置：大号在左，大号在右 */ public static String uavPosition="大号在右"; + /** + * 回路类型 + * 1-单回路， 2-双回路 + */ + public static int loopType; + + /** + * 导线位置 + * 单回路： 1-左地线/光缆, 2-右地线/光缆, 3-左相, 4-中相, 5-右相 + * 双回路： 6-左地线/光缆, 7-地线/光缆, 8-左上相, 9-左中相,10-左下相,11-右上相, 12-右中相,13-右下相 + */ + public static int linePosition; + + public static String linePositionName; + + // Map serialHelperMap = new HashMap<>(); Map serialHelperMap = new HashMap<>(); @@ -86,6 +103,9 @@ public class SerialConnectUtils { /** 是否记录日志 */ private int isPrint=0; + /** 是否记录悬停数据 */ + private int isRecord=0; + public SerialConnectUtils(ScheduledThreadManager threadManager, List list, DBHelper dbHelper, Context context) { this.threadManager = threadManager; this.list = list; @@ -93,6 +113,7 @@ public class SerialConnectUtils { towerDao = new TowerBasicDataDao(dbHelper); this.context = context; logger = new FileLogger(this.context); + hoverLogger = new FileHoverLogger(this.context); } /** @@ -310,7 +331,8 @@ public class SerialConnectUtils { if (lat == latC || lon == lonC || alt - altC>0.1 || altC - alt>0.1){ return; } - + hoverLogger.recordLog("RTK天线1数据：--经度：" + lon + ";纬度:" + lat + ";海拔:" + alt + ";定位模式:" + qual,isRecord); + hoverLogger.recordLog("RTK天线2数据：--经度：" + lonC + ";纬度:" + latC + ";海拔:" + altC + ";定位模式:" + qualC,isRecord); logger.log("RTK天线1数据：--经度：" + lon + ";纬度:" + lat + ";海拔:" + alt + ";定位模式:" + qual,isPrint); logger.log("RTK天线2数据：--经度：" + lonC + ";纬度:" + latC + ";海拔:" + altC + ";定位模式:" + qualC,isPrint); @@ -448,140 +470,169 @@ public class SerialConnectUtils { } } } else if ("02".equals(orderType)) { + //开始测量命令 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: "); - for (RadarPointBean point : group) { - System.out.print("(Angle: " + point.getAngle() + ", Distance: " + point.getDistance() + ") "); +// //解析数据 +// String data2 = result.substring(0, result.length() - 2); +// String check2 = substring; +// String sum162 = ByteUtil.getSum16(data2); +// //校验数据 +// if (check2.equals(sum162)){ +// //获取本次测量的导线以及测量位置 +// String wirePosition = data2.substring(4, 8); +// int wirePositionInt = ByteUtil.hexStrToDecInt(wirePosition) / 100; +// if (wirePositionInt>5){ +// //单回路 +// loopType=1; +// } else { +// //双回路 +// loopType=2; +// } +// linePosition = wirePositionInt; +// linePositionName =getLinePositionDescription(wirePositionInt); +// String uavPositionStr = data2.substring(8, 10); +// if ("01".equals(uavPositionStr)){ +// uavPosition="大号在右"; +// } else if ("02".equals(uavPositionStr)){ +// uavPosition="大号在左"; +// } + + + + 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: "); + for (RadarPointBean point : group) { + System.out.print("(Angle: " + point.getAngle() + ", Distance: " + point.getDistance() + ") "); + } + System.out.println(); + // 计算 《根据算法去调用不同的计算方法》 + RadarPointBean stdDevPoint = getMiddlePoint(group); + 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); } - System.out.println(); - // 计算 《根据算法去调用不同的计算方法》 - RadarPointBean stdDevPoint = getMiddlePoint(group); - 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); - } - //2.3判断是否返回对应结果集（即数据集合点数量等于导线分裂数量） + //2.3判断是否返回对应结果集（即数据集合点数量等于导线分裂数量） // if (calStandardDeviation.size() == towerDataBean.getSplitType()) { - //雷达数据不匹配时，返回其他数据 - boolean radarStatus = false; - //2.4根据公式去转换成测点坐标 - List coordinates = new ArrayList<>(); - if (calStandardDeviation.size() >= towerDataBean.getSplitType()) { - radarStatus = true; - //根据设定的分裂数量进行截取数据条数 - calStandardDeviation = calStandardDeviation.subList(0, towerDataBean.getSplitType()); + //雷达数据不匹配时，返回其他数据 + boolean radarStatus = false; + //2.4根据公式去转换成测点坐标 + List coordinates = new ArrayList<>(); + if (calStandardDeviation.size() >= towerDataBean.getSplitType()) { + radarStatus = true; + //根据设定的分裂数量进行截取数据条数 + calStandardDeviation = calStandardDeviation.subList(0, towerDataBean.getSplitType()); - for (RadarPointBean radarPointBean : calStandardDeviation) { + for (RadarPointBean radarPointBean : calStandardDeviation) { // 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); + //雷达点转测点，计算测点经纬度 + 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("雷达点经纬度：--经度："+sensorDataBean.getLon()+"纬度："+sensorDataBean.getLat()+"高度："+sensorDataBean.getH(),isPrint); - logger.log("测点经纬度：--经度："+coordinate.getLon()+"纬度："+coordinate.getLat()+"高度："+coordinate.getH(),isPrint); + logger.log("雷达点经纬度：--经度："+sensorDataBean.getLon()+"纬度："+sensorDataBean.getLat()+"高度："+sensorDataBean.getH(),isPrint); + logger.log("测点经纬度：--经度："+coordinate.getLon()+"纬度："+coordinate.getLat()+"高度："+coordinate.getH(),isPrint); - } - //2.5导线编号 - //先以0-90度为标准的导线if + } + //2.5导线编号 + //先以0-90度为标准的导线if - //判断无人机位置，上或下 - int re = checkAngleDistribution(coordinates); - if (re == 0) { - //提示该位置不合适 - SerialHelpers serialHelper = serialHelperMap.get("数传"); - assert serialHelper != null; - serialHelper.sendHex("F0000003"); - return; - } else { - //给导线编号 - classifyAndSortCoordinates(coordinates, true, re); + //判断无人机位置，上或下 + 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,根据编成指令返回遥控器端 - //生成返回指令 - StringBuilder sb = new StringBuilder(); - sb.append("F0F0F0"); - sb.append("03"); - //雷达数据 - if(radarStatus){ - for (int m = 0; m < (Math.min(coordinates.size(), 4)); 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); - sb.append(ByteUtil.decIntToHexStr(distance, 2)); - sb.append(ByteUtil.decIntToHexStr(angle, 2)); + //2.6 已编号结束的雷达数据 coordinates,传感器测量数据 sensorDataBean,根据编成指令返回遥控器端 + //生成返回指令 + StringBuilder sb = new StringBuilder(); + sb.append("F0F0F0"); + sb.append("03"); + //雷达数据 + if(radarStatus){ + for (int m = 0; m < (Math.min(coordinates.size(), 4)); 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); + sb.append(ByteUtil.decIntToHexStr(distance, 2)); + sb.append(ByteUtil.decIntToHexStr(angle, 2)); + } + // 如果 coordinates.size() 小于 4，填充 "0000" + int remaining = 4 - coordinates.size(); + for (int i = 0; i < remaining; i++) { + sb.append("00000000"); + } + }else{ + for (int i = 0; i < 4; i++) { + sb.append("00000000"); + } } - // 如果 coordinates.size() 小于 4，填充 "0000" - int remaining = 4 - coordinates.size(); - for (int i = 0; i < remaining; i++) { - sb.append("00000000"); - } - }else{ - for (int i = 0; i < 4; i++) { - sb.append("00000000"); - } - } - //传感器数据 - //俯仰角 - int pitchAngle = (int) (ArithUtil.round(sensorDataBean.getPitchAngle(), 2) * 100); - sb.append(ByteUtil.decIntToHexStr(pitchAngle, 4)); - //横滚角 - 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)); - //GPS经纬度高程定位模式 - long lon = (long) (ArithUtil.round(sensorDataBean.getLon(), 8) * 100000000); - sb.append(ByteUtil.decLongToHexStr(lon, 5)); - long lat = (long) (ArithUtil.round(sensorDataBean.getLat(), 8) * 100000000); - sb.append(ByteUtil.decLongToHexStr(lat, 5)); - int h = (int) (ArithUtil.round(sensorDataBean.getH(), 4) * 10000); - sb.append(ByteUtil.decIntToHexStr(h, 3)); - int qual = Integer.parseInt(sensorDataBean.getPositioningMode() == null ? "0" : sensorDataBean.getPositioningMode()); - sb.append(ByteUtil.decIntToHexStr(qual, 1)); - //将即分裂返回，方便处理数据 - sb.append("0" + towerDataBean.getSplitType()); + //传感器数据 + //俯仰角 + int pitchAngle = (int) (ArithUtil.round(sensorDataBean.getPitchAngle(), 2) * 100); + sb.append(ByteUtil.decIntToHexStr(pitchAngle, 4)); + //横滚角 + 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)); + //GPS经纬度高程定位模式 + long lon = (long) (ArithUtil.round(sensorDataBean.getLon(), 8) * 100000000); + sb.append(ByteUtil.decLongToHexStr(lon, 5)); + long lat = (long) (ArithUtil.round(sensorDataBean.getLat(), 8) * 100000000); + sb.append(ByteUtil.decLongToHexStr(lat, 5)); + int h = (int) (ArithUtil.round(sensorDataBean.getH(), 4) * 10000); + sb.append(ByteUtil.decIntToHexStr(h, 3)); + int qual = Integer.parseInt(sensorDataBean.getPositioningMode() == null ? "0" : sensorDataBean.getPositioningMode()); + sb.append(ByteUtil.decIntToHexStr(qual, 1)); + //将即分裂返回，方便处理数据 + sb.append("0" + towerDataBean.getSplitType()); + + if(radarStatus) { + //2.5-1.0后续添加逻辑，增加求点到雷达夹角，确认最佳测量角度 + double[] goodAngle = dealWithCoordinatesAngel(coordinates, sensorDataBean); + //添加最佳测量角 + int goodPitchAngle = (int) (ArithUtil.round(Math.abs(goodAngle[0]), 2) * 100); + sb.append(ByteUtil.decIntToHexStr(goodPitchAngle, 2)); + int goodRollAngle = (int) (ArithUtil.round(Math.abs(goodAngle[1]), 2) * 100); + sb.append(ByteUtil.decIntToHexStr(goodRollAngle, 2)); + }else { + sb.append("0000"); + sb.append("0000"); + } + + String checkByte = ByteUtil.getSum16(sb.toString()); + sb.append(checkByte); + sb.append("CCDD"); + SerialHelpers serialHelper = serialHelperMap.get("数传"); + assert serialHelper != null; + Log.e(TAG, "2.测量数据返回：" + sb.toString()); + logger.log("传给手柄的数据：--"+sb.toString(),isPrint); + serialHelper.sendHex(sb.toString()); +// } + - if(radarStatus) { - //2.5-1.0后续添加逻辑，增加求点到雷达夹角，确认最佳测量角度 - double[] goodAngle = dealWithCoordinatesAngel(coordinates, sensorDataBean); - //添加最佳测量角 - int goodPitchAngle = (int) (ArithUtil.round(Math.abs(goodAngle[0]), 2) * 100); - sb.append(ByteUtil.decIntToHexStr(goodPitchAngle, 2)); - int goodRollAngle = (int) (ArithUtil.round(Math.abs(goodAngle[1]), 2) * 100); - sb.append(ByteUtil.decIntToHexStr(goodRollAngle, 2)); - }else { - sb.append("0000"); - sb.append("0000"); - } - String checkByte = ByteUtil.getSum16(sb.toString()); - sb.append(checkByte); - sb.append("CCDD"); - 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("数传"); // assert serialHelper != null; @@ -589,8 +640,7 @@ public class SerialConnectUtils { // Log.e(TAG, "激光雷达解析数据与分裂类型不匹配：" + calStandardDeviation); // } } else if ("03".equals(orderType)) { - isPrint=1; - logger.log("遥控器端保存数据",isPrint); + logger.log("遥控器端保存数据",1); isPrint=0; } else if ("04".equals(orderType)) { String position = result.substring(4, 6); @@ -603,6 +653,15 @@ public class SerialConnectUtils { uavPosition="大号在左"; System.out.println("无人机测量位置："+uavPosition); } + } else if ("05".equals(orderType)) { + String position = result.substring(4, 6); + if ("01".equals(position)){ + //停止记录悬停数据 + isRecord=0; + } else if ("02".equals(position)){ + //开始记录悬停数据 + isRecord=1; + } } } } @@ -683,12 +742,53 @@ 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); +// 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() secondDistance) { + first.setWireNum(1); + second.setWireNum(2); + } + // 情况3：角度大且距离小的是1号，角度小且距离大的是2号 + else if (firstAngle > secondAngle && firstDistance < secondDistance) { + first.setWireNum(1); + second.setWireNum(2); + } + // 情况4：角度大且距离大的是1号，角度小且距离小的是2号 + else if (firstAngle > secondAngle && firstDistance > secondDistance) { + first.setWireNum(2); + second.setWireNum(1); + } + break; + default: + break; } }else if (size == 4) { switch (uavPosition) { @@ -1054,5 +1154,44 @@ public class SerialConnectUtils { } } + /** + * 根据传入的数字返回对应的导线位置描述 + * + * @param linePosition 导线位置编号 + * @return 对应的文字描述，如果编号无效，则返回"未知位置" + */ + public static String getLinePositionDescription(int linePosition) { + switch (linePosition) { + case 1: + return "左地线/光缆"; + case 2: + return "右地线/光缆"; + case 3: + return "左相"; + case 4: + return "中相"; + case 5: + return "右相"; + case 6: + return "左地线/光缆"; + case 7: + return "地线/光缆"; + case 8: + return "左上相"; + case 9: + return "左中相"; + case 10: + return "左下相"; + case 11: + return "右上相"; + case 12: + return "右中相"; + case 13: + return "右下相"; + default: + return "未知位置"; + } + } + } 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 6fc7234..ca85a3d 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 @@ -84,6 +84,7 @@ public class RadarMathUtil { * hGps 高程 * azimuthAngle 无人机方位角（罗盘角度） * 需根据实际情况进行调整 + * 雷达点转测点 * @return */ public static Coordinate transferPoint2(RadarPointBean radarPointBean, SensorDataBean sensorDataBean) {