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

算法调整

This commit is contained in:
hayu 2025-04-21 15:09:43 +08:00
parent b9e54e290f
commit 4df115646b
7 changed files with 618 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

139
app/src/main/java/com/bonus/uav/db/MeasurementDataDao.java Normal file
View File

@ -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<MeasurementDataBean> selectAll() {
String sql = "SELECT * FROM bm_measurement_data";
Cursor cr = null;
List<MeasurementDataBean> 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());
}
}
}

63
app/src/main/java/com/bonus/uav/entity/MeasurementDataBean.java Normal file
View File

@ -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; }
}

3
app/src/main/java/com/bonus/uav/utils/DBHelper.java
View File

@ -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);

125
app/src/main/java/com/bonus/uav/utils/FileHoverLogger.java Normal file
View File

@ -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);
}
}

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

@ -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};
}

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

@ -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<String, SerialHelper> serialHelperMap = new HashMap<>();
Map<String, SerialHelpers> serialHelperMap = new HashMap<>();
@ -86,6 +103,9 @@ public class SerialConnectUtils {
/** 是否记录日志 */
private int isPrint=0;
/** 是否记录悬停数据 */
private int isRecord=0;
public SerialConnectUtils(ScheduledThreadManager threadManager, List<MapBean> 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<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: ");
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<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: ");
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<Coordinate> coordinates = new ArrayList<>();
if (calStandardDeviation.size() >= towerDataBean.getSplitType()) {
radarStatus = true;
//根据设定的分裂数量进行截取数据条数
calStandardDeviation = calStandardDeviation.subList(0, towerDataBean.getSplitType());
//雷达数据不匹配时，返回其他数据
boolean radarStatus = false;
//2.4根据公式去转换成测点坐标
List<Coordinate> 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()<second.getRadarPoint().getAngle()){
first.setWireNum(1);
second.setWireNum(2);
} else {
first.setWireNum(2);
second.setWireNum(1);
}
break;
case "大号在右":
double firstAngle = first.getRadarPoint().getAngle();
double secondAngle = second.getRadarPoint().getAngle();
double firstDistance = first.getRadarPoint().getDistance();
double secondDistance = second.getRadarPoint().getDistance();
// 情况1：角度小且距离小的是2号，角度大且距离大的是1号
if (firstAngle < secondAngle && firstDistance < secondDistance) {
first.setWireNum(2);
second.setWireNum(1);
}
// 情况2：角度小且距离大的是1号，角度大且距离小的是2号
else if (firstAngle < secondAngle && firstDistance > 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 "未知位置";
}
}
}

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

@ -84,6 +84,7 @@ public class RadarMathUtil {
* hGps 高程
* azimuthAngle 无人机方位角（罗盘角度）
* 需根据实际情况进行调整
* 雷达点转测点
* @return
*/
public static Coordinate transferPoint2(RadarPointBean radarPointBean, SensorDataBean sensorDataBean) {