307 lines
11 KiB
Java
307 lines
11 KiB
Java
|
//package com.aros.apron.fly;
|
||
|
|
//
|
||
|
|
//import androidx.annotation.NonNull;
|
||
|
|
//
|
||
|
|
//import com.aros.apron.entity.Movement;
|
||
|
|
//import com.aros.apron.tools.LogUtil;
|
||
|
|
//import com.google.gson.Gson;
|
||
|
|
//
|
||
|
|
//import java.util.Timer;
|
||
|
|
//import java.util.TimerTask;
|
||
|
|
//
|
||
|
|
//import dji.sdk.keyvalue.key.FlightControllerKey;
|
||
|
|
//import dji.sdk.keyvalue.key.KeyTools;
|
||
|
|
//import dji.sdk.keyvalue.value.common.EmptyMsg;
|
||
|
|
//import dji.sdk.keyvalue.value.flightcontroller.FlightCoordinateSystem;
|
||
|
|
//import dji.sdk.keyvalue.value.flightcontroller.RollPitchControlMode;
|
||
|
|
//import dji.sdk.keyvalue.value.flightcontroller.VerticalControlMode;
|
||
|
|
//import dji.sdk.keyvalue.value.flightcontroller.VirtualStickFlightControlParam;
|
||
|
|
//import dji.sdk.keyvalue.value.flightcontroller.YawControlMode;
|
||
|
|
//import dji.v5.common.callback.CommonCallbacks;
|
||
|
|
//import dji.v5.common.error.IDJIError;
|
||
|
|
//import dji.v5.manager.KeyManager;
|
||
|
|
//import dji.v5.manager.aircraft.virtualstick.VirtualStickManager;
|
||
|
|
//
|
||
|
|
//
|
||
|
|
//public class FlyToAlternatePointManager {
|
||
|
|
// private String TAG = "FlyToAlternatePointManager";
|
||
|
|
//
|
||
|
|
// private AlternatePoint mWaypoint = new AlternatePoint();
|
||
|
|
// private float mSpeed = 3.0f;
|
||
|
|
// private double aircraftLng, aircraftLat, aircraftAlt, aircraftPich, aircraftRoll, aircraftYaw;
|
||
|
|
// //判定是否到点的误差
|
||
|
|
// private double delta = 1;
|
||
|
|
// private double heightDelta = 5;
|
||
|
|
// //任务是否开始执行
|
||
|
|
// private boolean isRunning = false;
|
||
|
|
// //航点任务是否开始执行，到达第一个点后才开始
|
||
|
|
// private boolean isMissionRunning = false;
|
||
|
|
// private int currentIndex = 0;
|
||
|
|
// private Timer mSendVirtualStickDataTimer;
|
||
|
|
// private SendVirtualStickDataTask mSendVirtualStickDataTask;
|
||
|
|
// private float mPitch;
|
||
|
|
// private float mRoll;
|
||
|
|
// private float mYaw;
|
||
|
|
// private float mThrottle;
|
||
|
|
// VirtualStickFlightControlParam param;
|
||
|
|
//
|
||
|
|
// private FlyToAlternatePointManager() {
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// private static class FlyToAlternatePointHolder {
|
||
|
|
// private static final FlyToAlternatePointManager INSTANCE = new FlyToAlternatePointManager();
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// public static FlyToAlternatePointManager getInstance() {
|
||
|
|
// return FlyToAlternatePointManager.FlyToAlternatePointHolder.INSTANCE;
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// public void setAlternateCinfig(AlternatePoint waypoint, float speed) {
|
||
|
|
// mWaypoint = waypoint;
|
||
|
|
// mSpeed = speed;
|
||
|
|
// Boolean isConnect = KeyManager.getInstance().getValue(KeyTools.createKey(FlightControllerKey.KeyConnection));
|
||
|
|
// if (isConnect != null && isConnect) {
|
||
|
|
// if (param == null) {
|
||
|
|
// param = new VirtualStickFlightControlParam();
|
||
|
|
// param.setRollPitchControlMode(RollPitchControlMode.VELOCITY);
|
||
|
|
// param.setYawControlMode(YawControlMode.ANGULAR_VELOCITY);
|
||
|
|
// param.setVerticalControlMode(VerticalControlMode.POSITION);
|
||
|
|
// param.setRollPitchCoordinateSystem(FlightCoordinateSystem.BODY);
|
||
|
|
// }
|
||
|
|
// VirtualStickManager.getInstance().sendVirtualStickAdvancedParam(param);
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// /**
|
||
|
|
// * 这里开始任务
|
||
|
|
// */
|
||
|
|
// public void start() {
|
||
|
|
// currentIndex = 0;
|
||
|
|
// isRunning = true;
|
||
|
|
// Boolean isConnect = KeyManager.getInstance().getValue(KeyTools.createKey(FlightControllerKey.KeyConnection));
|
||
|
|
// if (isConnect != null && isConnect) {
|
||
|
|
// VirtualStickManager.getInstance().enableVirtualStick(new CommonCallbacks.CompletionCallback() {
|
||
|
|
// @Override
|
||
|
|
// public void onSuccess() {
|
||
|
|
// VirtualStickManager.getInstance().setVirtualStickAdvancedModeEnabled(true);
|
||
|
|
// flyToPoint(mWaypoint);
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// @Override
|
||
|
|
// public void onFailure(@NonNull IDJIError error) {
|
||
|
|
// LogUtil.log(TAG, "备降点控制权获取失败:" + error.description());
|
||
|
|
// }
|
||
|
|
// });
|
||
|
|
// } else {
|
||
|
|
// LogUtil.log(TAG, "备降点控制权获取失败:飞控未连接");
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// /**
|
||
|
|
// * 飞到指定点
|
||
|
|
// * @param point
|
||
|
|
// */
|
||
|
|
// private void flyToPoint(AlternatePoint point) {
|
||
|
|
// //还没启动就不管
|
||
|
|
// if (!isRunning) {
|
||
|
|
// return;
|
||
|
|
// }
|
||
|
|
// //这两个固定，全程使用前进，全程保持高度
|
||
|
|
// mPitch = 0;
|
||
|
|
// mThrottle = point.getAltitude();
|
||
|
|
// //还没获取到飞机当前状态时不管
|
||
|
|
// if (Double.isNaN(aircraftLng) || Double.isNaN((aircraftLat))) {
|
||
|
|
// return;
|
||
|
|
// }
|
||
|
|
// //先飞到指定高度再继续，比较安全
|
||
|
|
// if (Math.abs(aircraftAlt - point.getAltitude()) > heightDelta) {
|
||
|
|
// executeVirtualStickDataTask();
|
||
|
|
// return;
|
||
|
|
// }
|
||
|
|
// double[] targetXY = GaussKruegerConverter.LngLat2XY(point.getLongitude(), point.getLatitude());
|
||
|
|
// double[] nowXY = GaussKruegerConverter.LngLat2XY(aircraftLng, aircraftLat);
|
||
|
|
// double dx = targetXY[0] - nowXY[0];
|
||
|
|
// double dy = targetXY[1] - nowXY[1];
|
||
|
|
// double d = Math.sqrt(dx * dx + dy * dy);
|
||
|
|
// //到点
|
||
|
|
// if (d < delta) {
|
||
|
|
// //这里可以判断是否飞到备降点上方
|
||
|
|
// LogUtil.log(TAG, "到达备降点上方");
|
||
|
|
// return;
|
||
|
|
// }
|
||
|
|
// //没到点继续飞
|
||
|
|
// mYaw = (float) calcDeg(nowXY, targetXY);
|
||
|
|
// mRoll = (float) calcSpeed(nowXY, targetXY);
|
||
|
|
// executeVirtualStickDataTask();
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// /***
|
||
|
|
// * 结束任务
|
||
|
|
// */
|
||
|
|
// public void stop() {
|
||
|
|
// isMissionRunning = false;
|
||
|
|
// isRunning = false;
|
||
|
|
// land();
|
||
|
|
// disableVirtualStickMode();
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// /**
|
||
|
|
// * 禁止虚拟摇杆
|
||
|
|
// */
|
||
|
|
// private void disableVirtualStickMode() {
|
||
|
|
// if (mSendVirtualStickDataTimer != null) {
|
||
|
|
// mSendVirtualStickDataTimer.cancel();
|
||
|
|
// }
|
||
|
|
// VirtualStickManager.getInstance().setVirtualStickAdvancedModeEnabled(true);
|
||
|
|
// VirtualStickManager.getInstance().disableVirtualStick(new CommonCallbacks.CompletionCallback() {
|
||
|
|
// @Override
|
||
|
|
// public void onSuccess() {
|
||
|
|
// LogUtil.log(TAG, "备降点取消控制权成功");
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// @Override
|
||
|
|
// public void onFailure(@NonNull IDJIError error) {
|
||
|
|
// LogUtil.log(TAG, "备降点取消控制权失败:" + new Gson().toJson(error));
|
||
|
|
// }
|
||
|
|
// });
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// /**
|
||
|
|
// * 降落
|
||
|
|
// */
|
||
|
|
// private void land() {
|
||
|
|
// Boolean isConnect = KeyManager.getInstance().getValue(KeyTools.createKey(FlightControllerKey.KeyConnection));
|
||
|
|
// if (isConnect != null && isConnect) {
|
||
|
|
// KeyManager.getInstance().performAction(KeyTools.createKey(FlightControllerKey.KeyStartAutoLanding), new CommonCallbacks.CompletionCallbackWithParam<EmptyMsg>() {
|
||
|
|
// @Override
|
||
|
|
// public void onSuccess(EmptyMsg emptyMsg) {
|
||
|
|
// LogUtil.log(TAG, "备降点降落调用成功");
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// @Override
|
||
|
|
// public void onFailure(@NonNull IDJIError error) {
|
||
|
|
// LogUtil.log(TAG, "备降点降落调用失败:" + new Gson().toJson(error));
|
||
|
|
// }
|
||
|
|
// });
|
||
|
|
// } else {
|
||
|
|
// LogUtil.log(TAG, "备降点降落调用失败:飞控未连接");
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
//
|
||
|
|
// /**
|
||
|
|
// * 飞行到下一个点
|
||
|
|
// * 如果
|
||
|
|
// */
|
||
|
|
//// public void goNextPoint() {
|
||
|
|
//// //任务执行时，或者起飞到第一个点时
|
||
|
|
//// if (isMissionRunning || currentIndex == 0) {
|
||
|
|
//// //如果是第一个点，就任务开始
|
||
|
|
//// if (currentIndex == 0) {
|
||
|
|
//// isMissionRunning = true;
|
||
|
|
//// }
|
||
|
|
//// //如果是正在执行任务并且最后一个点，就任务结束，进入停止流程
|
||
|
|
//// if (isMissionRunning && currentIndex == mWaypointList.size() - 1) {
|
||
|
|
//// isMissionRunning = false;
|
||
|
|
//// currentIndex = -1;
|
||
|
|
//// stop();
|
||
|
|
//// }
|
||
|
|
//// }
|
||
|
|
//// }
|
||
|
|
//
|
||
|
|
// /**
|
||
|
|
// * 计算飞行速度
|
||
|
|
// *
|
||
|
|
// * @param nowXY
|
||
|
|
// * @param targetXY
|
||
|
|
// * @return
|
||
|
|
// */
|
||
|
|
// private double calcSpeed(double[] nowXY, double[] targetXY) {
|
||
|
|
// double speed = mSpeed;
|
||
|
|
// double dx = targetXY[0] - nowXY[0];
|
||
|
|
// double dy = targetXY[1] - nowXY[1];
|
||
|
|
// double d = Math.sqrt(dx * dx + dy * dy);
|
||
|
|
//
|
||
|
|
// //根据距离目标的位置计算速度，快到点了速度降下来
|
||
|
|
// if (d < mSpeed * 2) {
|
||
|
|
// speed = (float) d / 2;
|
||
|
|
// }
|
||
|
|
// return speed;
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// /**
|
||
|
|
// * 计算飞行角度
|
||
|
|
// *
|
||
|
|
// * @param nowXY 当前飞机的位置
|
||
|
|
// * @param targetXY 目标位置
|
||
|
|
// * @return
|
||
|
|
// */
|
||
|
|
// private double calcDeg(double[] nowXY, double[] targetXY) {
|
||
|
|
// double deg = 0;
|
||
|
|
// double dx = targetXY[0] - nowXY[0];
|
||
|
|
// double dy = targetXY[1] - nowXY[1];
|
||
|
|
// //根据目标位置和飞机位置计算角度
|
||
|
|
// if (dy > 0) {
|
||
|
|
// deg = Math.toDegrees(Math.atan(dx / dy));
|
||
|
|
// } else if (dy == 0) {
|
||
|
|
// if (dx > 0) {
|
||
|
|
// deg = 90;
|
||
|
|
// } else if (dx == 0) {
|
||
|
|
// deg = 0;
|
||
|
|
// } else {
|
||
|
|
// deg = -90;
|
||
|
|
// }
|
||
|
|
// } else {
|
||
|
|
// if (dx > 0) {
|
||
|
|
// deg = 180 + Math.toDegrees(Math.atan((targetXY[0] - nowXY[0]) / (targetXY[1] - nowXY[1])));
|
||
|
|
// } else if (dx == 0) {
|
||
|
|
// deg = -180;
|
||
|
|
// } else {
|
||
|
|
// deg = Math.toDegrees(Math.atan((targetXY[0] - nowXY[0]) / (targetXY[1] - nowXY[1]))) - 180;
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
// return deg;
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
//
|
||
|
|
// /**
|
||
|
|
// * 执行虚拟摇杆发送任务
|
||
|
|
// */
|
||
|
|
// private void executeVirtualStickDataTask() {
|
||
|
|
// if (null == mSendVirtualStickDataTimer) {
|
||
|
|
// mSendVirtualStickDataTask = new SendVirtualStickDataTask();
|
||
|
|
// mSendVirtualStickDataTimer = new Timer();
|
||
|
|
// mSendVirtualStickDataTimer.schedule(mSendVirtualStickDataTask, 100, 200);
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// /**
|
||
|
|
// * 更新无人机状态
|
||
|
|
// */
|
||
|
|
// public void updateAircraftSate() {
|
||
|
|
// aircraftLng = Double.parseDouble(Movement.getInstance().getCurrentLongitude());
|
||
|
|
// aircraftLat = Double.parseDouble(Movement.getInstance().getCurrentLatitude());
|
||
|
|
// aircraftAlt = Movement.getInstance().getFlyingHeight();
|
||
|
|
// aircraftPich = Double.parseDouble(Movement.getInstance().getPitch());
|
||
|
|
// aircraftRoll = Double.parseDouble(Movement.getInstance().getRoll());
|
||
|
|
// aircraftYaw = Double.parseDouble(Movement.getInstance().getYaw());
|
||
|
|
// if (isRunning) {
|
||
|
|
// //在任务中就飞任务点
|
||
|
|
// if (isMissionRunning || currentIndex == 0) {
|
||
|
|
// flyToPoint(mWaypoint);
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// class SendVirtualStickDataTask extends TimerTask {
|
||
|
|
// @Override
|
||
|
|
// public void run() {
|
||
|
|
// param.setPitch((double) mPitch);//左右
|
||
|
|
// param.setRoll((double) mRoll);//前后
|
||
|
|
// param.setYaw((double) mYaw);//旋转
|
||
|
|
// param.setVerticalThrottle((double) mThrottle);//上下
|
||
|
|
// VirtualStickManager.getInstance().sendVirtualStickAdvancedParam(param);
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
//}
|