forked from uav/ard-uav-server
ard-route/src/main/java/com/ard/route/util/InitializationUtils.java
@@ -8,13 +8,21 @@ import java.util.ArrayList; import java.util.List; import static com.ard.route.enums.kml.DroneEnumValueEnums.M350_RTK; import static com.ard.route.util.ToolUtils.getDoublePoint; /** * 初始化数据 * 测试数据 */ public class InitializationUtils { private static final Double CHANSI_H20 = 23.76237623; // 禅思H20T 公倍数 private static final Double CHANSI_H20T = 23.76237623; // 禅思H20T 焦距值 private static final Double CHANSI_H20_FOCUS = 6.83; private static final String WAYPOINT_TURN_MODE_DEFAULT = "toPointAndStopWithDiscontinuityCurvature"; /** @@ -23,10 +31,11 @@ public UavRouteReq initialization(UavRouteReq uavRouteReq){ uavRouteReq.setTemplateType("waypoint"); // 起始点默认航线第一个点 if (uavRouteReq.getTakeOffRefPoint().isEmpty()){ if (uavRouteReq.getTakeOffRefPoint() == null){ uavRouteReq.setTakeOffRefPoint( uavRouteReq.getRoutePointList().get(0).getLatitude() + "," + uavRouteReq.getRoutePointList().get(0).getLongitude() + "," + "0"); uavRouteReq.getRoutePointList().get(0).getLongitude() + "," + uavRouteReq.getRoutePointList().get(0).getHeight()); } //uavRouteReq.setDroneType(89); uavRouteReq.setSubDroneType(0); @@ -44,28 +53,75 @@ if (uavRouteReq.getGlobalWaypointTurnMode() != null){ waypointTurnReq.setWaypointTurnMode(uavRouteReq.getGlobalWaypointTurnMode()); }else { waypointTurnReq.setWaypointTurnMode("toPointAndStopWithDiscontinuityCurvature"); waypointTurnReq.setWaypointTurnMode(WAYPOINT_TURN_MODE_DEFAULT); } uavRouteReq.setWaypointTurnReq(waypointTurnReq); uavRouteReq.setGimbalPitchMode("usePointSetting"); //buildZoom(uavRouteReq); if (M350_RTK.getValue().equals(uavRouteReq.getDroneType())) { // 变焦数值加工 buildZoom(uavRouteReq); } // 修改每个航点的高度 buildHeight(uavRouteReq); // 动作组编号数据加工 buildActionGroupReq(uavRouteReq); return uavRouteReq; } /** * 构建动作组编号 */ public void buildActionGroupReq(UavRouteReq uavRouteReq){ int pointActionCounter = 0; for (RoutePointReq routePointReq : uavRouteReq.getRoutePointList()) { if (routePointReq.getActionGroupList() != null){ for (ActionGroupReq actionGroupReq : routePointReq.getActionGroupList()) { actionGroupReq.setActionGroupId(pointActionCounter++); actionGroupReq.setActionGroupStartIndex(routePointReq.getRoutePointIndex()); actionGroupReq.setActionGroupEndIndex(routePointReq.getRoutePointIndex()); actionGroupReq.setActionTriggerType("reachPoint"); } } } } /** * 修改每个航点的高度 * 根据起飞点高度减去当前航点高度 */ public void buildHeight(UavRouteReq uavRouteReq){ if (!"EGM96".equals(uavRouteReq.getHeightMode())){ double takeOffHeight = getDoublePoint(uavRouteReq.getTakeOffRefPoint(),"alt"); for (RoutePointReq routePointReq : uavRouteReq.getRoutePointList()) { routePointReq.setHeight(routePointReq.getHeight() - takeOffHeight); } } } /** * 变焦动作加工 */ private UavRouteReq buildZoom(UavRouteReq uavRouteReq){ private void buildZoom(UavRouteReq uavRouteReq){ for (RoutePointReq routePointReq : uavRouteReq.getRoutePointList()) { for (ActionGroupReq actionGroupReq : routePointReq.getActionGroupList()) { for (PointActionReq action : actionGroupReq.getActions()) { if (action.getZoom() != null){ action.setZoom(action.getZoom() * CHANSI_H20); if (!routePointReq.getActionGroupList().isEmpty()){ for (ActionGroupReq actionGroupReq : routePointReq.getActionGroupList()) { for (PointActionReq action : actionGroupReq.getActions()) { if (action.getZoom() != null){ action.setZoom(multiple23(action.getZoom())); } } } } } return uavRouteReq; } public static Double multiple23(Double zoom){ return zoom * CHANSI_H20T; } public static Double divided23(Double zoom){ return zoom / CHANSI_H20T; } /** @@ -215,53 +271,4 @@ ardRouteChange.setPoints(points); } // private ArdRoutePoints buildArdRoutePoints(){ // ArdRoutePoints ardRoutePoints1 = new ArdRoutePoints(); // ardRoutePoints1.setDampingDistance(0); // ardRoutePoints1.setFlightPathMode(1); // ardRoutePoints1.setGroupIndex(0); // ardRoutePoints1.setHeading(0); // ardRoutePoints1.setHeadingMode(0); // ardRoutePoints1.setHeight(50); // ardRoutePoints1.setInterestIndex(0); // ardRoutePoints1.setLat(31.11339916); // ardRoutePoints1.setLon(103.75851462); // ardRoutePoints1.setSafeGohomeFlag(0); // ardRoutePoints1.setSpeed(8); // // // } // private ArdRouteActions buildArdRouteActions(){ // ArdRouteActions ardRouteActions0 = new ArdRouteActions(); // ardRouteActions0.setParam(10); // ardRouteActions0.setSpeakInfo(""); // ardRouteActions0.setType(1); // ardRouteActions0.setWaitTime(2); // ArdRouteActions ardRouteActions1 = new ArdRouteActions(); // ardRouteActions1.setParam(10); // ardRouteActions1.setSpeakInfo(""); // ardRouteActions1.setType(1); // ardRouteActions1.setWaitTime(2); // ArdRouteActions ardRouteActions2 = new ArdRouteActions(); // ardRouteActions2.setParam(10); // ardRouteActions2.setSpeakInfo(""); // ardRouteActions2.setType(1); // ardRouteActions2.setWaitTime(2); // } public byte[] testParameters() { String data = "74 79 01 3C 44 42 4D 32 35 30 39 37 34 30 36 35 30 30 38 D1 00 10 08 01 15 00 00 70 41 1D 00 00 00 41 22 4A 09 06 9C E7 80 8B F0 59 40 11 08 98 33 BA 07 1D 3F 40 1D 00 00 F0 41 25 00 00 00 41 2A 09 08 01 15 00 00 20 41 18 02 2A 09 08 03 15 00 00 80 3F 18 02 2A 09 08 04 15 00 00 80 3F 18 02 30 01 58 FF FF FF FF FF FF FF FF FF 01 22 55 09 F1 BC 12 50 7A F0 59 40 11 08 98 33 BA 07 1D 3F 40 1D 00 00 20 42 25 00 00 00 41 2A 09 08 09 15 00 00 A0 41 18 02 2A 09 08 06 15 00 00 20 42 18 02 2A 09 08 07 15 00 00 20 42 18 02 2A 09 08 08 15 00 00 20 42 18 02 30 01 58 FF FF FF FF FF FF FF FF FF 01 22 3F 09 F1 BC 12 50 7A F0 59 40 11 91 92 E4 DA CC 1C 3F 40 1D 00 00 48 42 25 00 00 00 41 2A 09 08 09 15 00 00 70 41 18 02 2A 09 08 0A 15 00 00 40 40 18 02 30 01 58 FF FF FF FF FF FF FF FF FF 01 22 34 09 06 9C E7 80 8B F0 59 40 11 91 92 E4 DA CC 1C 3F 40 1D 00 00 70 42 25 00 00 00 41 2A 09 08 02 15 00 00 80 3F 18 02 30 01 58 FF FF FF FF FF FF FF FF FF 01 30 32 70 32"; String[] hexStrings = data.split(" "); StringBuilder sb = new StringBuilder(); sb.append("byte[] encodedBytes = {"); for (int i = 0; i < hexStrings.length; i++) { sb.append("(byte) 0x").append(hexStrings[i]); if (i < hexStrings.length - 1) { sb.append(", "); } } sb.append("};"); return sb.toString().getBytes(); } }
