From d1f854d741456292862c8b4a84f91c0f7f9a0be9 Mon Sep 17 00:00:00 2001
From: liusuyi <1951119284@qq.com>
Date: Fri, 01 Nov 2024 15:15:13 +0800
Subject: [PATCH] 增加：航线开始/暂停/恢复/结束
---
ard-work/src/main/java/com/ard/tianyu/service/impl/TyServiceImpl.java | 350 +++++++++++++++++++++++++++++++++++++++++++++++++++++-----
1 files changed, 320 insertions(+), 30 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 f995b0f..fd8ae57 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
@@ -1,7 +1,9 @@
package com.ard.tianyu.service.impl;
+import com.ard.common.utils.ByteUtils;
import com.ard.tianyu.config.TyConfiguration;
import com.ard.tianyu.http.TyClient;
+import com.ard.tianyu.model.cache.TyTcpServerCache;
import com.ard.tianyu.model.cache.UavSignalInfoCache;
import com.ard.tianyu.model.cache.UavStateCache;
import com.ard.tianyu.model.cache.UavTelemetryDataCache;
@@ -15,12 +17,13 @@
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.ReturnHomeDTO;
-import com.ard.tianyu.model.dto.manual.SwitchUavControlDTO;
-import com.ard.tianyu.model.dto.manual.TakeOffDTO;
+import com.ard.tianyu.model.dto.converter.wayline.PlanLineDataConverter;
+import com.ard.tianyu.model.dto.manual.*;
import com.ard.tianyu.model.dto.signal.SignalInfo;
+import com.ard.tianyu.model.dto.speaker.*;
import com.ard.tianyu.model.dto.state.UavState;
import com.ard.tianyu.model.dto.telemetry.TelemetryData;
+import com.ard.tianyu.model.dto.wayline.*;
import com.ard.tianyu.model.receiver.*;
import com.ard.tianyu.netty.command.*;
import com.ard.tianyu.netty.TyNettyTcpClient;
@@ -36,8 +39,12 @@
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.command.speaker.*;
+import com.ard.tianyu.netty.command.wayline.*;
import com.ard.tianyu.netty.handler.ErrorCodeEnum;
+import com.ard.tianyu.netty.protobuf.TelemetryDataPB;
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;
@@ -47,7 +54,10 @@
import javax.annotation.Resource;
import java.net.InetSocketAddress;
import java.util.ArrayList;
+import java.util.Collections;
import java.util.List;
+import java.util.Map;
+import java.util.concurrent.ConcurrentHashMap;
/**
* TyServiceImpl
@@ -64,6 +74,9 @@
@Resource
private TyConfiguration tyConfiguration;
+
+ // 存储每个无人机的最后遥测处理时间
+ private Map<String, Long> lastTelemetryTimes = new ConcurrentHashMap<>();
/**
* 获取天宇token信息
@@ -140,29 +153,12 @@
*/
@Override
public void registerMessage() {
- AccessTokenReceiver tyToken = tyConfiguration.getAccessToken();
- RegisterCmd registerMessage = new RegisterCmd(tyToken.getCompanyId(), tyToken.getAccessToken().getBytes());
- this.sendMessage(tyConfiguration.getIpPort(), registerMessage);
- }
-
- /**
- * 发送消息
- */
- public void sendMessage(String key, BaseCmd msg) {
try {
- ChannelFuture future = TyNettyTcpClient.channels.get(key).getChannelFuture();
- Channel channel = future.channel();
- if (channel.isActive()) {
- InetSocketAddress ipSocket = (InetSocketAddress) channel.remoteAddress();
- String host = ipSocket.getHostString();
- int port = ipSocket.getPort();
- channel.writeAndFlush(msg); // 发送编码后的数据
- log.debug("向天宇服务器【{}:{}】发送数据，命令码：{}", host, port, String.format("0x%02X", msg.getCmdCode() & 0xFF));
- } else {
- log.error("与天宇服务器链接断开，无法发送数据！");
- }
+ AccessTokenReceiver tyToken = tyConfiguration.getAccessToken();
+ RegisterCmd registerMessage = new RegisterCmd(tyToken.getCompanyId(), tyToken.getAccessToken().getBytes());
+ this.sendMessage(tyConfiguration.getIpPort(), registerMessage);
} catch (Exception e) {
- log.error("发送消息异常：{}", e.getMessage());
+ log.error("注册天宇平台失败:{}", e.getMessage());
}
}
@@ -300,6 +296,78 @@
}
/**
+ * 取消返航
+ */
+ @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
@@ -324,10 +392,153 @@
}
/**
+ * 喊话模式切换
+ */
+ @Override
+ public void speakModeSwitch(SpeakModeSwitchDTO speakModeSwitchDTO) {
+ SpeakModeSwitchCmd speakModeSwitchCmd = new SpeakModeSwitchCmd(speakModeSwitchDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), speakModeSwitchCmd);
+ }
+
+ /**
+ * 文字喊话
+ */
+ @Override
+ public void textSpeak(TextSpeakDTO textSpeakDTO) {
+ TextSpeakCmd textSpeakCmd = new TextSpeakCmd(textSpeakDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), textSpeakCmd);
+ }
+
+ /**
+ * 实时喊话
+ */
+ @Override
+ public void realSpeak(RealSpeakDTO realSpeakDTO) {
+ RealSpeakCmd realSpeakCmd = new RealSpeakCmd(realSpeakDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), realSpeakCmd);
+ }
+
+ /**
+ * 录音喊话
+ */
+ @Override
+ public void recordSpeak(RecordSpeakDTO recordSpeakDTO) {
+ RecordSpeakCmd recordSpeakCmd = new RecordSpeakCmd(recordSpeakDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), recordSpeakCmd);
+ }
+
+ /**
+ * 设置音量
+ */
+ @Override
+ public void volumeSetting(VolumeSettingDTO volumeSettingDTO) {
+ VolumeSettingCmd volumeSettingCmd = new VolumeSettingCmd(volumeSettingDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), volumeSettingCmd);
+ }
+
+ /**
+ * 设置循环播放
+ */
+ @Override
+ public void loopSetting(LoopSettingDTO loopSettingDTO) {
+ LoopSettingCmd loopSettingCmd = new LoopSettingCmd(loopSettingDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), loopSettingCmd);
+ }
+
+ /**
+ * 停止喊话
+ */
+ @Override
+ public void stopSpeak(StopSpeakDTO stopSpeakDTO) {
+ StopSpeakCmd stopSpeakCmd = new StopSpeakCmd(stopSpeakDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), stopSpeakCmd);
+ }
+
+ /**
+ * 航线上传
+ */
+ @Override
+ public void wayLineUpload(WayLineUploadDTO wayLineUploadDTO) {
+ wayLineUploadDTO.setWayLineData(this.CreatePlanLineData());//设置航线数据
+ WayLineUploadCmd wayLineUploadCmd = new WayLineUploadCmd(wayLineUploadDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), wayLineUploadCmd);
+ }
+
+ //创建航线
+ private PlanLineData CreatePlanLineData() {
+ TelemetryDataPB.PointData pointData1 = TelemetryDataPB.PointData.newBuilder()
+ .setLng(124.12345).setLat(45.12345).setHeight(80).setSpeed(10)
+ .setFlightPathMode(1)//航线飞行模式：1=直线飞行，2=协调转弯，3=曲线飞行(停)，4=曲线飞行(不停)
+ .setSafeGohomeFlag(0)//是否为安全返航点 0=不是，1=是
+ .build();
+ TelemetryDataPB.PointData pointData2 = TelemetryDataPB.PointData.newBuilder()
+ .setLng(124.22222).setLat(45.22222).setHeight(80).setSpeed(10)
+ .setFlightPathMode(1)//航线飞行模式：1=直线飞行，2=协调转弯，3=曲线飞行(停)，4=曲线飞行(不停)
+ .setSafeGohomeFlag(0)//是否为安全返航点 0=不是，1=是
+ .build();
+ TelemetryDataPB.PointData pointData3 = TelemetryDataPB.PointData.newBuilder()
+ .setLng(124.33333).setLat(45.33333).setHeight(80).setSpeed(10)
+ .setFlightPathMode(1)//航线飞行模式：1=直线飞行，2=协调转弯，3=曲线飞行(停)，4=曲线飞行(不停)
+ .setSafeGohomeFlag(0)//是否为安全返航点 0=不是，1=是
+ .build();
+
+ //创建航线
+ TelemetryDataPB.PlanLineData PlanLineDataPB = TelemetryDataPB.PlanLineData.newBuilder()
+ .setFinishedAction(2)//结束航线后动作：1=返航，2=原地悬停，3=原地降落，4=原地绕飞
+ .setMaxSpeed(10)//最大飞行速度
+ .setAutoSpeed(10)//自动飞行速度
+ .addPoints(pointData1)//航线点1
+ .addPoints(pointData2)//航线点2
+ .addPoints(pointData3)//航线点3
+ .setLoseAction(1)//网络失联后动作：0=返回HOME点，1=继续航线，默认0
+ .setHomeHeight(6)//返航高度：(单位:米，范围20~1500)
+ .setIsSaveEnergyMode(0)////是否开启节能模式：0=不是，1=是(说明：节能模式下，飞机沿最短直线距离由起飞点到航线第一个航点，非升高到第一个航点高度再飞往第一个航点)
+ .build();
+
+ return PlanLineDataConverter.convertProtoToEntity(PlanLineDataPB);
+ }
+
+ /**
+ * 航线开始
+ */
+ @Override
+ public void wayLineBegin(WayLineBeginDTO wayLineBeginDTO) {
+ WayLineBeginCmd wayLineBeginCmd = new WayLineBeginCmd(wayLineBeginDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), wayLineBeginCmd);
+ }
+
+ /**
+ * 航线暂停
+ */
+ @Override
+ public void wayLinePause(WayLinePauseDTO wayLinePauseDTO) {
+ WayLinePauseCmd wayLinePauseCmd = new WayLinePauseCmd(wayLinePauseDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), wayLinePauseCmd);
+ }
+
+ /**
+ * 航线恢复
+ */
+ @Override
+ public void wayLineResume(WayLineResumeDTO wayLineResumeDTO) {
+ WayLineResumeCmd wayLineResumeCmd = new WayLineResumeCmd(wayLineResumeDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), wayLineResumeCmd);
+ }
+
+ /**
+ * 航线结束
+ */
+ @Override
+ public void wayLineEnd(WayLineEndDTO wayLineEndDTO) {
+ WayLineEndCmd wayLineEndCmd = new WayLineEndCmd(wayLineEndDTO);
+ this.sendMessage(tyConfiguration.getIpPort(), wayLineEndCmd);
+ }
+
+ /**
* 处理天宇服务器返回的消息
*/
@Override
- public void processMessage(String key, BaseCmd tyCommand) {
+ public void processReplyMessage(String key, BaseCmd tyCommand) {
byte cmdCode = tyCommand.getCmdCode();
switch (cmdCode) {
case 0x01:
@@ -362,14 +573,33 @@
UavState uavState = UavStateConverter.convertProtoToEntity(uavStateCmd.getUavState());
UavStateCache.putUavState(uavState);
logPrint("无人机状态", uavState.getBoxSn());
+ WebSocketUtil.sendMessageAll(uavState);
break;
case (byte) 0xA9:
// 处理无人机遥测消息
UavTelemetryDataCmd uavTelemetryDataCmd = (UavTelemetryDataCmd) tyCommand;
- TelemetryData telemetryData = UavTelemetryDataConverter.convertProtoToEntity(uavTelemetryDataCmd.getTelemetryData());
- UavTelemetryDataCache.putUavTelemetryData(telemetryData);
- logPrint("无人机遥测", telemetryData.getBoxSn());
+ if (uavTelemetryDataCmd.getBoxSn() == null) {
+ return;
+ }
+ String boxSn = ByteUtils.bytesToString(uavTelemetryDataCmd.getBoxSn()); // 假设有获取无人机ID的方法
+ long currentTime = System.currentTimeMillis();
+
+ // 获取无人机的最后处理时间，如果没有则设置为0
+ long lastTelemetryTime = lastTelemetryTimes.getOrDefault(boxSn, 0L);
+
+ // 检查与上次处理时间的间隔
+ if (currentTime - lastTelemetryTime >= 1000) { // 1秒
+ TelemetryData telemetryData = UavTelemetryDataConverter.convertProtoToEntity(uavTelemetryDataCmd.getTelemetryData());
+ UavTelemetryDataCache.putUavTelemetryData(telemetryData);
+ logPrint("无人机遥测", telemetryData.getBoxSn());
+
+ // 更新最后处理时间
+ lastTelemetryTimes.put(boxSn, currentTime);
+ } else {
+ log.debug("无人机 " + boxSn + " 遥测数据接收过于频繁，已跳过处理");
+ }
break;
+
case (byte) 0x0C:
// 处理无人机信道质量数据
SignalInfoCmd signalInfoCmd = (SignalInfoCmd) tyCommand;
@@ -419,10 +649,45 @@
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());
+ }
+ //上传航线回复
+ if (tyCommand instanceof WayLineUploadReplyCmd) {
+ WayLineUploadReplyCmd wayLineUploadReplyCmd = (WayLineUploadReplyCmd) tyCommand;
+ logPrint("上传航线", wayLineUploadReplyCmd.getResult(), wayLineUploadReplyCmd.getErrorCode());
+ }
break;
default:
// 处理其他消息
- log.debug("收到其他消息-->其他消息");
+ log.warn("收到其他消息-->{}", tyCommand.getCmdCode());
}
}
@@ -430,11 +695,12 @@
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),
+ ByteUtils.byteToHex(result),
errorCode,
ErrorCodeEnum.getDescByCode(errorCode));
}
@@ -444,9 +710,33 @@
*/
@Scheduled(fixedRate = 3000)
public void sendHeartBeat() {
+ if (!TyTcpServerCache.getTcpServerCache(tyConfiguration.getTcpserver() + ":" + tyConfiguration.getTcpport())) {
+ return;
+ }
long timestamp = System.currentTimeMillis();
log.debug("定时向天宇发送⼼跳,时间戳{}", timestamp);
HeartbeatCmd heartbeatMessage = new HeartbeatCmd(timestamp);
this.sendMessage(tyConfiguration.getIpPort(), heartbeatMessage);
}
+
+ /**
+ * 发送消息
+ */
+ private void sendMessage(String key, BaseCmd msg) {
+ try {
+ ChannelFuture future = TyNettyTcpClient.channels.get(key).getChannelFuture();
+ Channel channel = future.channel();
+ if (channel.isActive()) {
+ InetSocketAddress ipSocket = (InetSocketAddress) channel.remoteAddress();
+ String host = ipSocket.getHostString();
+ int port = ipSocket.getPort();
+ channel.writeAndFlush(msg); // 发送编码后的数据
+ log.debug("向天宇服务器【{}:{}】发送数据，命令码：{}", host, port, String.format("0x%02X", msg.getCmdCode() & 0xFF));
+ } else {
+ log.error("与天宇服务器链接断开，无法发送数据！");
+ }
+ } catch (Exception e) {
+ log.error("发送消息异常：{}", e.getMessage());
+ }
+ }
}
\ No newline at end of file
--
Gitblit v1.9.3