946 lines
42 KiB
Java
946 lines
42 KiB
Java
|
package com.aros.apron.tools;
|
|||
|
|
|
|||
|
|
import android.os.Handler;
|
|||
|
|
import android.os.Looper;
|
|||
|
|
|
|||
|
|
import com.aros.apron.constant.AMSConfig;
|
|||
|
|
import com.aros.apron.entity.ArucoMarker;
|
|||
|
|
import com.aros.apron.entity.ArucoMarkerDimensions;
|
|||
|
|
import com.aros.apron.entity.Movement;
|
|||
|
|
import com.aros.apron.manager.AlternateLandingManager;
|
|||
|
|
|
|||
|
|
import org.opencv.aruco.Aruco;
|
|||
|
|
import org.opencv.aruco.Dictionary;
|
|||
|
|
import org.opencv.calib3d.Calib3d;
|
|||
|
|
import org.opencv.core.Core;
|
|||
|
|
import org.opencv.core.CvType;
|
|||
|
|
import org.opencv.core.Mat;
|
|||
|
|
import org.opencv.core.MatOfInt;
|
|||
|
|
import org.opencv.core.MatOfPoint2f;
|
|||
|
|
import org.opencv.core.Point;
|
|||
|
|
import org.opencv.core.Scalar;
|
|||
|
|
import org.opencv.imgproc.Imgproc;
|
|||
|
|
|
|||
|
|
import java.util.ArrayList;
|
|||
|
|
import java.util.Arrays;
|
|||
|
|
import java.util.List;
|
|||
|
|
import java.util.concurrent.Executors;
|
|||
|
|
import java.util.concurrent.Future;
|
|||
|
|
import java.util.concurrent.ScheduledExecutorService;
|
|||
|
|
import java.util.concurrent.TimeUnit;
|
|||
|
|
|
|||
|
|
public class ApronArucoDetect {
|
|||
|
|
|
|||
|
|
//是否触发识别(如果丢失图传，此值为false)
|
|||
|
|
private boolean isTriggerSuccess;
|
|||
|
|
|
|||
|
|
//没识别到二维码
|
|||
|
|
private boolean arucoNotFoundTag;
|
|||
|
|
|
|||
|
|
private boolean isStartAruco=false;
|
|||
|
|
// public ExecutorService mThreadPool = Executors.newSingleThreadExecutor();
|
|||
|
|
ScheduledExecutorService executor = Executors.newScheduledThreadPool(1);
|
|||
|
|
Future<?> lastFuture = null;
|
|||
|
|
|
|||
|
|
private String TAG = getClass().getSimpleName();
|
|||
|
|
Double resultYaw = 0.0;
|
|||
|
|
private List<ArucoMarker> mFindArucoList = new ArrayList<>();
|
|||
|
|
List<Mat> mArucoCornerList = new ArrayList<>();
|
|||
|
|
//触发去备降点
|
|||
|
|
private boolean triggerToAlternateLandingPoint;
|
|||
|
|
long startTime;
|
|||
|
|
long endTime;
|
|||
|
|
private int detectedBigMarkerId;
|
|||
|
|
private int detectedSmallMarkerId;
|
|||
|
|
private boolean detectedMediumMarkers;
|
|||
|
|
private boolean detectedSmallMarkers;
|
|||
|
|
//当确认识别单一的二维码后，有概率下降途中识别不到，此时次数超过15次,可以控制识别别的二维码
|
|||
|
|
private int sigleMarkerDetectFailsTimes;
|
|||
|
|
//复降触发条件
|
|||
|
|
private boolean dropTimesTag;
|
|||
|
|
//复降次数
|
|||
|
|
private int dropTimes;
|
|||
|
|
//是否双挂
|
|||
|
|
private boolean isDoublePayload;
|
|||
|
|
|
|||
|
|
public PIDControl pidControlX = null;
|
|||
|
|
public PIDControl pidControlY = null;
|
|||
|
|
|
|||
|
|
//为了解决降落后不停浆问题，增加记录，开启速降时startFastStick之后的过滤次数
|
|||
|
|
public int checkThrowingErrorsTimes;
|
|||
|
|
|
|||
|
|
public int getCheckThrowingErrorsTimes() {
|
|||
|
|
return checkThrowingErrorsTimes;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public void setCheckThrowingErrorsTimes(int checkThrowingErrorsTimes) {
|
|||
|
|
this.checkThrowingErrorsTimes = checkThrowingErrorsTimes;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public boolean isTriggerSuccess() {
|
|||
|
|
return isTriggerSuccess;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public void setTriggerSuccess(boolean triggerSuccess) {
|
|||
|
|
isTriggerSuccess = triggerSuccess;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public boolean isDoublePayload() {
|
|||
|
|
return isDoublePayload;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public void setDoublePayload(boolean doublePayload) {
|
|||
|
|
isDoublePayload = doublePayload;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
private ApronArucoDetect() {
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private static class OpenCVHelperHolder {
|
|||
|
|
private static final ApronArucoDetect INSTANCE = new ApronArucoDetect();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public static ApronArucoDetect getInstance() {
|
|||
|
|
return OpenCVHelperHolder.INSTANCE;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public void init() {
|
|||
|
|
pidControlX = new PIDControl(0.8f, 0.002f, 0.09f, 0.05f, 2.0f, 0.05f);
|
|||
|
|
pidControlY = new PIDControl(0.8f, 0.002f, 0.09f, 0.05f, 2.0f, 0.05f);
|
|||
|
|
pidControlX.reset();
|
|||
|
|
pidControlY.reset();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
public void detectArucoTags(int height, int width, byte[] data, Dictionary dictionary) {
|
|||
|
|
//这里说明图传正常
|
|||
|
|
isTriggerSuccess=true;
|
|||
|
|
Movement.getInstance().setVirtualStickEnableReason(2);
|
|||
|
|
if (isStartAruco || startFastStick) {
|
|||
|
|
LogUtil.log(TAG, "过滤:" + isStartAruco + startFastStick);
|
|||
|
|
if (!isStartAruco&&startFastStick){
|
|||
|
|
checkThrowingErrorsTimes++;
|
|||
|
|
}
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
isStartAruco = true;
|
|||
|
|
if (lastFuture != null && !lastFuture.isDone()) {
|
|||
|
|
LogUtil.log(TAG, "break---");
|
|||
|
|
lastFuture.cancel(true);
|
|||
|
|
}
|
|||
|
|
lastFuture =executor.schedule(new Runnable() {
|
|||
|
|
@Override
|
|||
|
|
public void run() {
|
|||
|
|
try {
|
|||
|
|
Mat yuvMat = new Mat(height + height / 2, width, CvType.CV_8UC1);
|
|||
|
|
yuvMat.put(0, 0, data);
|
|||
|
|
Mat rgbMat = new Mat();
|
|||
|
|
Imgproc.cvtColor(yuvMat, rgbMat, Imgproc.COLOR_YUV2BGR_I420);
|
|||
|
|
// 灰度
|
|||
|
|
Mat grayImgMat = new Mat();
|
|||
|
|
Imgproc.cvtColor(rgbMat, grayImgMat, Imgproc.COLOR_RGBA2GRAY);
|
|||
|
|
MatOfInt ids = new MatOfInt();
|
|||
|
|
mFindArucoList.clear();
|
|||
|
|
mArucoCornerList.clear();
|
|||
|
|
MatOfPoint2f corner = new MatOfPoint2f();
|
|||
|
|
|
|||
|
|
Aruco.detectMarkers(grayImgMat, dictionary, mArucoCornerList, ids);
|
|||
|
|
if (ids.depth() > 0) {
|
|||
|
|
arucoNotFoundTag = false;
|
|||
|
|
int[] idArray = ids.toArray();
|
|||
|
|
int ultrasonicHeight = Movement.getInstance().getUltrasonicHeight();
|
|||
|
|
double flyingHeight = Movement.getInstance().getElevation();
|
|||
|
|
if (idArray.length == 1 && idArray[0] == 5) {
|
|||
|
|
Core.extractChannel(mArucoCornerList.get(0), corner, 0);
|
|||
|
|
Point[] points = corner.toArray();
|
|||
|
|
// 计算宽度（两个相邻角点之间的距离）
|
|||
|
|
double width = calculateDistance(points[0], points[1]);
|
|||
|
|
if (!startFastStick&&width >= 1250 && ultrasonicHeight <= 4) {
|
|||
|
|
String logMessage = "参考5号Marker尺寸降落:" + idArray[0] + " arucoW" + width +
|
|||
|
|
" Flying Height:" + flyingHeight + "--" +
|
|||
|
|
" Ultrasonic Height:" + ultrasonicHeight;
|
|||
|
|
startFastStick = true;
|
|||
|
|
handler.post(runnable);
|
|||
|
|
LogUtil.log(TAG, logMessage);
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (idArray[0] == 11 || idArray[0] == 12 || idArray[0] == 13 ||
|
|||
|
|
idArray[0] == 14 || idArray[0] == 15 || idArray[0] == 16 || idArray[0] == 17
|
|||
|
|
|| idArray[0] == 18 || idArray[0] == 19|| idArray[0] == 20|| idArray[0] == 21|| idArray[0] == 22
|
|||
|
|
|| idArray[0] == 23|| idArray[0] == 24|| idArray[0] == 25) {
|
|||
|
|
if (!startFastStick) {
|
|||
|
|
if ((idArray.length >= 7 && ultrasonicHeight <= 3 && flyingHeight < 3)) {
|
|||
|
|
String logMessage = "参考Marker数目降落:" + idArray.length +
|
|||
|
|
" Flying Height:" + flyingHeight + "--" +
|
|||
|
|
" Ultrasonic Height:" + ultrasonicHeight;
|
|||
|
|
startFastStick = true;
|
|||
|
|
handler.post(runnable);
|
|||
|
|
LogUtil.log(TAG, logMessage);
|
|||
|
|
return;
|
|||
|
|
} else {
|
|||
|
|
Core.extractChannel(mArucoCornerList.get(0), corner, 0);
|
|||
|
|
Point[] points = corner.toArray();
|
|||
|
|
// 计算宽度（两个相邻角点之间的距离）
|
|||
|
|
double width = calculateDistance(points[0], points[1]);
|
|||
|
|
// 计算高度（另外两个相邻角点之间的距离）
|
|||
|
|
// double height = calculateDistance(points[1], points[2]);
|
|||
|
|
//存在识别到小Marker，但参考5号的尺寸降落的可能性
|
|||
|
|
if (width >= 260 && idArray.length >= 5 && ultrasonicHeight <= 3) {
|
|||
|
|
String logMessage = "参考Marker尺寸降落:" + idArray[0] + " arucoW" + width +
|
|||
|
|
" Flying Height:" + flyingHeight + "--" +
|
|||
|
|
" Ultrasonic Height:" + ultrasonicHeight;
|
|||
|
|
startFastStick = true;
|
|||
|
|
handler.post(runnable);
|
|||
|
|
LogUtil.log(TAG, logMessage);
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
findAruco(idArray);
|
|||
|
|
|
|||
|
|
if (mFindArucoList.size() == 0) {
|
|||
|
|
sigleMarkerDetectFailsTimes++;
|
|||
|
|
if (sigleMarkerDetectFailsTimes >= 25) {
|
|||
|
|
sigleMarkerDetectFailsTimes = 0;
|
|||
|
|
setDetectedBigMarkers();
|
|||
|
|
LogUtil.log(TAG, "重置识别二维码状态:" + idArray.length+" id:"+idArray[0]);
|
|||
|
|
markerId5MaxFindHeight=-20.0;
|
|||
|
|
}
|
|||
|
|
} else {
|
|||
|
|
sigleMarkerDetectFailsTimes = 0;
|
|||
|
|
Core.extractChannel(mFindArucoList.get(0).getConner(), corner, 0);
|
|||
|
|
Point[] points = corner.toArray();
|
|||
|
|
// 计算宽度（两个相邻角点之间的距离）
|
|||
|
|
double width = calculateDistance(points[0], points[1]);
|
|||
|
|
moveOnArucoDetected(mFindArucoList, rgbMat.width(), rgbMat.height(), width);
|
|||
|
|
}
|
|||
|
|
dropTimesTag = true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
else {
|
|||
|
|
if (!arucoNotFoundTag) {
|
|||
|
|
startTime = System.currentTimeMillis();
|
|||
|
|
arucoNotFoundTag = true;
|
|||
|
|
}
|
|||
|
|
endTime = System.currentTimeMillis();
|
|||
|
|
//记录第一次识别不到二维码的时间,如果小于20s,拉高或拉低复降,否则降落至备降点
|
|||
|
|
if (endTime - startTime > 700 && endTime - startTime <= 8000) {
|
|||
|
|
if (Movement.getInstance().getElevation() <= 8) {
|
|||
|
|
//可能由于appCrash后，识别不到二维码，尝试将飞机拉高识别
|
|||
|
|
setDetectedBigMarkers();
|
|||
|
|
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 0.7);
|
|||
|
|
if (dropTimes > Integer.parseInt(AMSConfig.getInstance().getAlternateLandingTimes())) {
|
|||
|
|
LogUtil.log(TAG, "超过复降限制,去备降点");
|
|||
|
|
AlternateLandingManager.getInstance().startTaskProcess(null);
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
if (dropTimesTag) {
|
|||
|
|
dropTimesTag = false;
|
|||
|
|
dropTimes++;
|
|||
|
|
// virtualStickAdvancedParam=0.145;
|
|||
|
|
LogUtil.log(TAG, "复降第:" + dropTimes + "次");
|
|||
|
|
}
|
|||
|
|
} else if (Movement.getInstance().getElevation() > 8) {
|
|||
|
|
//可能是由于飞机太高，识别不到二维码，尝试将飞机拉低识别
|
|||
|
|
setDetectedBigMarkers();
|
|||
|
|
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.4);
|
|||
|
|
}
|
|||
|
|
} else if (endTime - startTime > 8000) {
|
|||
|
|
if (!triggerToAlternateLandingPoint) {
|
|||
|
|
triggerToAlternateLandingPoint = true;
|
|||
|
|
LogUtil.log(TAG, "判定未识别到二维码,飞往备降点");
|
|||
|
|
AlternateLandingManager.getInstance().startTaskProcess(null);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
ids.release();
|
|||
|
|
yuvMat.release();
|
|||
|
|
rgbMat.release();
|
|||
|
|
grayImgMat.release();
|
|||
|
|
mFindArucoList.clear();
|
|||
|
|
mArucoCornerList.clear();
|
|||
|
|
corner.release();
|
|||
|
|
isStartAruco = false;
|
|||
|
|
} catch (Exception e) {
|
|||
|
|
isStartAruco = false;
|
|||
|
|
mFindArucoList.clear();
|
|||
|
|
mArucoCornerList.clear();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
},0, TimeUnit.MILLISECONDS);
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* 61 * 计算两个点之间的欧几里得距离
|
|||
|
|
* 62 * @param p1 第一个点
|
|||
|
|
* 63 * @param p2 第二个点
|
|||
|
|
* 64 * @return 两点之间的距离
|
|||
|
|
* 65
|
|||
|
|
*/
|
|||
|
|
private double calculateDistance(Point p1, Point p2) {
|
|||
|
|
double dx = p2.x - p1.x;
|
|||
|
|
double dy = p2.y - p1.y;
|
|||
|
|
return Math.sqrt(dx * dx + dy * dy);
|
|||
|
|
}
|
|||
|
|
//飞机椭球高度返回过低，导致5号Marker过早的不识别，悬停在3-4米处(浙江海事，汾湖演示出现过)
|
|||
|
|
private double markerId5MaxFindHeight=0.7;
|
|||
|
|
private double markerId1234MinFindHeight=7;
|
|||
|
|
public void findAruco(int[] idArray) {
|
|||
|
|
if (Movement.getInstance().getElevation()<markerId1234MinFindHeight){
|
|||
|
|
if (Movement.getInstance().getElevation() <= 1.5
|
|||
|
|
|| Movement.getInstance().getUltrasonicHeight() < 15) {
|
|||
|
|
if (isDoublePayload) {
|
|||
|
|
if (mFindArucoList.isEmpty()) {
|
|||
|
|
for (int i = 0; i < idArray.length; i++) {
|
|||
|
|
if (idArray[i] == 21) {
|
|||
|
|
detectedSmallMarkers = true;
|
|||
|
|
detectedSmallMarkerId = 21;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
if (idArray[i] == 22 && detectedSmallMarkerId != 21) {
|
|||
|
|
detectedSmallMarkers = true;
|
|||
|
|
detectedSmallMarkerId = 22;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
if (idArray[i] == 23 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22) {
|
|||
|
|
detectedSmallMarkers = true;
|
|||
|
|
detectedSmallMarkerId = 23;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (idArray[i] == 24 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22 && detectedSmallMarkerId != 23) {
|
|||
|
|
detectedSmallMarkers = true;
|
|||
|
|
detectedSmallMarkerId = 24;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (idArray[i] == 19 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22 && detectedSmallMarkerId != 23 && detectedSmallMarkerId != 24) {
|
|||
|
|
detectedSmallMarkers = true;
|
|||
|
|
detectedSmallMarkerId = 19;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (idArray[i] == 20 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22 && detectedSmallMarkerId != 23 && detectedSmallMarkerId != 24 && detectedSmallMarkerId != 19) {
|
|||
|
|
detectedSmallMarkers = true;
|
|||
|
|
detectedSmallMarkerId = 20;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
if (idArray[i] == 17 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22 && detectedSmallMarkerId != 23 && detectedSmallMarkerId != 24 && detectedSmallMarkerId != 19 && detectedSmallMarkerId != 20) {
|
|||
|
|
detectedSmallMarkers = true;
|
|||
|
|
detectedSmallMarkerId = 17;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
if (idArray[i] == 18 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22 && detectedSmallMarkerId != 23 && detectedSmallMarkerId != 24 && detectedSmallMarkerId != 19 && detectedSmallMarkerId != 20 && detectedSmallMarkerId != 17) {
|
|||
|
|
detectedSmallMarkers = true;
|
|||
|
|
detectedSmallMarkerId = 18;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
} else {
|
|||
|
|
if (mFindArucoList.isEmpty()) {
|
|||
|
|
for (int i = 0; i < idArray.length; i++) {
|
|||
|
|
if (idArray[i] == 19) {
|
|||
|
|
detectedSmallMarkers = true;
|
|||
|
|
detectedSmallMarkerId = 19;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
if (idArray[i] == 20 && detectedSmallMarkerId != 19) {
|
|||
|
|
detectedSmallMarkers = true;
|
|||
|
|
detectedSmallMarkerId = 20;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
if (idArray[i] == 21 && detectedSmallMarkerId != 19 && detectedSmallMarkerId != 20) {
|
|||
|
|
detectedSmallMarkers = true;
|
|||
|
|
detectedSmallMarkerId = 21;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
if (idArray[i] == 22 && detectedSmallMarkerId != 19 && detectedSmallMarkerId != 20 && detectedSmallMarkerId != 21) {
|
|||
|
|
detectedSmallMarkers = true;
|
|||
|
|
detectedSmallMarkerId = 22;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (idArray[i] == 17 && detectedSmallMarkerId != 19 && detectedSmallMarkerId != 20 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22) {
|
|||
|
|
detectedSmallMarkers = true;
|
|||
|
|
detectedSmallMarkerId = 17;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
if (idArray[i] == 18 && detectedSmallMarkerId != 19 && detectedSmallMarkerId != 20 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22 && detectedSmallMarkerId != 17) {
|
|||
|
|
detectedSmallMarkers = true;
|
|||
|
|
detectedSmallMarkerId = 18;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
if (idArray[i] == 23 && detectedSmallMarkerId != 19 && detectedSmallMarkerId != 20 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22 && detectedSmallMarkerId != 17 && detectedSmallMarkerId != 18) {
|
|||
|
|
detectedSmallMarkers = true;
|
|||
|
|
detectedSmallMarkerId = 23;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
if (idArray[i] == 24 && detectedSmallMarkerId != 19 && detectedSmallMarkerId != 20 && detectedSmallMarkerId != 21 && detectedSmallMarkerId != 22 && detectedSmallMarkerId != 17 && detectedSmallMarkerId != 18 && detectedSmallMarkerId != 23) {
|
|||
|
|
detectedSmallMarkers = true;
|
|||
|
|
detectedSmallMarkerId = 24;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.03f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (Movement.getInstance().getElevation() > markerId5MaxFindHeight &&
|
|||
|
|
mFindArucoList.isEmpty() && !detectedSmallMarkers) {
|
|||
|
|
|
|||
|
|
if (
|
|||
|
|
(detectedBigMarkerId == 0
|
|||
|
|
|| detectedBigMarkerId == 1
|
|||
|
|
|| detectedBigMarkerId == 2
|
|||
|
|
|| detectedBigMarkerId == 3
|
|||
|
|
|| detectedBigMarkerId == 4
|
|||
|
|
|| detectedBigMarkerId == 5
|
|||
|
|
|| detectedBigMarkerId == 6
|
|||
|
|
)) {
|
|||
|
|
for (int i = 0; i < idArray.length; i++) {
|
|||
|
|
if (idArray[i] == 5) {
|
|||
|
|
detectedBigMarkerId = 5;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.15f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (Movement.getInstance().getElevation() > 2.5) {
|
|||
|
|
if (mFindArucoList.isEmpty() && !detectedSmallMarkers &&
|
|||
|
|
(detectedBigMarkerId == 0 || detectedBigMarkerId == 1 || detectedBigMarkerId == 2
|
|||
|
|
|| detectedBigMarkerId == 3 || detectedBigMarkerId == 4)) {
|
|||
|
|
for (int i = 0; i < idArray.length; i++) {
|
|||
|
|
if (idArray[i] == 1) {
|
|||
|
|
detectedBigMarkerId = 1;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.18f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (mFindArucoList.isEmpty() && !detectedSmallMarkers && (detectedBigMarkerId == 0 ||
|
|||
|
|
detectedBigMarkerId == 2 || detectedBigMarkerId == 3 || detectedBigMarkerId == 4)) {
|
|||
|
|
for (int i = 0; i < idArray.length; i++) {
|
|||
|
|
if (idArray[i] == 2) {
|
|||
|
|
detectedBigMarkerId = 2;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.18f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if (mFindArucoList.isEmpty() && !detectedSmallMarkers && (detectedBigMarkerId == 0 || detectedBigMarkerId == 3)) {
|
|||
|
|
for (int i = 0; i < idArray.length; i++) {
|
|||
|
|
if (idArray[i] == 3) {
|
|||
|
|
detectedBigMarkerId = 3;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.18f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if (mFindArucoList.isEmpty() && !detectedSmallMarkers && (detectedBigMarkerId == 0 || detectedBigMarkerId == 4)) {
|
|||
|
|
for (int i = 0; i < idArray.length; i++) {
|
|||
|
|
if (idArray[i] == 4) {
|
|||
|
|
detectedBigMarkerId = 4;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.18f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if (mFindArucoList.isEmpty() && !detectedSmallMarkers && (detectedBigMarkerId == 0 || detectedBigMarkerId == 6)) {
|
|||
|
|
for (int i = 0; i < idArray.length; i++) {
|
|||
|
|
if (idArray[i] == 6) {
|
|||
|
|
detectedBigMarkerId = 6;
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), 0.24f));
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
} else {
|
|||
|
|
if (idArray.length > 5) {
|
|||
|
|
idArray = Arrays.copyOf(idArray, 5);
|
|||
|
|
}
|
|||
|
|
for (int i = 0; i < idArray.length; i++) {
|
|||
|
|
mFindArucoList.add(new ArucoMarker(idArray[i], mArucoCornerList.get(i), ArucoMarkerDimensions.getSizeById(idArray[i])));
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
public void setDetectedBigMarkers() {
|
|||
|
|
detectedBigMarkerId = 0;
|
|||
|
|
detectedSmallMarkerId = 0;
|
|||
|
|
detectedMediumMarkers = false;
|
|||
|
|
detectedSmallMarkers = false;
|
|||
|
|
startFastStick = false;
|
|||
|
|
isStartAruco = false;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//根据识别到的二维码移动无人机
|
|||
|
|
private void moveOnArucoDetected(List<ArucoMarker> arucoMarkers, int imageWidth, int imageHeight, double arucoWidth) {
|
|||
|
|
int id = arucoMarkers.get(0).getId();
|
|||
|
|
int ultrasonicHeight = Movement.getInstance().getUltrasonicHeight();
|
|||
|
|
// 计算图像中心
|
|||
|
|
Point imgCenter = new Point(imageWidth / 2.0, imageHeight / 2.0);
|
|||
|
|
double sumX = 0, sumY = 0;
|
|||
|
|
int markerCount = arucoMarkers.size();
|
|||
|
|
// 遍历所有 marker 计算中心点
|
|||
|
|
for (int i = 0; i < markerCount; i++) {
|
|||
|
|
Mat markerCorners = arucoMarkers.get(i).getConner();
|
|||
|
|
double centerX = 0, centerY = 0;
|
|||
|
|
|
|||
|
|
for (int j = 0; j < 4; j++) {
|
|||
|
|
double[] point = markerCorners.get(0, j);
|
|||
|
|
centerX += point[0];
|
|||
|
|
centerY += point[1];
|
|||
|
|
}
|
|||
|
|
centerX /= 4.0;
|
|||
|
|
centerY /= 4.0;
|
|||
|
|
|
|||
|
|
sumX += centerX;
|
|||
|
|
sumY += centerY;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 计算所有 marker 的平均中心点
|
|||
|
|
double avgCenterX = sumX / markerCount;
|
|||
|
|
double avgCenterY = sumY / markerCount;
|
|||
|
|
|
|||
|
|
// 计算整体偏移量
|
|||
|
|
double offsetX = avgCenterX - imgCenter.x;
|
|||
|
|
double offsetY = avgCenterY - imgCenter.y;
|
|||
|
|
double outX;
|
|||
|
|
double outY;
|
|||
|
|
double outZ;
|
|||
|
|
|
|||
|
|
//相机内参
|
|||
|
|
Mat cameraMatrix = Mat.zeros(3, 3, CvType.CV_64F);
|
|||
|
|
cameraMatrix.put(0, 0, 1131.3484309796945);
|
|||
|
|
cameraMatrix.put(1, 1, 1143.0319750579686);
|
|||
|
|
cameraMatrix.put(0, 2, 676.696876660099);
|
|||
|
|
cameraMatrix.put(1, 2, 532.6254545540435);
|
|||
|
|
cameraMatrix.put(2, 2, 1.0);
|
|||
|
|
//相机畸变
|
|||
|
|
Mat distCoeffs = Mat.zeros(5, 1, CvType.CV_64FC1);
|
|||
|
|
distCoeffs.put(0, 0, -0.16879686656897544);
|
|||
|
|
distCoeffs.put(1, 0, 0.4252674979687209);
|
|||
|
|
distCoeffs.put(2, 0, 0.004260616672174669);
|
|||
|
|
distCoeffs.put(3, 0, 0.010597384861297276);
|
|||
|
|
distCoeffs.put(4, 0, -0.6032569042575567);
|
|||
|
|
//旋转矩阵
|
|||
|
|
Mat rvecs = new Mat();
|
|||
|
|
//位移矩阵
|
|||
|
|
Mat tvecs = new Mat();
|
|||
|
|
//姿态预估
|
|||
|
|
List<Mat> conners = new ArrayList<>();
|
|||
|
|
conners.add(arucoMarkers.get(0).getConner());
|
|||
|
|
Aruco.estimatePoseSingleMarkers(conners, arucoMarkers.get(0).getSize(), cameraMatrix, distCoeffs, rvecs, tvecs);
|
|||
|
|
|
|||
|
|
Mat tvec = tvecs.row(0);
|
|||
|
|
double z = tvec.get(0, 0)[2];
|
|||
|
|
double x = tvec.get(0,0)[0];
|
|||
|
|
double y = tvec.get(0,0)[1];
|
|||
|
|
if (z > 2 && (
|
|||
|
|
id == 1
|
|||
|
|
|| id == 2
|
|||
|
|
|| id == 3
|
|||
|
|
|| id == 4
|
|||
|
|
|| id == 5
|
|||
|
|
|| id == 6
|
|||
|
|
)
|
|||
|
|
) {
|
|||
|
|
//罗德里变换
|
|||
|
|
Mat R = new Mat(3, 3, CvType.CV_32FC1);
|
|||
|
|
Mat rvec = rvecs.row(0);
|
|||
|
|
Calib3d.Rodrigues(rvec, R);
|
|||
|
|
Mat camR = R.t();
|
|||
|
|
//左乘
|
|||
|
|
Mat _camR = new Mat();
|
|||
|
|
Scalar _1 = new Scalar(-1.0);
|
|||
|
|
Core.multiply(camR, _1, _camR);
|
|||
|
|
//旋转向量转欧拉角
|
|||
|
|
List<Double> eulerAngles = RotationConversion.INSTANCE.rotationMatrixToEulerAngles(camR);
|
|||
|
|
//欧拉角转飞机偏航角度
|
|||
|
|
double yawCamera = MathUtils.toDegree(eulerAngles.get(2));
|
|||
|
|
if (yawCamera < 0) {
|
|||
|
|
if (yawCamera < -15 && z < 9 && z > 3) {
|
|||
|
|
if (yawCamera < -80) {
|
|||
|
|
resultYaw = -40.0;
|
|||
|
|
} else {
|
|||
|
|
resultYaw = -30.0;
|
|||
|
|
}
|
|||
|
|
} else {
|
|||
|
|
resultYaw = 0.0;
|
|||
|
|
}
|
|||
|
|
} else {
|
|||
|
|
if (yawCamera >= 15 && z < 9 && z > 3) {
|
|||
|
|
if (yawCamera > 80) {
|
|||
|
|
resultYaw = 40.0;
|
|||
|
|
} else {
|
|||
|
|
resultYaw = 30.0;
|
|||
|
|
}
|
|||
|
|
} else {
|
|||
|
|
resultYaw = 0.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
rvecs.release();
|
|||
|
|
tvecs.release();
|
|||
|
|
rvec.release();
|
|||
|
|
camR.release();
|
|||
|
|
_camR.release();
|
|||
|
|
R.release();
|
|||
|
|
} else {
|
|||
|
|
resultYaw = 0.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//先旋转,再平移或降落
|
|||
|
|
double absX = Math.abs(offsetX);
|
|||
|
|
double absY = Math.abs(offsetY);
|
|||
|
|
|
|||
|
|
if (resultYaw != 0.0) {
|
|||
|
|
outX = 0.0f;
|
|||
|
|
outY = 0.0f;
|
|||
|
|
outZ = 0.0f;
|
|||
|
|
} else {
|
|||
|
|
|
|||
|
|
if(z <=0.4){
|
|||
|
|
pidControlX.setInputFilterAll((float)offsetX/1750);
|
|||
|
|
pidControlY.setInputFilterAll(-(float)offsetY/1750);
|
|||
|
|
if (pidControlX.get_pid()<0){
|
|||
|
|
if (pidControlX.get_pid()<-0.125){
|
|||
|
|
outX=absX<120?0:-0.125;
|
|||
|
|
}else{
|
|||
|
|
outX=absX<120?0:pidControlX.get_pid();
|
|||
|
|
}
|
|||
|
|
}else{
|
|||
|
|
if (pidControlX.get_pid()>0.125){
|
|||
|
|
outX=absX<120?0:0.125;
|
|||
|
|
}else{
|
|||
|
|
outX=absX<120?0:pidControlX.get_pid();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (pidControlY.get_pid()<0){
|
|||
|
|
if (pidControlY.get_pid()<-0.125){
|
|||
|
|
outY=absY<120?0:-0.125;
|
|||
|
|
}else{
|
|||
|
|
outY=absY<120?0:pidControlY.get_pid();
|
|||
|
|
}
|
|||
|
|
}else{
|
|||
|
|
if (pidControlY.get_pid()>0.125){
|
|||
|
|
outY=absY<120?0:0.125;
|
|||
|
|
}else{
|
|||
|
|
outY=absY<120?0:pidControlY.get_pid();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
outZ = (absX < 230)
|
|||
|
|
&& (absY < 250)
|
|||
|
|
? -0.2 : 0;
|
|||
|
|
}else if(z <=0.7){
|
|||
|
|
pidControlX.setInputFilterAll((float)offsetX/1750);
|
|||
|
|
pidControlY.setInputFilterAll(-(float)offsetY/1750);
|
|||
|
|
if (pidControlX.get_pid()<0){
|
|||
|
|
if (pidControlX.get_pid()<-0.135){
|
|||
|
|
outX=absX<120?0:-0.135;
|
|||
|
|
}else{
|
|||
|
|
outX=absX<120?0:pidControlX.get_pid();
|
|||
|
|
}
|
|||
|
|
}else{
|
|||
|
|
if (pidControlX.get_pid()>0.135){
|
|||
|
|
outX=absX<120?0:0.135;
|
|||
|
|
}else{
|
|||
|
|
outX=absX<120?0:pidControlX.get_pid();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (pidControlY.get_pid()<0){
|
|||
|
|
if (pidControlY.get_pid()<-0.135){
|
|||
|
|
outY=absY<120?0:-0.135;
|
|||
|
|
}else{
|
|||
|
|
outY=absY<120?0:pidControlY.get_pid();
|
|||
|
|
}
|
|||
|
|
}else{
|
|||
|
|
if (pidControlY.get_pid()>0.135){
|
|||
|
|
outY=absY<120?0:0.135;
|
|||
|
|
}else{
|
|||
|
|
outY=absY<120?0:pidControlY.get_pid();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
outZ = (absX < 250)
|
|||
|
|
&& (absY < 250)
|
|||
|
|
? -0.35 : 0;
|
|||
|
|
}else if(z <=1){
|
|||
|
|
pidControlX.setInputFilterAll((float)offsetX/1550);
|
|||
|
|
pidControlY.setInputFilterAll(-(float)offsetY/1550);
|
|||
|
|
if (pidControlX.get_pid()<0){
|
|||
|
|
if (pidControlX.get_pid()<-0.145){
|
|||
|
|
outX=absX<120?0:-0.145;
|
|||
|
|
}else{
|
|||
|
|
outX=absX<120?0:pidControlX.get_pid();
|
|||
|
|
}
|
|||
|
|
}else{
|
|||
|
|
if (pidControlX.get_pid()>0.145){
|
|||
|
|
outX=absX<120?0:0.145;
|
|||
|
|
}else{
|
|||
|
|
outX=absX<120?0:pidControlX.get_pid();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (pidControlY.get_pid()<0){
|
|||
|
|
if (pidControlY.get_pid()<-0.145){
|
|||
|
|
outY=absY<120?0:-0.145;
|
|||
|
|
}else{
|
|||
|
|
outY=absY<120?0:pidControlY.get_pid();
|
|||
|
|
}
|
|||
|
|
}else{
|
|||
|
|
if (pidControlY.get_pid()>0.145){
|
|||
|
|
outY=absY<120?0:0.145;
|
|||
|
|
}else{
|
|||
|
|
outY=absY<120?0:pidControlY.get_pid();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
outZ = (absX < 180)
|
|||
|
|
&& (absY < 180)
|
|||
|
|
? -0.4 : 0;
|
|||
|
|
}else if(z <=1.5){
|
|||
|
|
pidControlX.setInputFilterAll((float)offsetX/1450);
|
|||
|
|
pidControlY.setInputFilterAll(-(float)offsetY/1450);
|
|||
|
|
if (pidControlX.get_pid()<0){
|
|||
|
|
if (pidControlX.get_pid()<-0.185){
|
|||
|
|
outX=absX<120?0:-0.185;
|
|||
|
|
}else{
|
|||
|
|
outX=absX<120?0:pidControlX.get_pid();
|
|||
|
|
}
|
|||
|
|
}else{
|
|||
|
|
if (pidControlX.get_pid()>0.185){
|
|||
|
|
outX=absX<120?0:0.185;
|
|||
|
|
}else{
|
|||
|
|
outX=absX<120?0:pidControlX.get_pid();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (pidControlY.get_pid()<0){
|
|||
|
|
if (pidControlY.get_pid()<-0.185){
|
|||
|
|
outY=absY<120?0:-0.185;
|
|||
|
|
}else{
|
|||
|
|
outY=absY<120?0:pidControlY.get_pid();
|
|||
|
|
}
|
|||
|
|
}else{
|
|||
|
|
if (pidControlY.get_pid()>0.185){
|
|||
|
|
outY=absY<120?0:0.185;
|
|||
|
|
}else{
|
|||
|
|
outY=absY<120?0:pidControlY.get_pid();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
outZ = (absX < 180)
|
|||
|
|
&& (absY < 180)
|
|||
|
|
? -0.4 : 0;
|
|||
|
|
}else if(z <=2){
|
|||
|
|
pidControlX.setInputFilterAll((float)offsetX/1350);
|
|||
|
|
pidControlY.setInputFilterAll(-(float)offsetY/1350);
|
|||
|
|
outX = absX<120?0:pidControlX.get_pid();
|
|||
|
|
outY = absY<120?0:pidControlY.get_pid();
|
|||
|
|
outZ = (absX < 200)
|
|||
|
|
&& (absY < 200)
|
|||
|
|
? -0.575 : 0;
|
|||
|
|
|
|||
|
|
}else if(z <=3){
|
|||
|
|
pidControlX.setInputFilterAll((float)offsetX/1250);
|
|||
|
|
pidControlY.setInputFilterAll(-(float)offsetY/1250);
|
|||
|
|
outX = absX<120?0:pidControlX.get_pid();
|
|||
|
|
outY = absY<120?0:pidControlY.get_pid();
|
|||
|
|
outZ = (absX < 200)
|
|||
|
|
&& (absY < 200)
|
|||
|
|
? -0.575 : 0;
|
|||
|
|
}else if(z <=5){
|
|||
|
|
pidControlX.setInputFilterAll((float)offsetX/1050);
|
|||
|
|
pidControlY.setInputFilterAll(-(float)offsetY/1050);
|
|||
|
|
outX = absX<120?0:pidControlX.get_pid();
|
|||
|
|
outY = absY<120?0:pidControlY.get_pid();
|
|||
|
|
outZ = (absX < 200)
|
|||
|
|
&& (absY < 200)
|
|||
|
|
? -0.575 : 0;
|
|||
|
|
}else if (z <= 7) {
|
|||
|
|
pidControlX.setInputFilterAll((float)offsetX/950);
|
|||
|
|
pidControlY.setInputFilterAll(-(float)offsetY/950);
|
|||
|
|
outX = absX<120?0:pidControlX.get_pid();
|
|||
|
|
outY = absY<120?0:pidControlY.get_pid();
|
|||
|
|
outZ = (absX < 200)
|
|||
|
|
&& (absY < 200)
|
|||
|
|
? -0.575 : 0;
|
|||
|
|
|
|||
|
|
}else {
|
|||
|
|
pidControlX.setInputFilterAll((float)offsetX/850);
|
|||
|
|
pidControlY.setInputFilterAll(-(float)offsetY/850);
|
|||
|
|
outX = absX<80?0:pidControlX.get_pid();
|
|||
|
|
outY = absY<80?0:pidControlY.get_pid();
|
|||
|
|
outZ = (absX < 130)
|
|||
|
|
&& (absY < 130)
|
|||
|
|
? -0.65 : 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// LogUtil.log(TAG,
|
|||
|
|
// " 杆量x=" + outX +
|
|||
|
|
// " 偏移x=" + offsetX +
|
|||
|
|
// " 杆量y=" + outY +
|
|||
|
|
// " 偏移y=" + offsetY +
|
|||
|
|
// " Id=" + id +
|
|||
|
|
// " Size=" + arucoMarkers.size() +
|
|||
|
|
// " 宽度=" + arucoWidth +
|
|||
|
|
// " 椭球=" + Movement.getInstance().getElevation() +
|
|||
|
|
// " 融合=" + ultrasonicHeight +
|
|||
|
|
// " X=" + x +
|
|||
|
|
// " Z=" + z
|
|||
|
|
// );
|
|||
|
|
if ((Math.abs(outX)>0.3||Math.abs(outY)>0.3)&&(Movement.getInstance().getElevation()<3.5)){
|
|||
|
|
if (ultrasonicHeight<=25){
|
|||
|
|
outX=outX>0?0.125:-0.125;
|
|||
|
|
outY=outY>0?0.125:-0.125;
|
|||
|
|
LogUtil.log(TAG,"过滤帧："+" 杆量x=" + outX+" 杆量y=" + y);
|
|||
|
|
}else{
|
|||
|
|
LogUtil.log(TAG,"过滤帧>："+" 杆量x=" + outX+" 杆量y=" + y);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
DroneHelper.getInstance().moveVxVyYawrateHeight(outX,
|
|||
|
|
outY,
|
|||
|
|
resultYaw, outZ);
|
|||
|
|
|
|||
|
|
if (id == 11 || id == 12 || id == 13 || id == 14 || id == 15
|
|||
|
|
|| id == 16 || id == 17 || id == 18 || id == 19|| id == 20
|
|||
|
|
|| id == 21|| id == 22|| id == 23|| id == 24|| id == 25) {
|
|||
|
|
checkConditions(absX, absY, id, arucoWidth);
|
|||
|
|
|
|||
|
|
} else {
|
|||
|
|
canLanding = false;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
private void checkConditions(double absX, double absY, int id, double arucoWidth) {
|
|||
|
|
double ultrasonicHeight = Movement.getInstance().getUltrasonicHeight();
|
|||
|
|
double flyingHeight = Movement.getInstance().getElevation();
|
|||
|
|
boolean xy = absX < 250 && absY < 300;
|
|||
|
|
String logMessage = "";
|
|||
|
|
if (!startFastStick) {
|
|||
|
|
|
|||
|
|
// if (absX <= 250 && absY <= 300 && ultrasonicHeight <= 4 && flyingHeight <= 3) {
|
|||
|
|
if (absX <= 230 && absY <= 250 && ultrasonicHeight <= 3 && flyingHeight <= 3) {
|
|||
|
|
logMessage = "参考融合高度降落:" + id + " arucoW" + arucoWidth +
|
|||
|
|
" Flying Height:" + flyingHeight + "--" +
|
|||
|
|
" Ultrasonic Height:" + ultrasonicHeight;
|
|||
|
|
LogUtil.log(TAG, logMessage);
|
|||
|
|
startFastStick = true;
|
|||
|
|
handler.post(runnable);
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
if (xy && arucoWidth >= 280) {
|
|||
|
|
logMessage = "参考ArUco宽度+偏移量降落:" + id + " arucoW" + arucoWidth +
|
|||
|
|
" Flying Height:" + flyingHeight + "--" +
|
|||
|
|
" Ultrasonic Height:" + ultrasonicHeight;
|
|||
|
|
LogUtil.log(TAG, logMessage);
|
|||
|
|
startFastStick = true;
|
|||
|
|
handler.post(runnable);
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
if (arucoWidth >= 480) {
|
|||
|
|
logMessage = "参考ArUco宽度降落:" + id + " arucoW" + arucoWidth +
|
|||
|
|
" Flying Height:" + flyingHeight + "--" +
|
|||
|
|
" Ultrasonic Height:" + ultrasonicHeight;
|
|||
|
|
LogUtil.log(TAG, logMessage);
|
|||
|
|
startFastStick = true;
|
|||
|
|
handler.post(runnable);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
public boolean startFastStick;
|
|||
|
|
|
|||
|
|
public boolean isStartFastStick() {
|
|||
|
|
return startFastStick;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public void setStartFastStick(boolean startFastStick) {
|
|||
|
|
this.startFastStick = startFastStick;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public boolean canLanding;
|
|||
|
|
|
|||
|
|
public boolean isCanLanding() {
|
|||
|
|
return canLanding;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public void setCanLanding(boolean canLanding) {
|
|||
|
|
//测试重置未识别和识别时间,避免刚触发识别就飞向备降点
|
|||
|
|
startTime = 0;
|
|||
|
|
endTime = 0;
|
|||
|
|
this.canLanding = canLanding;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
private int handlerCallbackCount = 0; // 记录回调次数
|
|||
|
|
private Handler handler = new Handler(Looper.getMainLooper());
|
|||
|
|
private Runnable runnable = new Runnable() {
|
|||
|
|
@Override
|
|||
|
|
public void run() {
|
|||
|
|
performOperation();
|
|||
|
|
if (handlerCallbackCount < 20) {
|
|||
|
|
handler.postDelayed(this, 50); // 每 50 毫秒执行一次，1 秒内执行 20 次
|
|||
|
|
} else {
|
|||
|
|
performNextStep();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
private void performOperation() {
|
|||
|
|
// LogUtil.log(TAG,"快速下拉中..."+handlerCallbackCount);
|
|||
|
|
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -4);
|
|||
|
|
handlerCallbackCount++; // 增加计数器
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private void performNextStep() {
|
|||
|
|
canLanding = true;
|
|||
|
|
handlerCallbackCount = 0;
|
|||
|
|
dropTimes = 0;//手动测试避免多次累加后直接飞往备降点
|
|||
|
|
markerId5MaxFindHeight = 0.7;
|
|||
|
|
LogUtil.log(TAG,"下拉完成：触发下一步自动降落");
|
|||
|
|
handler.removeCallbacks(runnable); // 防止重复执行
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|