From 05ba868736725db54db1944ed5cf7a95d14efc37 Mon Sep 17 00:00:00 2001
From: liusuyi <1951119284@qq.com>
Date: Wed, 30 Oct 2024 17:07:51 +0800
Subject: [PATCH] 增加：手动飞行控制
---
ard-work/src/main/java/com/ard/tianyu/service/impl/TyServiceImpl.java | 187 +++++++++++++++++++++++++++++++++++++++++++---
1 files changed, 174 insertions(+), 13 deletions(-)
diff --git a/ard-work/src/main/java/com/ard/tianyu/service/impl/TyServiceImpl.java b/ard-work/src/main/java/com/ard/tianyu/service/impl/TyServiceImpl.java
index 637d878..88d749c 100644
--- a/ard-work/src/main/java/com/ard/tianyu/service/impl/TyServiceImpl.java
+++ b/ard-work/src/main/java/com/ard/tianyu/service/impl/TyServiceImpl.java
@@ -15,6 +15,7 @@
import com.ard.tianyu.model.dto.converter.UavSignalInfoConverter;
import com.ard.tianyu.model.dto.converter.UavStateConverter;
import com.ard.tianyu.model.dto.converter.UavTelemetryDataConverter;
+import com.ard.tianyu.model.dto.manual.*;
import com.ard.tianyu.model.dto.signal.SignalInfo;
import com.ard.tianyu.model.dto.state.UavState;
import com.ard.tianyu.model.dto.telemetry.TelemetryData;
@@ -25,6 +26,7 @@
import com.ard.tianyu.netty.command.control.AbsAngleControlCmd;
import com.ard.tianyu.netty.command.control.AngleControlCmd;
import com.ard.tianyu.netty.command.control.SpeedControlCmd;
+import com.ard.tianyu.netty.command.manual.*;
import com.ard.tianyu.netty.command.notice.BoxPowerStatusReplyCmd;
import com.ard.tianyu.netty.command.notice.FlyWayEndReplyCmd;
import com.ard.tianyu.netty.command.notice.UavReadyReplyCmd;
@@ -32,7 +34,9 @@
import com.ard.tianyu.netty.command.real.UavStateCmd;
import com.ard.tianyu.netty.command.real.UavTelemetryDataCmd;
import com.ard.tianyu.netty.command.register.*;
+import com.ard.tianyu.netty.handler.ErrorCodeEnum;
import com.ard.tianyu.service.ITyService;
+import com.ard.tianyu.websocket.utils.WebSocketUtil;
import io.netty.channel.Channel;
import io.netty.channel.ChannelFuture;
import lombok.extern.slf4j.Slf4j;
@@ -268,6 +272,105 @@
}
/**
+ * 切换无人机控制权
+ */
+ @Override
+ public void switchUavControl(SwitchUavControlDTO switchUavControlDTO) {
+ SwitchUavControlCmd switchUavControlCmd = new SwitchUavControlCmd(switchUavControlDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), switchUavControlCmd);
+ }
+
+ /**
+ * 起飞
+ */
+ @Override
+ public void takeOff(TakeOffDTO takeOffDTO) {
+ TakeOffCmd takeOffCmd = new TakeOffCmd(takeOffDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), takeOffCmd);
+ }
+
+ /**
+ * 返航
+ */
+ @Override
+ public void returnHome(ReturnHomeDTO returnHomeDTO) {
+ ReturnHomeCmd returnHomeCmd = new ReturnHomeCmd(returnHomeDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), returnHomeCmd);
+ }
+
+ /**
+ * 取消返航
+ */
+ @Override
+ public void returnHomeCancel(ReturnHomeCancelDTO returnHomeCancelDTO) {
+ ReturnHomeCancelCmd returnHomeCancelCmd = new ReturnHomeCancelCmd(returnHomeCancelDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), returnHomeCancelCmd);
+ }
+
+ /**
+ * 降落
+ */
+ @Override
+ public void land(LandDTO landDTO) {
+ LandCmd landCmd = new LandCmd(landDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), landCmd);
+ }
+
+ /**
+ * 取消降落
+ */
+ @Override
+ public void landCancel(LandCancelDTO landCancelDTO) {
+ LandCancelCmd landCancelCmd = new LandCancelCmd(landCancelDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), landCancelCmd);
+ }
+
+ /**
+ * 强制降落
+ */
+ @Override
+ public void landForced(LandForcedDTO landForcedDTO) {
+ LandForcedCmd landForcedCmd = new LandForcedCmd(landForcedDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), landForcedCmd);
+ }
+
+ /**
+ * 方向控制
+ */
+ @Override
+ public void flyControl(FlyControlDTO flyControlDTO) {
+ FlyControlCmd flyControlCmd = new FlyControlCmd(flyControlDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), flyControlCmd);
+ }
+
+ /**
+ * 指点飞行控制
+ */
+ @Override
+ public void flyPointControl(FlyPointControlDTO flyPointControlDTO) {
+ FlyPointControlCmd flyPointControlCmd = new FlyPointControlCmd(flyPointControlDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), flyPointControlCmd);
+ }
+
+ /**
+ * 停止指点飞行控制
+ */
+ @Override
+ public void flyPointControlCancel(FlyPointControlCancelDTO flyPointControlCancelDTO) {
+ FlyPointControlCancelCmd flyPointControlCancelCmd = new FlyPointControlCancelCmd(flyPointControlCancelDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), flyPointControlCancelCmd);
+ }
+
+ /**
+ * 紧急制动
+ */
+ @Override
+ public void emeragencyBrake(EmeragencyBrakeDTO emeragencyBrakeDTO) {
+ EmergencyBrakeCmd emergencyBrakeCmd=new EmergencyBrakeCmd(emeragencyBrakeDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), emergencyBrakeCmd);
+ }
+
+ /**
* 获取无人机状态
*/
@Override
@@ -301,7 +404,7 @@
case 0x01:
// 处理心跳消息
RegisterReplyCmd registerReplyCommand = (RegisterReplyCmd) tyCommand;
- log.debug("收到注册消息-->处理注册消息:{}", registerReplyCommand.getSuccess());
+ logPrint("注册消息", registerReplyCommand.getSuccess());
if (registerReplyCommand.getSuccess() != 1) {
// 注册失败，重新注册
log.info("注册失败，重新注册");
@@ -311,7 +414,7 @@
case 0x02:
// 处理注册消息
HeartbeatReplyCmd heartbeatReplyCmd = (HeartbeatReplyCmd) tyCommand;
- log.debug("收到心跳消息-->处理心跳消息:{}", heartbeatReplyCmd.getTimestamp());
+ logPrint("心跳消息", heartbeatReplyCmd.getTimestamp());
break;
case 0x03:
// 处理订阅消息
@@ -322,28 +425,29 @@
case 0x0A:
// 处理云盒开关机通知消息
BoxPowerStatusReplyCmd boxPowerStatusReplyCmd = (BoxPowerStatusReplyCmd) tyCommand;
- log.debug("收到云盒开关机通知消息-->状态:{}", boxPowerStatusReplyCmd.getStatus());
+ logPrint("云盒开关机通知消息", boxPowerStatusReplyCmd.getStatus());
break;
case (byte) 0xA8:
// 处理无人机状态消息
- log.debug("收到无人机状态消息-->无人机状态消息");
UavStateCmd uavStateCmd = (UavStateCmd) tyCommand;
UavState uavState = UavStateConverter.convertProtoToEntity(uavStateCmd.getUavState());
UavStateCache.putUavState(uavState);
+ logPrint("无人机状态", uavState.getBoxSn());
+ WebSocketUtil.sendMessageAll(uavState);
break;
case (byte) 0xA9:
// 处理无人机遥测消息
- log.debug("收到无人机遥测消息-->无人机遥测消息");
UavTelemetryDataCmd uavTelemetryDataCmd = (UavTelemetryDataCmd) tyCommand;
TelemetryData telemetryData = UavTelemetryDataConverter.convertProtoToEntity(uavTelemetryDataCmd.getTelemetryData());
UavTelemetryDataCache.putUavTelemetryData(telemetryData);
+ logPrint("无人机遥测", telemetryData.getBoxSn());
break;
case (byte) 0x0C:
// 处理无人机信道质量数据
- log.debug("收到无人机信道质量数据消息-->无人机信道质量数据消息");
SignalInfoCmd signalInfoCmd = (SignalInfoCmd) tyCommand;
SignalInfo signalInfo = UavSignalInfoConverter.convertProtoToEntity(signalInfoCmd.getSignalInfo());
UavSignalInfoCache.putUavSignalInfoCache(signalInfo);
+ logPrint("无人机信道质量", signalInfo.getBoxSn());
break;
case (byte) 0xD1:
// 处理无人机
@@ -351,31 +455,71 @@
UavInitReplyCmd uavInitReplyCmd = (UavInitReplyCmd) tyCommand;
UavInitInfoCmd uavInitCommand = new UavInitInfoCmd(uavInitReplyCmd.getBoxSn());
this.sendMessage(tyConfiguration.getIpPort(), uavInitCommand);
- log.info("收到无人机初始化详情-回复防止再次发送");
+ logPrint("无人机初始化详情", uavInitCommand.getBoxSn());
}
if (tyCommand instanceof UavReadyReplyCmd) {
UavReadyReplyCmd uavReadyReplyCmd = (UavReadyReplyCmd) tyCommand;
- log.info("收到无人机准备完成通知,电池电量{} 经度{} 纬度{} 海拔{}",uavReadyReplyCmd.getBatteryPower(),uavReadyReplyCmd.getLongitude(),uavReadyReplyCmd.getLatitude(),uavReadyReplyCmd.getAltitude());
+ logPrint("无人机准备完成通知", uavReadyReplyCmd.getBatteryPower());
}
if (tyCommand instanceof CameraModeReplyCmd) {
CameraModeReplyCmd cameraModeReplyCommand = (CameraModeReplyCmd) tyCommand;
- log.info("收到无人机设置相机模式回复:{} 错误码：{}", String.format("0x%02X", cameraModeReplyCommand.getResult() & 0xFF), cameraModeReplyCommand.getErrorCode());
+ logPrint("无人机设置相机模式", cameraModeReplyCommand.getResult(), cameraModeReplyCommand.getErrorCode());
}
if (tyCommand instanceof FlyWayEndReplyCmd) {
FlyWayEndReplyCmd flyWayEndReplyCmd = (FlyWayEndReplyCmd) tyCommand;
- log.info("收到无人机飞行结束通知:状态：{}",flyWayEndReplyCmd.getResult());
+ logPrint("无人机飞行结束", flyWayEndReplyCmd.getResult(), flyWayEndReplyCmd.getErrorCode());
}
if (tyCommand instanceof CapturePicReplyCmd) {
CapturePicReplyCmd capturePicReplyCommand = (CapturePicReplyCmd) tyCommand;
- log.info("收到无人机拍照回复:{} 错误码：{}", String.format("0x%02X", capturePicReplyCommand.getResult() & 0xFF), capturePicReplyCommand.getErrorCode());
+ logPrint("无人机拍照", capturePicReplyCommand.getResult(), capturePicReplyCommand.getErrorCode());
}
if (tyCommand instanceof RecordStartReplyCmd) {
RecordStartReplyCmd recordStartReplyCommand = (RecordStartReplyCmd) tyCommand;
- log.info("收到无人机开始录像回复:{} 错误码：{}", String.format("0x%02X", recordStartReplyCommand.getResult() & 0xFF), recordStartReplyCommand.getErrorCode());
+ logPrint("无人机开始录像", recordStartReplyCommand.getResult(), recordStartReplyCommand.getErrorCode());
}
if (tyCommand instanceof RecordStopReplyCmd) {
RecordStopReplyCmd recordStopReplyCommand = (RecordStopReplyCmd) tyCommand;
- log.info("收到无人机停止录像回复:{} 错误码：{}", String.format("0x%02X", recordStopReplyCommand.getResult() & 0xFF), recordStopReplyCommand.getErrorCode());
+ logPrint("无人机停止录像", recordStopReplyCommand.getResult(), recordStopReplyCommand.getErrorCode());
+ }
+ //起飞回复
+ if (tyCommand instanceof TakeOffReplyCmd) {
+ TakeOffReplyCmd takeOffReplyCommand = (TakeOffReplyCmd) tyCommand;
+ logPrint("无人机起飞", takeOffReplyCommand.getResult(), takeOffReplyCommand.getErrorCode());
+ }
+ //返航回复
+ if (tyCommand instanceof ReturnHomeReplyCmd) {
+ ReturnHomeReplyCmd returnHomeReplyCmd = (ReturnHomeReplyCmd) tyCommand;
+ logPrint("无人机返航", returnHomeReplyCmd.getResult(), returnHomeReplyCmd.getErrorCode());
+ }
+ //取消返航回复
+ if (tyCommand instanceof ReturnHomeCancelReplyCmd) {
+ ReturnHomeCancelReplyCmd returnHomeCancelReplyCmd = (ReturnHomeCancelReplyCmd) tyCommand;
+ logPrint("取消无人机返航", returnHomeCancelReplyCmd.getResult(), returnHomeCancelReplyCmd.getErrorCode());
+ }
+ //降落回复
+ if (tyCommand instanceof LandReplyCmd) {
+ LandReplyCmd landReplyCmd = (LandReplyCmd) tyCommand;
+ logPrint("降落", landReplyCmd.getResult(), landReplyCmd.getErrorCode());
+ }
+ //取消降落回复
+ if (tyCommand instanceof LandCancelReplyCmd) {
+ LandCancelReplyCmd landCancelReplyCmd = (LandCancelReplyCmd) tyCommand;
+ logPrint("取消降落", landCancelReplyCmd.getResult(), landCancelReplyCmd.getErrorCode());
+ }
+ //强制降落回复
+ if (tyCommand instanceof LandForcedReplyCmd) {
+ LandForcedReplyCmd landForcedReplyCmd = (LandForcedReplyCmd) tyCommand;
+ logPrint("强制降落", landForcedReplyCmd.getResult(), landForcedReplyCmd.getErrorCode());
+ }
+ //到达指点飞行位置回复
+ if (tyCommand instanceof PointControlReachedReplyCmd) {
+ PointControlReachedReplyCmd pointControlReachedReplyCmd = (PointControlReachedReplyCmd) tyCommand;
+ logPrint("到达指点飞行位置", pointControlReachedReplyCmd.getResult(), pointControlReachedReplyCmd.getErrorCode());
+ }
+ //紧急制动回复
+ if (tyCommand instanceof EmergencyBrakeReplyCmd) {
+ EmergencyBrakeReplyCmd emergencyBrakeReplyCmd = (EmergencyBrakeReplyCmd) tyCommand;
+ logPrint("紧急制动回复", emergencyBrakeReplyCmd.getResult(), emergencyBrakeReplyCmd.getErrorCode());
}
break;
default:
@@ -384,6 +528,23 @@
}
}
+ //公共日志打印
+ private void logPrint(String message, Object obj) {
+ log.debug("收到{}回复{}", message, obj);
+ }
+
+ //公共日志打印
+ private void logPrint(String message, byte result, Integer errorCode) {
+ log.info("收到{}回复:{} 错误码：{}【{}】",
+ message,
+ String.format("0x%02X", result & 0xFF),
+ errorCode,
+ ErrorCodeEnum.getDescByCode(errorCode));
+ }
+
+ /**
+ * 定时3秒发送心跳指令
+ */
@Scheduled(fixedRate = 3000)
public void sendHeartBeat() {
long timestamp = System.currentTimeMillis();
--
Gitblit v1.9.3