From 923a5e9fdfcfcc765606bce6fa7f99cb3717dcbe Mon Sep 17 00:00:00 2001
From: guoshilong <123456>
Date: Wed, 15 Nov 2023 14:05:06 +0800
Subject: [PATCH] 图斑飞行
---
src/main/java/com/dji/sample/control/service/impl/ControlServiceImpl.java | 81 +++++++++++++++++++++++++++++++++++-----
1 files changed, 71 insertions(+), 10 deletions(-)
diff --git a/src/main/java/com/dji/sample/control/service/impl/ControlServiceImpl.java b/src/main/java/com/dji/sample/control/service/impl/ControlServiceImpl.java
index 2fb775f..3cee5df 100644
--- a/src/main/java/com/dji/sample/control/service/impl/ControlServiceImpl.java
+++ b/src/main/java/com/dji/sample/control/service/impl/ControlServiceImpl.java
@@ -1,5 +1,6 @@
package com.dji.sample.control.service.impl;
+import com.alibaba.fastjson.JSONObject;
import com.dji.sample.common.error.CommonErrorEnum;
import com.dji.sample.common.model.ResponseResult;
import com.dji.sample.component.mqtt.model.*;
@@ -9,10 +10,12 @@
import com.dji.sample.component.websocket.model.BizCodeEnum;
import com.dji.sample.component.websocket.service.ISendMessageService;
import com.dji.sample.control.model.dto.FlyToProgressReceiver;
+import com.dji.sample.control.model.dto.PointDTO;
import com.dji.sample.control.model.dto.ResultNotifyDTO;
import com.dji.sample.control.model.dto.TakeoffProgressReceiver;
import com.dji.sample.control.model.enums.DroneAuthorityEnum;
import com.dji.sample.control.model.enums.DroneControlMethodEnum;
+import com.dji.sample.control.model.enums.FlyToStatusEnum;
import com.dji.sample.control.model.enums.RemoteDebugMethodEnum;
import com.dji.sample.control.model.param.*;
import com.dji.sample.control.service.IControlService;
@@ -24,6 +27,7 @@
import com.dji.sample.manage.service.IDeviceRedisService;
import com.dji.sample.manage.service.IDeviceService;
import com.dji.sample.wayline.model.enums.WaylineErrorCodeEnum;
+import com.dji.sample.wayline.model.param.PointPOJO;
import com.fasterxml.jackson.core.type.TypeReference;
import com.fasterxml.jackson.databind.ObjectMapper;
import lombok.extern.slf4j.Slf4j;
@@ -32,9 +36,7 @@
import org.springframework.messaging.MessageHeaders;
import org.springframework.stereotype.Service;
-import java.util.Objects;
-import java.util.Optional;
-import java.util.UUID;
+import java.util.*;
/**
* @author sean
@@ -97,7 +99,7 @@
"error: " + serviceIdentifier + serviceReply.getResult());
}
if (controlMethodEnum.getProgress()) {
- RedisOpsUtils.setWithExpire(serviceIdentifier + RedisConst.DELIMITER + bid, sn,
+ RedisOpsUtils.setWithExpire(serviceIdentifier + RedisConst.DELIMITER + bid, sn,
RedisConst.DEVICE_ALIVE_SECOND * RedisConst.DEVICE_ALIVE_SECOND);
}
return ResponseResult.success();
@@ -105,6 +107,7 @@
/**
* Handles multi-state command progress information.
+ *
* @param receiver
* @param headers
* @return
@@ -118,7 +121,8 @@
String sn = RedisOpsUtils.get(key).toString();
EventsReceiver<EventsOutputProgressReceiver> eventsReceiver = mapper.convertValue(receiver.getData(),
- new TypeReference<EventsReceiver<EventsOutputProgressReceiver>>(){});
+ new TypeReference<EventsReceiver<EventsOutputProgressReceiver>>() {
+ });
eventsReceiver.setBid(receiver.getBid());
eventsReceiver.setSn(sn);
@@ -186,7 +190,7 @@
@ServiceActivator(inputChannel = ChannelName.INBOUND_EVENTS_FLY_TO_POINT_PROGRESS, outputChannel = ChannelName.OUTBOUND_EVENTS)
public CommonTopicReceiver handleFlyToPointProgress(CommonTopicReceiver receiver, MessageHeaders headers) {
- String dockSn = receiver.getGateway();
+ String dockSn = receiver.getGateway();
Optional<DeviceDTO> deviceOpt = deviceRedisService.getDeviceOnline(dockSn);
if (deviceOpt.isEmpty()) {
@@ -194,7 +198,8 @@
return null;
}
- FlyToProgressReceiver eventsReceiver = mapper.convertValue(receiver.getData(), new TypeReference<FlyToProgressReceiver>(){});
+ FlyToProgressReceiver eventsReceiver = mapper.convertValue(receiver.getData(), new TypeReference<FlyToProgressReceiver>() {
+ });
webSocketMessageService.sendBatch(deviceOpt.get().getWorkspaceId(), UserTypeEnum.WEB.getVal(),
BizCodeEnum.FLY_TO_POINT_PROGRESS.getCode(),
ResultNotifyDTO.builder().sn(dockSn)
@@ -202,8 +207,63 @@
eventsReceiver.getStatus().getMessage() : eventsReceiver.getResult().getErrorMsg())
.result(eventsReceiver.getResult().getErrorCode())
.build());
+
+ if (eventsReceiver.getStatus().equals(FlyToStatusEnum.WAYLINE_OK)) {
+ JSONObject jsonObject = (JSONObject) RedisOpsUtils.get("tuban:" + dockSn);
+ if (jsonObject != null) {
+ List<PointPOJO> targetList = (List<PointPOJO>) jsonObject.get("targetList");
+ int curIndex = (Integer) jsonObject.get("curIndex");
+ flyToNextPoint(targetList, curIndex, dockSn);
+ }
+ }
return receiver;
}
+
+ private ResponseResult flyToNextPoint(List<PointPOJO> targetList, int curIndex, String sn) {
+ curIndex = curIndex + 1;
+ //当无人机状态为人工时再发布下一个命令
+ while (true) {
+ Optional<DeviceDTO> dockOpt = deviceRedisService.getDeviceOnline(sn);
+ DeviceModeCodeEnum deviceMode = deviceService.getDeviceMode(dockOpt.get().getChildDeviceSn());
+
+ if (DeviceModeCodeEnum.MANUAL == deviceMode) {
+ if (curIndex == targetList.size()) {
+ //当前是最后一个点，返航
+ ResponseResult returnHome = controlDockDebug(sn, "return_home", null);
+
+ RedisOpsUtils.del("tuban:" + sn);
+
+ return returnHome;
+ }
+ else {
+ //当前不是最后一个点，飞行到下一个点
+ FlyToPointParam flyToPointParam = new FlyToPointParam();
+ flyToPointParam.setMaxSpeed(14);
+ List<PointDTO> pointDTOS = new ArrayList<>();
+
+ PointDTO pointDTO = new PointDTO();
+ pointDTO.setHeight(150.0);
+ pointDTO.setLongitude(targetList.get(curIndex).getLon());
+ pointDTO.setLatitude(targetList.get(curIndex).getLat());
+ pointDTOS.add(pointDTO);
+ flyToPointParam.setPoints(pointDTOS);
+
+
+ ResponseResult flyToRes = flyToPoint(sn, flyToPointParam);
+ if (flyToRes.getCode() == ResponseResult.CODE_SUCCESS) {
+ JSONObject jsonObject = new JSONObject();
+ jsonObject.put("targetList", targetList);
+ jsonObject.put("curIndex", curIndex);
+ RedisOpsUtils.set("tuban:" + sn, jsonObject);
+ }
+ return flyToRes;
+
+
+ }
+ }
+ }
+ }
+
private void checkTakeoffCondition(String dockSn) {
Optional<DeviceDTO> dockOpt = deviceRedisService.getDeviceOnline(dockSn);
@@ -231,14 +291,15 @@
@ServiceActivator(inputChannel = ChannelName.INBOUND_EVENTS_TAKE_OFF_TO_POINT_PROGRESS, outputChannel = ChannelName.OUTBOUND_EVENTS)
public CommonTopicReceiver handleTakeoffToPointProgress(CommonTopicReceiver receiver, MessageHeaders headers) {
- String dockSn = receiver.getGateway();
+ String dockSn = receiver.getGateway();
Optional<DeviceDTO> deviceOpt = deviceRedisService.getDeviceOnline(dockSn);
if (deviceOpt.isEmpty()) {
log.error("机场离线");
return null;
}
- TakeoffProgressReceiver eventsReceiver = mapper.convertValue(receiver.getData(), new TypeReference<TakeoffProgressReceiver>(){});
+ TakeoffProgressReceiver eventsReceiver = mapper.convertValue(receiver.getData(), new TypeReference<TakeoffProgressReceiver>() {
+ });
webSocketMessageService.sendBatch(deviceOpt.get().getWorkspaceId(), UserTypeEnum.WEB.getVal(),
BizCodeEnum.TAKE_OFF_TO_POINT_PROGRESS.getCode(),
@@ -299,7 +360,7 @@
}
@Override
- public ResponseResult requestsConfig(String sn,String method, RequestsParam param) {
+ public ResponseResult requestsConfig(String sn, String method, RequestsParam param) {
ServiceReply serviceReply = messageSenderService.publishRequestsTopic(sn, method, param);
return ResponseResult.CODE_SUCCESS != serviceReply.getResult() ?
ResponseResult.error(serviceReply.getResult(), "错误码：" + serviceReply.getResult())
--
Gitblit v1.9.3