From 4b232345e0686343508dc49760c20c03310a4f26 Mon Sep 17 00:00:00 2001
From: Xiajijian <1176061145@qq.com>
Date: Thu, 22 Jan 2026 09:28:17 +0800
Subject: [PATCH] 1. 执行航线的时候修改航线任务表执行状态。
---
ard-route/src/main/java/com/ard/route/util/InitializationUtils.java | 255 +++++++++++++++++++++++++++++++++++++++++++++++++-
1 files changed, 246 insertions(+), 9 deletions(-)
diff --git a/ard-route/src/main/java/com/ard/route/util/InitializationUtils.java b/ard-route/src/main/java/com/ard/route/util/InitializationUtils.java
index 187f75e..e114575 100644
--- a/ard-route/src/main/java/com/ard/route/util/InitializationUtils.java
+++ b/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,255 @@
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()));
+ }
}
}
}
}
- 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);
+// }
+
}
--
Gitblit v1.9.3