图斑飞行

guoshilong

2023-11-15 923a5e9fdfcfcc765606bce6fa7f99cb3717dcbe

 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
@@ -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);

@@ -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);
@@ -238,7 +298,8 @@
            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(),