From 073239cedaab68465e109b1957144f5fca17958d Mon Sep 17 00:00:00 2001
From: liusuyi <1951119284@qq.com>
Date: Thu, 18 Dec 2025 12:22:18 +0800
Subject: [PATCH] 增加航线状态机
---
ard-work/src/main/java/com/ard/work/service/impl/TyServiceImpl.java | 116 +++++++++++++++++++++++++++++++++++++++++++++++++++-------
1 files changed, 102 insertions(+), 14 deletions(-)
diff --git a/ard-work/src/main/java/com/ard/work/service/impl/TyServiceImpl.java b/ard-work/src/main/java/com/ard/work/service/impl/TyServiceImpl.java
index bb840da..949e296 100644
--- a/ard-work/src/main/java/com/ard/work/service/impl/TyServiceImpl.java
+++ b/ard-work/src/main/java/com/ard/work/service/impl/TyServiceImpl.java
@@ -14,7 +14,7 @@
import com.ard.work.component.netty.command.control.SpeedControlCmd;
import com.ard.work.component.netty.command.manual.*;
import com.ard.work.component.netty.command.notice.BoxPowerStatusReplyCmd;
-import com.ard.work.component.netty.command.notice.FlyWayEndReplyCmd;
+import com.ard.work.component.netty.command.notice.WayLineFinishedReplyCmd;
import com.ard.work.component.netty.command.notice.UavReadyReplyCmd;
import com.ard.work.component.netty.command.real.SignalInfoCmd;
import com.ard.work.component.netty.command.real.UavStateCmd;
@@ -26,6 +26,7 @@
import com.ard.work.component.netty.handler.ErrorCodeEnum;
import com.ard.work.component.netty.protobuf.TelemetryDataPB;
import com.ard.work.component.websocket.utils.WebSocketUtil;
+import com.ard.work.config.RouteStateMachineService;
import com.ard.work.domain.cache.UavSignalInfoCache;
import com.ard.work.domain.cache.UavStateCache;
import com.ard.work.domain.cache.UavTelemetryDataCache;
@@ -48,6 +49,7 @@
import com.ard.work.domain.dto.wayline.*;
import com.ard.work.domain.entity.ArdDroneInfo;
import com.ard.work.domain.entity.ArdServerInfo;
+import com.ard.work.domain.enums.RouteEvent;
import com.ard.work.domain.message.ServerReplyMessage;
import com.ard.work.domain.message.UavControlType;
import com.ard.work.domain.receiver.*;
@@ -55,6 +57,7 @@
import com.ard.work.service.IArdServerInfoService;
import com.ard.work.service.ITyService;
import lombok.extern.slf4j.Slf4j;
+import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;
import javax.annotation.Resource;
@@ -77,6 +80,8 @@
private IArdDroneInfoService ardDroneService;
@Resource
private TyClient tyClient;
+ @Autowired
+ private RouteStateMachineService stateMachineService;
/**
* 获取边缘服务器token信息
@@ -123,8 +128,8 @@
if (ardDroneInfo == null) {
throw new ControlException("未找到对应的飞行器信息,boxSn:{}" + boxSn);
}
-
-
+
+
TyHttpCommonReceiver<BoxStreamUrlReceiver> boxLiveUrl = tyClient.getBoxLiveUrl(ardDroneInfo.getServerId(),
boxSn);
return boxLiveUrl == null ? null : boxLiveUrl.getContent();
@@ -175,7 +180,7 @@
throw new ControlException("未找到对应的飞行器信息,boxSn:{}" + boxSn);
}
TyHttpCommonReceiver<?> result = tyClient.updateBoxSettings(ardDroneInfo.getServerId(), boxSettingsDTO);
- return result.getState() == 1;
+ return result.getState() == 0;
}
/**
@@ -284,6 +289,18 @@
}
SpeedControlCmd speedControlCommand = new SpeedControlCmd(speedControlDto);
this.sendMessage(ardDroneInfo.getServerId(), speedControlCommand);
+ return true;
+ }
+
+ @Override
+ public Boolean formatSdCard(FormatSdCardDTO formatSdCardDTO) {
+ String boxSn = formatSdCardDTO.getBoxSn();
+ ArdDroneInfo ardDroneInfo = ardDroneService.selectArdDroneByboxSn(boxSn);
+ if (ardDroneInfo == null) {
+ throw new ControlException("未找到对应的飞行器信息,boxSn:{}" + boxSn);
+ }
+ FormatSdCardCmd formatSdCardCmd = new FormatSdCardCmd(formatSdCardDTO);
+ this.sendMessage(ardDroneInfo.getServerId(), formatSdCardCmd);
return true;
}
@@ -542,7 +559,7 @@
* 方向控制
*/
@CheckControlRight(UavControlType.FLY)
- @CheckTakeoffAltitude(minAltitude = 30.0)
+ @CheckTakeoffAltitude
@Override
public Boolean flyControl(FlyControlDTO flyControlDTO) {
String boxSn = flyControlDTO.getBoxSn();
@@ -559,7 +576,7 @@
* 指点飞行控制
*/
@CheckControlRight(UavControlType.FLY)
- @CheckTakeoffAltitude(minAltitude = 30.0)
+ @CheckTakeoffAltitude
@CheckNoFlyZone
@Override
public Boolean flyPointControl(FlyPointControlDTO flyPointControlDTO) {
@@ -584,10 +601,41 @@
if (ardDroneInfo == null) {
throw new ControlException("未找到对应的飞行器信息,boxSn:{}" + boxSn);
}
-
-
+
+
FlyPointControlCancelCmd flyPointControlCancelCmd = new FlyPointControlCancelCmd(flyPointControlCancelDTO);
this.sendMessage(ardDroneInfo.getServerId(), flyPointControlCancelCmd);
+ return true;
+ }
+
+
+ /**
+ * 环绕飞行控制
+ */
+ @Override
+ public Boolean FlyRoundControl(FlyRoundControlDTO flyRoundControlDTO) {
+ String boxSn = flyRoundControlDTO.getBoxSn();
+ ArdDroneInfo ardDroneInfo = ardDroneService.selectArdDroneByboxSn(boxSn);
+ if (ardDroneInfo == null) {
+ throw new ControlException("未找到对应的飞行器信息,boxSn:{}" + boxSn);
+ }
+ FlyRoundControlCmd flyRoundControlCmd = new FlyRoundControlCmd(flyRoundControlDTO);
+ this.sendMessage(ardDroneInfo.getServerId(), flyRoundControlCmd);
+ return true;
+ }
+
+ /**
+ * 停止环绕飞行控制
+ */
+ @Override
+ public Boolean FlyRoundControlCancel(FlyRoundControlCancelDTO flyRoundControlCancelDTO) {
+ String boxSn = flyRoundControlCancelDTO.getBoxSn();
+ ArdDroneInfo ardDroneInfo = ardDroneService.selectArdDroneByboxSn(boxSn);
+ if (ardDroneInfo == null) {
+ throw new ControlException("未找到对应的飞行器信息,boxSn:{}" + boxSn);
+ }
+ FlyRoundControlCancelCmd flyRoundControlCancelCmd = new FlyRoundControlCancelCmd(flyRoundControlCancelDTO);
+ this.sendMessage(ardDroneInfo.getServerId(), flyRoundControlCancelCmd);
return true;
}
@@ -602,8 +650,8 @@
if (ardDroneInfo == null) {
throw new ControlException("未找到对应的飞行器信息,boxSn:{}" + boxSn);
}
-
-
+
+
EmergencyBrakeCmd emergencyBrakeCmd = new EmergencyBrakeCmd(emeragencyBrakeDTO);
this.sendMessage(ardDroneInfo.getServerId(), emergencyBrakeCmd);
return true;
@@ -750,7 +798,10 @@
throw new ControlException("未找到对应的飞行器信息,boxSn:{}" + boxSn);
}
//wayLineUploadDTO.setWayLineData(this.CreatePlanLineData());//设置航线数据
+ System.out.println("wayLineUploadDTO:-----------------" + wayLineUploadDTO);
+ log.info("打印数据 ++++ wayLineUploadDTO:-----------------" + wayLineUploadDTO);
WayLineUploadCmd wayLineUploadCmd = new WayLineUploadCmd(wayLineUploadDTO);
+ System.out.println("wayLineUploadCmd:-----------------" + wayLineUploadCmd);
this.sendMessage(ardDroneInfo.getServerId(), wayLineUploadCmd);
return true;
}
@@ -873,7 +924,7 @@
case 0x02:
// 处理注册消息
HeartbeatReplyCmd heartbeatReplyCmd = (HeartbeatReplyCmd) tyCommand;
- logPrint("心跳消息", heartbeatReplyCmd.getTimestamp());
+ //logPrint("心跳消息", heartbeatReplyCmd.getTimestamp());
break;
case 0x03:
// 处理订阅消息
@@ -928,9 +979,10 @@
CameraModeReplyCmd cameraModeReplyCommand = (CameraModeReplyCmd) tyCommand;
logPrint("无人机设置相机模式", cameraModeReplyCommand.getResult(), cameraModeReplyCommand.getErrorCode());
}
- if (tyCommand instanceof FlyWayEndReplyCmd) {
- FlyWayEndReplyCmd flyWayEndReplyCmd = (FlyWayEndReplyCmd) tyCommand;
- logPrint("无人机飞行结束", flyWayEndReplyCmd.getResult(), flyWayEndReplyCmd.getErrorCode());
+ //格式化存储卡
+ if (tyCommand instanceof FormatSdCardReplyCmd) {
+ FormatSdCardReplyCmd formatSdCardReplyCmd = (FormatSdCardReplyCmd) tyCommand;
+ logPrint("格式化存储卡", formatSdCardReplyCmd.getResult(), formatSdCardReplyCmd.getErrorCode());
}
if (tyCommand instanceof CapturePicReplyCmd) {
CapturePicReplyCmd capturePicReplyCommand = (CapturePicReplyCmd) tyCommand;
@@ -992,23 +1044,53 @@
}
//开始航线回复
if (tyCommand instanceof WayLineBeginReplyCmd) {
+
WayLineBeginReplyCmd wayLineBeginReplyCmd = (WayLineBeginReplyCmd) tyCommand;
logPrint("开始航线", wayLineBeginReplyCmd.getResult(), wayLineBeginReplyCmd.getErrorCode());
+ if (wayLineBeginReplyCmd.getResult() == 0) {
+ byte[] boxSnBytes = tyCommand.getBoxSn();
+ String boxSn = new String(boxSnBytes);
+ stateMachineService.sendEvent(boxSn, RouteEvent.START_SUCCESS);
+ }
}
//暂停航线回复
if (tyCommand instanceof WayLinePauseReplyCmd) {
WayLinePauseReplyCmd wayLinePauseReplyCmd = (WayLinePauseReplyCmd) tyCommand;
logPrint("暂停航线", wayLinePauseReplyCmd.getResult(), wayLinePauseReplyCmd.getErrorCode());
+ if (wayLinePauseReplyCmd.getResult() == 0) {
+ byte[] boxSnBytes = tyCommand.getBoxSn();
+ String boxSn = new String(boxSnBytes);
+ stateMachineService.sendEvent(boxSn, RouteEvent.PAUSE_SUCCESS);
+ }
}
//恢复航线回复
if (tyCommand instanceof WayLineResumeReplyCmd) {
WayLineResumeReplyCmd wayLineResumeReplyCmd = (WayLineResumeReplyCmd) tyCommand;
logPrint("恢复航线", wayLineResumeReplyCmd.getResult(), wayLineResumeReplyCmd.getErrorCode());
+ if (wayLineResumeReplyCmd.getResult() == 0) {
+ byte[] boxSnBytes = tyCommand.getBoxSn();
+ String boxSn = new String(boxSnBytes);
+ stateMachineService.sendEvent(boxSn, RouteEvent.RESUME_SUCCESS);
+ }
}
//结束航线回复
if (tyCommand instanceof WayLineEndReplyCmd) {
WayLineEndReplyCmd wayLineEndReplyCmd = (WayLineEndReplyCmd) tyCommand;
logPrint("结束航线", wayLineEndReplyCmd.getResult(), wayLineEndReplyCmd.getErrorCode());
+ if (wayLineEndReplyCmd.getResult() == 0) {
+ byte[] boxSnBytes = tyCommand.getBoxSn();
+ String boxSn = new String(boxSnBytes);
+ stateMachineService.sendEvent(boxSn, RouteEvent.STOP_SUCCESS);
+ }
+ }
+ if (tyCommand instanceof WayLineFinishedReplyCmd) {
+ WayLineFinishedReplyCmd wayLineFinishedReplyCmd = (WayLineFinishedReplyCmd) tyCommand;
+ logPrint("航线执行完成", wayLineFinishedReplyCmd.getResult(), wayLineFinishedReplyCmd.getErrorCode());
+ if (wayLineFinishedReplyCmd.getResult() == 0) {
+ byte[] boxSnBytes = tyCommand.getBoxSn();
+ String boxSn = new String(boxSnBytes);
+ stateMachineService.sendEvent(boxSn, RouteEvent.RESET_ROUTE);
+ }
}
//设置返航高度回复
if (tyCommand instanceof ReturnHeightSettingReplyCmd) {
@@ -1022,6 +1104,12 @@
logPrint("设置返航点", returnPointSettingReplyCmd.getResult(),
returnPointSettingReplyCmd.getErrorCode());
}
+ //到达环绕位置回复
+ if (tyCommand instanceof FlyRoundReplyCmd) {
+ FlyRoundReplyCmd flyRoundReplyCmd = (FlyRoundReplyCmd) tyCommand;
+ logPrint("到达环绕位置", flyRoundReplyCmd.getResult(),
+ flyRoundReplyCmd.getErrorCode());
+ }
break;
case (byte) 0xD2:
//设置视频码流回复
--
Gitblit v1.9.3