forked from uav/ard-uav-server
ard-route/src/main/java/com/ard/route/util/InitializationUtils.java
@@ -1,7 +1,15 @@ package com.ard.route.util; import com.ard.route.domain.djuav.*; import com.ard.route.domain.sixhe.ArdRouteActions; import com.ard.route.domain.sixhe.ArdRouteChange; import com.ard.route.domain.sixhe.ArdRoutePoints; 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; /** * 初始化数据 @@ -9,7 +17,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"; /** @@ -39,31 +52,256 @@ 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){ 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(multiple_23(action.getZoom())); //action.setZoom(multiple_6(action.getZoom())); } } } } } return uavRouteReq; } public static Double multiple_23(Double zoom){ return zoom * CHANSI_H20T; } public static Double divided_23(Double zoom){ return zoom / CHANSI_H20T; } /** * 初始化数据 */ 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); ardRouteChange.setSecurityHeight(50); ardRouteChange.setTemplateId(0); List<ArdRoutePoints> points = new ArrayList<>(); //----------------------------------------------------- ArdRoutePoints ardRoutePoints00 = new ArdRoutePoints(); ardRoutePoints00.setDampingDistance(0); ardRoutePoints00.setFlightPathMode(1); ardRoutePoints00.setGroupIndex(0); ardRoutePoints00.setHeading(0); ardRoutePoints00.setHeadingMode(0); ardRoutePoints00.setHeight(50); ardRoutePoints00.setInterestIndex(-1); ardRoutePoints00.setLat(31.11339916); ardRoutePoints00.setLon(103.75851462); ardRoutePoints00.setSafeGohomeFlag(0); ardRoutePoints00.setSpeed(8); List<ArdRouteActions> actions00 = new ArrayList<>(); ArdRouteActions ardRouteActions00 = new ArdRouteActions(); ardRouteActions00.setParam(10); ardRouteActions00.setSpeakInfo(""); ardRouteActions00.setType(1); ardRouteActions00.setWaitTime(2); ArdRouteActions ardRouteActions01 = new ArdRouteActions(); ardRouteActions01.setParam(10); ardRouteActions01.setSpeakInfo(""); ardRouteActions01.setType(1); ardRouteActions01.setWaitTime(2); ArdRouteActions ardRouteActions02 = new ArdRouteActions(); ardRouteActions02.setParam(10); ardRouteActions02.setSpeakInfo(""); ardRouteActions02.setType(1); ardRouteActions02.setWaitTime(2); actions00.add(ardRouteActions00); actions00.add(ardRouteActions01); actions00.add(ardRouteActions02); ardRoutePoints00.setActions(actions00); //----------------------------------------------------- ArdRoutePoints ardRoutePoints10 = new ArdRoutePoints(); ardRoutePoints10.setDampingDistance(0); ardRoutePoints10.setFlightPathMode(1); ardRoutePoints10.setGroupIndex(0); ardRoutePoints10.setHeading(0); ardRoutePoints10.setHeadingMode(0); ardRoutePoints10.setHeight(50); ardRoutePoints10.setInterestIndex(-1); ardRoutePoints10.setLat(31.11339916); ardRoutePoints10.setLon(103.75746538); ardRoutePoints10.setSafeGohomeFlag(0); ardRoutePoints10.setSpeed(8); List<ArdRouteActions> actions10 = new ArrayList<>(); ArdRouteActions ardRouteActions10 = new ArdRouteActions(); ardRouteActions10.setParam(20); ardRouteActions10.setSpeakInfo(""); ardRouteActions10.setType(9); ardRouteActions10.setWaitTime(2); ArdRouteActions ardRouteActions11 = new ArdRouteActions(); ardRouteActions11.setParam(40); ardRouteActions11.setSpeakInfo(""); ardRouteActions11.setType(6); ardRouteActions11.setWaitTime(2); ArdRouteActions ardRouteActions12 = new ArdRouteActions(); ardRouteActions12.setParam(40); ardRouteActions12.setSpeakInfo(""); ardRouteActions12.setType(7); ardRouteActions12.setWaitTime(2); ArdRouteActions ardRouteActions13 = new ArdRouteActions(); ardRouteActions13.setParam(40); ardRouteActions13.setSpeakInfo(""); ardRouteActions13.setType(8); ardRouteActions13.setWaitTime(2); actions10.add(ardRouteActions10); actions10.add(ardRouteActions11); actions10.add(ardRouteActions12); ardRoutePoints10.setActions(actions10); //----------------------------------------------------- ArdRoutePoints ardRoutePoints20 = new ArdRoutePoints(); ardRoutePoints20.setDampingDistance(0); ardRoutePoints20.setFlightPathMode(1); ardRoutePoints20.setGroupIndex(0); ardRoutePoints20.setHeading(0); ardRoutePoints20.setHeadingMode(0); ardRoutePoints20.setHeight(50); ardRoutePoints20.setInterestIndex(-1); ardRoutePoints20.setLat(31.11250084); ardRoutePoints20.setLon(103.75746538); ardRoutePoints20.setSafeGohomeFlag(0); ardRoutePoints20.setSpeed(8); List<ArdRouteActions> actions20 = new ArrayList<>(); ArdRouteActions ardRouteActions20 = new ArdRouteActions(); ardRouteActions20.setParam(15); ardRouteActions20.setSpeakInfo(""); ardRouteActions20.setType(9); ardRouteActions20.setWaitTime(2); ArdRouteActions ardRouteActions21 = new ArdRouteActions(); ardRouteActions21.setParam(3); ardRouteActions21.setSpeakInfo(""); ardRouteActions21.setType(10); ardRouteActions21.setWaitTime(2); actions20.add(ardRouteActions20); actions20.add(ardRouteActions21); ardRoutePoints20.setActions(actions20); //----------------------------------------------------- ArdRoutePoints ardRoutePoints30 = new ArdRoutePoints(); ardRoutePoints30.setDampingDistance(0); ardRoutePoints30.setFlightPathMode(1); ardRoutePoints30.setGroupIndex(0); ardRoutePoints30.setHeading(0); ardRoutePoints30.setHeadingMode(0); ardRoutePoints30.setHeight(60); ardRoutePoints30.setInterestIndex(-1); ardRoutePoints30.setLat(31.11250084); ardRoutePoints30.setLon(103.75851462); ardRoutePoints30.setSafeGohomeFlag(0); ardRoutePoints30.setSpeed(8); List<ArdRouteActions> actions30 = new ArrayList<>(); ArdRouteActions ardRouteActions30 = new ArdRouteActions(); ardRouteActions30.setParam(1); ardRouteActions30.setSpeakInfo(""); ardRouteActions30.setType(2); ardRouteActions30.setWaitTime(2); actions30.add(ardRouteActions30); ardRoutePoints30.setActions(actions30); points.add(ardRoutePoints00); points.add(ardRoutePoints10); points.add(ardRoutePoints20); points.add(ardRoutePoints30); 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); // } }
