forked from uav/ard-uav-server
ard-route/src/main/java/com/ard/route/util/InitializationUtils.java
@@ -4,9 +4,15 @@ import com.ard.route.domain.sixhe.ArdRouteActions; import com.ard.route.domain.sixhe.ArdRouteChange; import com.ard.route.domain.sixhe.ArdRoutePoints; import com.ard.route.enums.kml.DroneEnumValueEnums; import com.ard.route.enums.kml.PayloadTypeEnums; import java.util.ArrayList; import java.util.List; import java.util.Objects; import static com.ard.route.enums.kml.DroneEnumValueEnums.M350_RTK; import static com.ard.route.util.ToolUtils.getDoublePoint; /** * 初始化数据 @@ -14,7 +20,12 @@ */ 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,14 +34,25 @@ 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); uavRouteReq.setPayloadType(43); // 机场1：M30/M30T 使用机身自带双光/三光相机，payloadEnumValue 分别为 52/53， // 不能沿用 H20T(43)。其余机型保持原有逻辑，机场2/3 的航线生成结果不变。 if (Objects.equals(DroneEnumValueEnums.M30_M30T.getValue(), uavRouteReq.getDroneType())) { if (uavRouteReq.getSubDroneType() == null) { uavRouteReq.setSubDroneType(0); } uavRouteReq.setPayloadType(Objects.equals(1, uavRouteReq.getSubDroneType()) ? PayloadTypeEnums.M30T.getValue() : PayloadTypeEnums.M30.getValue()); } else { uavRouteReq.setSubDroneType(0); uavRouteReq.setPayloadType(43); } uavRouteReq.setPayloadPosition(0); uavRouteReq.setImageFormat("wide"); uavRouteReq.setFinishAction("goHome"); @@ -44,112 +66,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; } /** * 初始化数据 */ private void initializationSixHe(){ ArdRouteChange ardRouteChange = new ArdRouteChange(); ardRouteChange.setAutoSpeed(8); ardRouteChange.setFinishedAction(1); ardRouteChange.setHomeHeight(50); //ardRouteChange.setInterests(0); ardRouteChange.setIsSaveEnergyMode(0); ardRouteChange.setLoseAction(0); ardRouteChange.setMaxSpeed(15); List<ArdRoutePoints> points = new ArrayList<>(); ArdRoutePoints ardRoutePoints0 = new ArdRoutePoints(); ardRoutePoints0.setDampingDistance(0); ardRoutePoints0.setFlightPathMode(1); ardRoutePoints0.setGroupIndex(0); ardRoutePoints0.setHeading(0); ardRoutePoints0.setHeadingMode(0); ardRoutePoints0.setHeight(50); ardRoutePoints0.setInterestIndex(0); ardRoutePoints0.setLat(31.11339916); ardRoutePoints0.setLon(103.75851462); ardRoutePoints0.setSafeGohomeFlag(0); ardRoutePoints0.setSpeed(8); List<ArdRouteActions> actions = new ArrayList<>(); 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); actions.add(ardRouteActions0); actions.add(ardRouteActions1); actions.add(ardRouteActions2); points.add(ardRoutePoints0); ardRouteChange.setPoints(points); public static Double multiple23(Double zoom){ return zoom * CHANSI_H20T; } // 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); // // // } public static Double divided23(Double zoom){ return zoom / CHANSI_H20T; } // 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); // } }
