融合降落
This commit is contained in:
parent
1b0de5e340
commit
b601be0e49
|
|
@ -0,0 +1,670 @@
|
|||
package com.aros.apron.mix;
|
||||
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.Movement;
|
||||
import com.aros.apron.manager.AlternateLandingManager;
|
||||
import com.aros.apron.tools.DroneHelper;
|
||||
import com.aros.apron.tools.LogUtil;
|
||||
import com.aros.apron.tools.PIDControl;
|
||||
|
||||
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.core.Size;
|
||||
import org.opencv.imgproc.Imgproc;
|
||||
import org.opencv.objdetect.ArucoDetector;
|
||||
import org.opencv.objdetect.DetectorParameters;
|
||||
import org.opencv.objdetect.Dictionary;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.concurrent.Executors;
|
||||
import java.util.concurrent.Future;
|
||||
import java.util.concurrent.ScheduledExecutorService;
|
||||
import java.util.concurrent.TimeUnit;
|
||||
|
||||
import dji.sdk.keyvalue.key.FlightControllerKey;
|
||||
import dji.sdk.keyvalue.key.KeyTools;
|
||||
import dji.v5.common.callback.CommonCallbacks;
|
||||
import dji.v5.common.error.IDJIError;
|
||||
import dji.v5.manager.KeyManager;
|
||||
|
||||
/**
|
||||
* 2帧一处理：0处理-1跳过-2处理-3跳过
|
||||
* 先预处理检测，失败再回退原图检测
|
||||
*/
|
||||
public class Aprondown {
|
||||
|
||||
private static final String TAG = "Aprondown";
|
||||
|
||||
// ========== 原有参数 ==========
|
||||
private static double LENS_OFFSET_X = 0;
|
||||
private static double LENS_OFFSET_Y = 0;
|
||||
private static final int CENTER_ERR_MAX = 30;
|
||||
private static final float SLOW_LAND_SPEED = -0.2f;
|
||||
private static final float SLOW_SUPER_SPEED = -0.1f;
|
||||
private static final int PIXEL_TRIGGER = 360;
|
||||
private static final int TRIGGER_FRAME_THRESHOLD = 2;
|
||||
|
||||
// ========== 原有状态 ==========
|
||||
private boolean isTriggerSuccess;
|
||||
private boolean arucoNotFoundTag = false;
|
||||
private boolean isStartAruco = false;
|
||||
|
||||
public boolean isStartAruco() {
|
||||
return isStartAruco;
|
||||
}
|
||||
|
||||
public void setStartAruco(boolean startAruco) {
|
||||
isStartAruco = startAruco;
|
||||
}
|
||||
|
||||
ScheduledExecutorService executor = Executors.newScheduledThreadPool(1);
|
||||
Future<?> lastFuture = null;
|
||||
private String TAG_LOG = getClass().getSimpleName();
|
||||
Double resultYaw = 0.0;
|
||||
private boolean triggerToAlternateLandingPoint;
|
||||
long startTime;
|
||||
long endTime;
|
||||
private int sigleMarkerDetectFailsTimes;
|
||||
private boolean dropTimesTag;
|
||||
private int dropTimes = 0;
|
||||
|
||||
public int getDropTimes() {
|
||||
return dropTimes;
|
||||
}
|
||||
|
||||
public void setDropTimes(int dropTimes) {
|
||||
this.dropTimes = dropTimes;
|
||||
}
|
||||
|
||||
private boolean isDoublePayload;
|
||||
private int trycount = 0;
|
||||
|
||||
public PIDControl pidControlX = null;
|
||||
public PIDControl pidControlY = null;
|
||||
public int checkThrowingErrorsTimes;
|
||||
private int consecutiveTriggerCount = 0;
|
||||
|
||||
// ========== 【新增】2帧一处理计数器 ==========
|
||||
private int frameCounter = 0;
|
||||
|
||||
private long lastHeightCheckTime = 0;
|
||||
private double lastUltrasonicHeight = 0;
|
||||
private static final long HEIGHT_STABLE_THRESHOLD = 45000; // 45秒
|
||||
private static final double HEIGHT_CHANGE_THRESHOLD = 1; // 10厘米
|
||||
private boolean isHeightStableMonitoring = false;
|
||||
|
||||
// ========== 原有方法 ==========
|
||||
public int getCheckThrowingErrorsTimes() { return checkThrowingErrorsTimes; }
|
||||
public void setCheckThrowingErrorsTimes(int v) { checkThrowingErrorsTimes = v; }
|
||||
public boolean isTriggerSuccess() { return isTriggerSuccess; }
|
||||
public void setTriggerSuccess(boolean v) { isTriggerSuccess = v; }
|
||||
public boolean isDoublePayload() { return isDoublePayload; }
|
||||
public void setDoublePayload(boolean v) { isDoublePayload = v; }
|
||||
public boolean isStartFastStick() { return startFastStick; }
|
||||
public void setStartFastStick(boolean v) { startFastStick = v; }
|
||||
public boolean isCanLanding() { return canLanding; }
|
||||
public void setCanLanding(boolean v) {
|
||||
startTime = 0;
|
||||
endTime = 0;
|
||||
canLanding = v;
|
||||
}
|
||||
|
||||
private Aprondown() {}
|
||||
private static class OpenCVHelperHolder {
|
||||
private static final com.aros.apron.mix.Aprondown INSTANCE = new com.aros.apron.mix.Aprondown();
|
||||
static { INSTANCE.init(); }
|
||||
}
|
||||
public static com.aros.apron.mix.Aprondown getInstance() { return com.aros.apron.mix.Aprondown.OpenCVHelperHolder.INSTANCE; }
|
||||
|
||||
public void init() {
|
||||
pidControlX = new PIDControl(0.65f, 0.02f, 0.2f, 0.05f, 2.5f, 0.1f);
|
||||
pidControlY = new PIDControl(0.65f, 0.02f, 0.2f, 0.05f, 2.5f, 0.1f);
|
||||
pidControlX.reset();
|
||||
pidControlY.reset();
|
||||
}
|
||||
|
||||
public boolean startFastStick;
|
||||
public boolean canLanding;
|
||||
|
||||
// ========== 【核心修改】主入口：2帧一处理 + 预处理回退 ==========
|
||||
public void detectArucoTags(int height, int width, byte[] data, Dictionary dictionary) {
|
||||
isTriggerSuccess = true;
|
||||
Movement.getInstance().setVirtualStickEnableReason(2);
|
||||
|
||||
// 原有过滤
|
||||
if (isStartAruco || startFastStick) {
|
||||
LogUtil.log(TAG_LOG, "过滤:" + isStartAruco + startFastStick);
|
||||
if (!isStartAruco && startFastStick) {
|
||||
checkThrowingErrorsTimes++;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// ========== 【关键】2帧一处理：0处理-1跳过-2处理-3跳过 ==========
|
||||
int currentFrame = frameCounter++;
|
||||
boolean shouldProcess = (currentFrame % 2 == 0); // 偶数帧处理，奇数帧跳过
|
||||
|
||||
if (!shouldProcess) {
|
||||
// 【关键】跳过的帧：啥也不干，直接return，让飞机自己飘
|
||||
LogUtil.log(TAG_LOG, "【跳过帧】" + currentFrame + " 让飞机自稳");
|
||||
return;
|
||||
}
|
||||
|
||||
LogUtil.log(TAG_LOG, "【处理帧】" + currentFrame + " 执行修正");
|
||||
|
||||
isStartAruco = true;
|
||||
|
||||
if (lastFuture != null && !lastFuture.isDone()) {
|
||||
LogUtil.log(TAG_LOG, "break---");
|
||||
lastFuture.cancel(true);
|
||||
}
|
||||
|
||||
// 动态偏移（原有）
|
||||
int ultHeight = Movement.getInstance().getUltrasonicHeight();
|
||||
if (ultHeight >= 30) { LENS_OFFSET_X = 0; LENS_OFFSET_Y = 0; }
|
||||
else if (ultHeight >= 20) { LENS_OFFSET_X = 0; LENS_OFFSET_Y = 0; }
|
||||
else if (ultHeight >= 10) { LENS_OFFSET_X = 20; LENS_OFFSET_Y = 10; }
|
||||
else if (ultHeight >= 5) { LENS_OFFSET_X = 30; LENS_OFFSET_Y = 20; }
|
||||
else { LENS_OFFSET_X = 55; LENS_OFFSET_Y = 35; }
|
||||
|
||||
// ========== 【核心逻辑】先预处理检测，失败回退原图 ==========
|
||||
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_BGR2GRAY);
|
||||
|
||||
MatOfInt ids = new MatOfInt();
|
||||
List<Mat> corners = new ArrayList<>();
|
||||
Mat corner6 = new Mat();
|
||||
|
||||
DetectorParameters parameters = new DetectorParameters();
|
||||
ArucoDetector detector = new ArucoDetector(dictionary, parameters);
|
||||
|
||||
// ========== 【关键修改】第1次：预处理图检测 ==========
|
||||
Mat processedMat = fixedPreprocess(grayImgMat);
|
||||
detector.detectMarkers(processedMat, corners, ids);
|
||||
LogUtil.log(TAG_LOG, "预处理图检测: " + (ids.empty() ? "未检出" : "成功，数量=" + corners.size()));
|
||||
|
||||
// ========== 【关键修改】第2次：如果失败，回退原图检测 ==========
|
||||
if (ids.empty()) {
|
||||
LogUtil.log(TAG_LOG, "预处理失败，回退原图检测");
|
||||
corners.clear(); // 清空之前的结果
|
||||
detector.detectMarkers(grayImgMat, corners, ids);
|
||||
LogUtil.log(TAG_LOG, "原图检测: " + (ids.empty() ? "仍未检出" : "成功，数量=" + corners.size()));
|
||||
}
|
||||
|
||||
// 释放预处理图（必须！避免内存泄漏）
|
||||
processedMat.release();
|
||||
|
||||
// 只保留6号码
|
||||
ids = keepOnly6(ids, corners);
|
||||
|
||||
boolean marker6Found = false;
|
||||
|
||||
int ultrasonicHeight = Movement.getInstance().getUltrasonicHeight();
|
||||
|
||||
// ========== 【新增】高度稳定性监测 ==========
|
||||
if (ultrasonicHeight > 0) {
|
||||
if (!isHeightStableMonitoring) {
|
||||
// 开始监测
|
||||
isHeightStableMonitoring = true;
|
||||
lastHeightCheckTime = System.currentTimeMillis();
|
||||
lastUltrasonicHeight = ultrasonicHeight;
|
||||
LogUtil.log(TAG_LOG, "fpv开始监测高度稳定性，基准高度: " + ultrasonicHeight + " 分米");
|
||||
} else {
|
||||
// 检查高度变化
|
||||
long currentTime = System.currentTimeMillis();
|
||||
double heightChange = Math.abs(ultrasonicHeight - lastUltrasonicHeight);
|
||||
|
||||
if (heightChange > HEIGHT_CHANGE_THRESHOLD) {
|
||||
// 高度有明显变化，重置计时器
|
||||
lastHeightCheckTime = currentTime;
|
||||
lastUltrasonicHeight = ultrasonicHeight;
|
||||
//LogUtil.log(TAG_LOG, "高度变化: " + heightChange + " 分米，重置计时器");
|
||||
} else if (currentTime - lastHeightCheckTime > HEIGHT_STABLE_THRESHOLD) {
|
||||
// 45秒内高度没有变动，执行拉高切换
|
||||
LogUtil.log(TAG_LOG, "45秒内高度未变动，执行拉高切换到云台");
|
||||
|
||||
Apronmixvalue.getInstance().inchangeTrytimes();
|
||||
if(Apronmixvalue.getInstance().getTrytimes()>5){
|
||||
AlternateLandingManager.getInstance().startTaskProcess(null);
|
||||
Movement.getInstance().setAlternate(true);
|
||||
return;
|
||||
}
|
||||
Apronmixvalue.getInstance().switchthemmodeGime();
|
||||
Apronmixvalue.getInstance().pullup();
|
||||
|
||||
// 重置监测状态
|
||||
isHeightStableMonitoring = false;
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
if (!ids.empty()) {
|
||||
trycount = 0;
|
||||
arucoNotFoundTag = false;
|
||||
int[] idArray = ids.toArray();
|
||||
|
||||
if (idArray.length == 1 && idArray[0] == 6) {
|
||||
marker6Found = true;
|
||||
corner6 = corners.get(0);
|
||||
Point[] points = new Point[4];
|
||||
for (int i = 0; i < 4; i++) {
|
||||
double[] p = corner6.get(0, i);
|
||||
points[i] = new Point(p[0], p[1]);
|
||||
}
|
||||
|
||||
double pixelWidth = calculateDistance(points[0], points[1]);
|
||||
|
||||
double centerX = (points[0].x + points[1].x + points[2].x + points[3].x) / 4.0 + LENS_OFFSET_X;
|
||||
double errX = Math.abs(centerX - rgbMat.width() / 2.0);
|
||||
double centerY = (points[0].y + points[1].y + points[2].y + points[3].y) / 4.0 + LENS_OFFSET_Y;
|
||||
double errY = Math.abs(centerY - rgbMat.height() / 2.0);
|
||||
|
||||
LogUtil.log(TAG_LOG, "像素" + (int) pixelWidth +
|
||||
" errX=" + (int) errX + " errY=" + (int) errY + " (含偏移" + LENS_OFFSET_X + ")");
|
||||
|
||||
// 速降判断（原有）
|
||||
if (!startFastStick) {
|
||||
if (pixelWidth >= PIXEL_TRIGGER &&
|
||||
ultrasonicHeight <= 4 &&
|
||||
errX < CENTER_ERR_MAX &&
|
||||
errY < CENTER_ERR_MAX) {
|
||||
|
||||
consecutiveTriggerCount++;
|
||||
LogUtil.log(TAG_LOG, "速降条件满足，累计帧数: " + consecutiveTriggerCount);
|
||||
|
||||
if (consecutiveTriggerCount >= TRIGGER_FRAME_THRESHOLD) {
|
||||
startFastStick = true;
|
||||
consecutiveTriggerCount = 0;
|
||||
|
||||
LogUtil.log(TAG_LOG, "【触发速降】连续满足" + TRIGGER_FRAME_THRESHOLD +
|
||||
"帧，pixel=" + (int) pixelWidth + " errX=" + (int) errX);
|
||||
|
||||
handler.postDelayed(new Runnable() {
|
||||
@Override
|
||||
public void run() {
|
||||
handler.post(runnable);
|
||||
LogUtil.log(TAG_LOG, "延迟结束，开始速降");
|
||||
}
|
||||
}, 300);
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
if (consecutiveTriggerCount > 0) {
|
||||
LogUtil.log(TAG_LOG, "速降条件中断，重置累计帧数（原因：像素=" + (int)pixelWidth +
|
||||
" 超声=" + ultrasonicHeight + " errX=" + (int)errX + " errY=" + (int)errY + ")");
|
||||
consecutiveTriggerCount = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 原有修正逻辑
|
||||
if (marker6Found) {
|
||||
moveOnArucoDetected(new ArucoMarker(1, corner6, 0.24f), rgbMat.width(), rgbMat.height());
|
||||
}
|
||||
dropTimesTag = true;
|
||||
|
||||
} else {
|
||||
// 原有丢失处理
|
||||
LogUtil.log(TAG_LOG, "找不到了二维码");
|
||||
if (!arucoNotFoundTag) {
|
||||
startTime = System.currentTimeMillis();
|
||||
arucoNotFoundTag = true;
|
||||
}
|
||||
endTime = System.currentTimeMillis();
|
||||
long lostDuration = endTime - startTime;
|
||||
|
||||
if (lostDuration > 1000 && lostDuration <= 12000) {
|
||||
if (Movement.getInstance().getUltrasonicHeight() <= 20) {
|
||||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 3f);
|
||||
if (dropTimes > 1) {
|
||||
LogUtil.log(TAG_LOG, "超过复降限制,去备降点");
|
||||
//切换
|
||||
|
||||
Apronmixvalue.getInstance().inchangeTrytimes();
|
||||
if(Apronmixvalue.getInstance().getTrytimes()>5){
|
||||
AlternateLandingManager.getInstance().startTaskProcess(null);
|
||||
Movement.getInstance().setAlternate(true);
|
||||
return;
|
||||
}
|
||||
Apronmixvalue.getInstance().switchthemmodeGime();
|
||||
Apronmixvalue.getInstance().pullup();
|
||||
|
||||
return;
|
||||
}
|
||||
if (dropTimesTag) {
|
||||
dropTimesTag = false;
|
||||
dropTimes++;
|
||||
LogUtil.log(TAG_LOG, "复降第:" + dropTimes + "次");
|
||||
}
|
||||
} else {
|
||||
LogUtil.log(TAG_LOG, "执行位移");
|
||||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.3f);
|
||||
}
|
||||
} else if (lostDuration > 8000) {
|
||||
LogUtil.log(TAG_LOG, "判定未识别到二维码,飞往备降点");
|
||||
|
||||
Apronmixvalue.getInstance().inchangeTrytimes();
|
||||
if(Apronmixvalue.getInstance().getTrytimes()>5){
|
||||
AlternateLandingManager.getInstance().startTaskProcess(null);
|
||||
Movement.getInstance().setAlternate(true);
|
||||
return;
|
||||
}
|
||||
Apronmixvalue.getInstance().switchthemmodeGime();
|
||||
Apronmixvalue.getInstance().pullup();
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// 释放资源
|
||||
grayImgMat.release();
|
||||
rgbMat.release();
|
||||
yuvMat.release();
|
||||
ids.release();
|
||||
|
||||
if (!corners.isEmpty()) {
|
||||
for (Mat c : corners) {
|
||||
if (c != null) c.release();
|
||||
}
|
||||
}
|
||||
isStartAruco = false;
|
||||
|
||||
} catch (Exception e) {
|
||||
LogUtil.log(TAG_LOG, "Exception" + e);
|
||||
isStartAruco = false;
|
||||
}
|
||||
}
|
||||
}, 0, TimeUnit.MILLISECONDS);
|
||||
}
|
||||
// ========== 以下全部原有方法，一点不改 ==========
|
||||
private static MatOfInt keepOnly6(MatOfInt ids, List<Mat> corners) {
|
||||
if (ids.empty()) return ids;
|
||||
int[] idArr = ids.toArray();
|
||||
List<Integer> keepIdx = new ArrayList<>();
|
||||
for (int i = 0; i < idArr.length; i++) if (idArr[i] == 6) keepIdx.add(i);
|
||||
|
||||
List<Mat> tmpCorners = new ArrayList<>(keepIdx.size());
|
||||
for (int i : keepIdx) tmpCorners.add(corners.get(i));
|
||||
|
||||
corners.clear();
|
||||
ids.release();
|
||||
int[] newIds = new int[keepIdx.size()];
|
||||
for (int j = 0; j < keepIdx.size(); j++) newIds[j] = 6;
|
||||
corners.addAll(tmpCorners);
|
||||
return new MatOfInt(newIds);
|
||||
}
|
||||
|
||||
private Mat fixedPreprocess(Mat src) {
|
||||
Mat result = new Mat();
|
||||
try {
|
||||
// 【新增】判断亮度，区分白天/夜景
|
||||
Scalar meanValue = Core.mean(src);
|
||||
double meanBrightness = meanValue.val[0];
|
||||
|
||||
if (meanBrightness < 80) { // 夜景模式（FPV晚上通常<80）
|
||||
LogUtil.log(TAG_LOG, "【夜景预处理】亮度=" + (int)meanBrightness);
|
||||
|
||||
// Step 1: 中值滤波（去椒盐噪点，比双边滤波更适合夜景）
|
||||
Mat denoised = new Mat();
|
||||
Imgproc.medianBlur(src, denoised, 5); // 5x5，去噪且保边
|
||||
|
||||
// Step 2: 直方图均衡化（全局对比度拉伸，把黑灰白拉开）
|
||||
Mat equalized = new Mat();
|
||||
Imgproc.equalizeHist(denoised, equalized);
|
||||
|
||||
// Step 3: 轻度CLAHE（局部微调，避免过曝）
|
||||
Mat clahe = new Mat();
|
||||
Imgproc.createCLAHE(2.0, new Size(8, 8)).apply(equalized, clahe); // 2.0比白天4.0保守
|
||||
|
||||
// Step 4: 夜景下降低锐化强度（避免放大残余噪点）
|
||||
Mat blurred = new Mat();
|
||||
Imgproc.GaussianBlur(clahe, blurred, new Size(0, 0), 2.0);
|
||||
Mat detail = new Mat();
|
||||
Core.subtract(clahe, blurred, detail);
|
||||
|
||||
double amount = 0.6; // 【关键】夜景降到0.6，白天1.2会放大噪点
|
||||
Core.multiply(detail, new Scalar(amount), detail);
|
||||
Core.add(clahe, detail, result);
|
||||
|
||||
// 限制范围
|
||||
Core.min(result, new Scalar(255), result);
|
||||
Core.max(result, new Scalar(0), result);
|
||||
|
||||
// 释放
|
||||
denoised.release();
|
||||
equalized.release();
|
||||
blurred.release();
|
||||
detail.release();
|
||||
clahe.release();
|
||||
|
||||
LogUtil.log(TAG_LOG, "夜景处理完成：中值滤波+均衡化+弱锐化(amount=" + amount + ")");
|
||||
return result;
|
||||
}
|
||||
|
||||
// 【原逻辑】白天模式（亮度>=80）
|
||||
LogUtil.log(TAG_LOG, "【白天预处理】亮度=" + (int)meanBrightness);
|
||||
|
||||
// Step 1: 双边滤波（保边缘降噪）
|
||||
Mat filtered = new Mat();
|
||||
Imgproc.bilateralFilter(src, filtered, 9, 75, 75);
|
||||
|
||||
// Step 2: CLAHE 局部对比度增强
|
||||
Mat clahe = new Mat();
|
||||
Imgproc.createCLAHE(4.0, new Size(8, 8)).apply(filtered, clahe);
|
||||
filtered.release();
|
||||
|
||||
// Step 3: Unsharp Mask（反锐化掩膜）
|
||||
Mat blurred = new Mat();
|
||||
Imgproc.GaussianBlur(clahe, blurred, new Size(0, 0), 2.0);
|
||||
|
||||
Mat detail = new Mat();
|
||||
Core.subtract(clahe, blurred, detail);
|
||||
|
||||
double amount = 1.2; // 白天可以用1.2
|
||||
Core.multiply(detail, new Scalar(amount), detail);
|
||||
Core.add(clahe, detail, result);
|
||||
|
||||
Core.min(result, new Scalar(255), result);
|
||||
Core.max(result, new Scalar(0), result);
|
||||
|
||||
blurred.release();
|
||||
detail.release();
|
||||
clahe.release();
|
||||
|
||||
LogUtil.log(TAG_LOG, "白天处理完成：双边滤波+CLAHE+UnsharpMask(amount=" + amount + ")");
|
||||
return result;
|
||||
|
||||
} catch (Exception e) {
|
||||
LogUtil.log(TAG_LOG, "预处理失败: " + e.getMessage());
|
||||
src.copyTo(result);
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
public void setDetectedBigMarkers() {
|
||||
startFastStick = false;
|
||||
isStartAruco = false;
|
||||
consecutiveTriggerCount = 0;
|
||||
frameCounter = 0; // 【新增】重置帧计数
|
||||
}
|
||||
|
||||
|
||||
private double calculateYawErrorFromCorners(Point[] pts) {
|
||||
double dxTop = pts[1].x - pts[0].x;
|
||||
double dyTop = pts[1].y - pts[0].y;
|
||||
double angleTop = Math.toDegrees(Math.atan2(dyTop, dxTop));
|
||||
double dxBottom = pts[2].x - pts[3].x;
|
||||
double dyBottom = pts[2].y - pts[3].y;
|
||||
double angleBottom = Math.toDegrees(Math.atan2(dyBottom, dxBottom));
|
||||
double yawError = (angleTop + angleBottom) / 2.0;
|
||||
while (yawError > 180) yawError -= 360;
|
||||
while (yawError < -180) yawError += 360;
|
||||
return yawError;
|
||||
}
|
||||
|
||||
private void moveOnArucoDetected(ArucoMarker marker, int imageWidth, int imageHeight) {
|
||||
Mat corner6 = marker.getConner();
|
||||
Point[] pts = new Point[4];
|
||||
for (int i = 0; i < 4; i++) {
|
||||
double[] p = corner6.get(0, i);
|
||||
pts[i] = new Point(p[0], p[1]);
|
||||
}
|
||||
|
||||
double cx = (pts[0].x + pts[1].x + pts[2].x + pts[3].x) / 4.0 + LENS_OFFSET_X;
|
||||
double cy = (pts[0].y + pts[1].y + pts[2].y + pts[3].y) / 4.0 + LENS_OFFSET_Y;
|
||||
Point screenCenter = new Point(imageWidth / 2.0, imageHeight / 2.0);
|
||||
|
||||
double errX = (cx - screenCenter.x);
|
||||
double errY = (cy - screenCenter.y);
|
||||
int currentHeight = Movement.getInstance().getUltrasonicHeight();
|
||||
|
||||
double scaleFactor;
|
||||
if (currentHeight >= 50) {
|
||||
scaleFactor = 300.0; // 50m: 像素3, 大力修正
|
||||
} else if (currentHeight >= 30) {
|
||||
scaleFactor = 500.0; // 30m: 像素5
|
||||
} else if (currentHeight >= 20) {
|
||||
scaleFactor = 600.0; // 20m: 像素8
|
||||
} else if (currentHeight >= 10) {
|
||||
scaleFactor = 700.0; // 10m: 像素16
|
||||
} else {
|
||||
scaleFactor = 900.0; // 1m: 像素159, 温柔修正
|
||||
}
|
||||
|
||||
pidControlX.setInputFilterAll((float) (errX / scaleFactor));
|
||||
pidControlY.setInputFilterAll((float) (-errY / scaleFactor));
|
||||
|
||||
float rawVx = pidControlX.get_pid();
|
||||
float rawVy = pidControlY.get_pid();
|
||||
float yawRate = 0f;
|
||||
|
||||
float vx = (float) Math.max(-0.2, Math.min(0.2, rawVx));
|
||||
float vy = (float) Math.max(-0.2, Math.min(0.2, rawVy));
|
||||
|
||||
double pixelWidth = Math.sqrt(Math.pow(pts[1].x - pts[0].x, 2) +
|
||||
Math.pow(pts[1].y - pts[0].y, 2));
|
||||
if(Apronmixvalue.getInstance().isIsaglinetrue()==false){
|
||||
if (currentHeight >= 10 && currentHeight <= 55) {
|
||||
double yawError = calculateYawErrorFromCorners(pts);
|
||||
double absError = Math.abs(yawError);
|
||||
|
||||
if (absError > 10.0) {
|
||||
float targetRate;
|
||||
if (absError > 100.0) targetRate = 50.0f;
|
||||
else if (absError > 50.0) targetRate = 30.0f;
|
||||
else if (absError > 30.0) targetRate = 20.0f;
|
||||
else targetRate = 10.0f;
|
||||
yawRate = yawError > 0 ? targetRate : -targetRate;
|
||||
|
||||
String speedLabel = absError > 100 ? "高速50" :
|
||||
absError > 50 ? "中速30" :
|
||||
absError > 30 ? "低速20" : "微调10";
|
||||
LogUtil.log(TAG_LOG, "机头矫正：误差=" + (int)yawError + "° 转速=" + yawRate + "°/s [" + speedLabel + "]");
|
||||
} else {
|
||||
|
||||
yawRate = 0.0f;
|
||||
|
||||
Apronmixvalue.getInstance().setIsaglinetrue(true);
|
||||
|
||||
LogUtil.log(TAG_LOG, "机头已对准：偏航误差=" + (int)yawError + "°");
|
||||
}
|
||||
} else {
|
||||
yawRate = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
float vz;
|
||||
if (currentHeight <= 4) {
|
||||
vz = 0.0f;
|
||||
if (Math.abs(errX) > 120) {
|
||||
vx = rawVx > 0 ? 0.135f : -0.135f;
|
||||
} else if (Math.abs(errX) > 80) {
|
||||
vx = rawVx > 0 ? 0.09f : -0.09f;
|
||||
} else if (Math.abs(errX) > 60) {
|
||||
vx = rawVx > 0 ? 0.07f : -0.07f;
|
||||
} else if (Math.abs(errX) > 30) {
|
||||
vx = rawVx > 0 ? 0.05f : -0.05f;
|
||||
} else {
|
||||
vx = 0f;
|
||||
}
|
||||
|
||||
|
||||
if (Math.abs(errY) > 120) {
|
||||
vy = rawVy > 0 ? 0.135f : -0.135f;
|
||||
} else if (Math.abs(errY) > 80) {
|
||||
vy = rawVy > 0 ? 0.09f : -0.09f;
|
||||
} else if (Math.abs(errY) > 60) {
|
||||
vy = rawVy > 0 ? 0.07f : -0.07f;
|
||||
} else if (Math.abs(errY) > 30) {
|
||||
vy = rawVy > 0 ? 0.05f : -0.05f;
|
||||
} else {
|
||||
vy = 0f; // 【修正】
|
||||
}
|
||||
|
||||
} else if (currentHeight <= 8) {
|
||||
vz = SLOW_SUPER_SPEED;
|
||||
} else {
|
||||
vz = SLOW_LAND_SPEED;
|
||||
}
|
||||
|
||||
DroneHelper.getInstance().moveVxVyYawrateHeight(vx, vy, yawRate, vz);
|
||||
|
||||
LogUtil.log(TAG_LOG, "vx" + vx + "vy" + vy + " errX=" + (int) errX + " errY=" + (int) errY +
|
||||
" pixelW=" + (int) pixelWidth + " vz=" + vz + " ult=" + currentHeight + " yaw=" + yawRate);
|
||||
}
|
||||
|
||||
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);
|
||||
} else {
|
||||
performNextStep();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
private void performOperation() {
|
||||
LogUtil.log(TAG_LOG, "快速下拉中..." + handlerCallbackCount);
|
||||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -5);
|
||||
handlerCallbackCount++;
|
||||
}
|
||||
|
||||
private void performNextStep() {
|
||||
canLanding = true;
|
||||
handlerCallbackCount = 0;
|
||||
dropTimes = 0;
|
||||
handler.removeCallbacks(runnable);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,920 @@
|
|||
package com.aros.apron.mix;
|
||||
|
||||
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.Movement;
|
||||
import com.aros.apron.manager.AlternateLandingManager;
|
||||
import com.aros.apron.tools.DroneHelper;
|
||||
import com.aros.apron.tools.LogUtil;
|
||||
import com.aros.apron.tools.PIDControl;
|
||||
|
||||
import org.opencv.core.CvType;
|
||||
import org.opencv.core.Mat;
|
||||
import org.opencv.core.MatOfInt;
|
||||
import org.opencv.core.Point;
|
||||
import org.opencv.imgproc.Imgproc;
|
||||
import org.opencv.objdetect.ArucoDetector;
|
||||
import org.opencv.objdetect.DetectorParameters;
|
||||
import org.opencv.objdetect.Dictionary;
|
||||
|
||||
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;
|
||||
|
||||
/**
|
||||
* 多二维码融合版本（V2.0）：
|
||||
* 1. 【关键修改】先用6号二维码进行机头旋转对准，转到位(isYawAligned=true)才进入后续阶段
|
||||
* 2. 旋转阶段(vz=0)不下降，丢失时悬停等待
|
||||
* 3. 【禁用】5号二维码逻辑已注释掉
|
||||
* 4. 旋转完成后：识别1-4号/小码进行水平对准+下降
|
||||
*/
|
||||
public class Aprongim {
|
||||
|
||||
private static final String TAG = "Aprongim";
|
||||
|
||||
// ========== 原有参数（参数别改） ==========
|
||||
private static double LENS_OFFSET_X = 0;
|
||||
private static double LENS_OFFSET_Y = 0;
|
||||
private static final int CENTER_ERR_MAX = 50;
|
||||
private static final float SLOW_LAND_SPEED = -0.3f;
|
||||
private static final float SLOW_SUPER_SPEED = -0.15f;
|
||||
private static final int PIXEL_TRIGGER = 360;
|
||||
private static final int TRIGGER_FRAME_THRESHOLD = 2;
|
||||
|
||||
// ========== 原有状态 ==========
|
||||
private boolean isTriggerSuccess;
|
||||
private boolean arucoNotFoundTag = false;
|
||||
private boolean isStartAruco = false;
|
||||
ScheduledExecutorService executor = Executors.newScheduledThreadPool(1);
|
||||
Future<?> lastFuture = null;
|
||||
private String TAG_LOG = getClass().getSimpleName();
|
||||
private boolean triggerToAlternateLandingPoint;
|
||||
long startTime;
|
||||
long endTime;
|
||||
private int sigleMarkerDetectFailsTimes;
|
||||
private boolean dropTimesTag;
|
||||
private int dropTimes = 0;
|
||||
|
||||
public int getDropTimes() {
|
||||
return dropTimes;
|
||||
}
|
||||
|
||||
public void setDropTimes(int dropTimes) {
|
||||
this.dropTimes = dropTimes;
|
||||
}
|
||||
|
||||
private boolean isDoublePayload;
|
||||
private int trycount = 0;
|
||||
|
||||
public PIDControl pidControlX = null;
|
||||
public PIDControl pidControlY = null;
|
||||
public int checkThrowingErrorsTimes;
|
||||
private int consecutiveTriggerCount = 0;
|
||||
private int frameCounter = 0;
|
||||
|
||||
// ========== 【新增】阶段控制标志 ==========
|
||||
// 偏航是否已对准（6号旋转阶段完成标志）
|
||||
private volatile boolean isYawAligned = false;
|
||||
// 当前激活的降落模式：0=旋转阶段(6号), 1=小码, 2=1-4大码, 3=6号纯下降
|
||||
private int currentLandingMode = 0;
|
||||
|
||||
// ========== 【新增】连续降落条件判断 ==========
|
||||
// 记录上一帧满足的降落条件类型：0=无, 1=单码精准降落(15/17号), 2=几何中心降落
|
||||
private int lastLandingCondition = 0;
|
||||
|
||||
// ========== 【新增】高度稳定性监测 ==========
|
||||
private long lastHeightCheckTime = 0;
|
||||
private double lastUltrasonicHeight = 0;
|
||||
private static final long HEIGHT_STABLE_THRESHOLD = 45000; // 45秒
|
||||
private static final double HEIGHT_CHANGE_THRESHOLD = 1; // 10厘米
|
||||
private boolean isHeightStableMonitoring = false;
|
||||
|
||||
// ========== 标准方法 ==========
|
||||
public int getCheckThrowingErrorsTimes() { return checkThrowingErrorsTimes; }
|
||||
public void setCheckThrowingErrorsTimes(int v) { checkThrowingErrorsTimes = v; }
|
||||
public boolean isTriggerSuccess() { return isTriggerSuccess; }
|
||||
public void setTriggerSuccess(boolean v) { isTriggerSuccess = v; }
|
||||
public boolean isDoublePayload() { return isDoublePayload; }
|
||||
public void setDoublePayload(boolean v) { isDoublePayload = v; }
|
||||
public boolean isStartFastStick() { return startFastStick; }
|
||||
public void setStartFastStick(boolean v) { startFastStick = v; }
|
||||
|
||||
private boolean setMF=false;
|
||||
public boolean isCanLanding() { return canLanding; }
|
||||
public boolean isStartAruco() {
|
||||
return isStartAruco;
|
||||
}
|
||||
|
||||
public void setStartAruco(boolean startAruco) {
|
||||
isStartAruco = startAruco;
|
||||
}
|
||||
public void setCanLanding(boolean v) {
|
||||
startTime = 0;
|
||||
endTime = 0;
|
||||
canLanding = v;
|
||||
}
|
||||
|
||||
private Aprongim() {}
|
||||
private static class OpenCVHelperHolder {
|
||||
private static final com.aros.apron.mix.Aprongim INSTANCE = new com.aros.apron.mix.Aprongim();
|
||||
static { INSTANCE.init(); }
|
||||
}
|
||||
public static com.aros.apron.mix.Aprongim getInstance() { return com.aros.apron.mix.Aprongim.OpenCVHelperHolder.INSTANCE; }
|
||||
|
||||
public void init() {
|
||||
// 参数别改：PID初始值
|
||||
pidControlX = new PIDControl(0.7f, 0.02f, 0.22f, 0.05f, 2.5f, 0.1f);
|
||||
pidControlY = new PIDControl(0.7f, 0.02f, 0.22f, 0.05f, 2.5f, 0.1f);
|
||||
pidControlX.reset();
|
||||
pidControlY.reset();
|
||||
}
|
||||
|
||||
public boolean startFastStick;
|
||||
public boolean canLanding;
|
||||
|
||||
// ========== 【新增】计算二维码像素宽度 ==========
|
||||
private double calculatePixelWidth(Mat corner) {
|
||||
try {
|
||||
Point[] pts = new Point[4];
|
||||
for (int i = 0; i < 4; i++) {
|
||||
double[] p = corner.get(0, i);
|
||||
pts[i] = new Point(p[0], p[1]);
|
||||
}
|
||||
// 计算上下两边长度的平均值作为像素宽度
|
||||
double top = Math.sqrt(Math.pow(pts[1].x - pts[0].x, 2) + Math.pow(pts[1].y - pts[0].y, 2));
|
||||
double bottom = Math.sqrt(Math.pow(pts[2].x - pts[3].x, 2) + Math.pow(pts[2].y - pts[3].y, 2));
|
||||
return (top + bottom) / 2.0;
|
||||
} catch (Exception e) {
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
// ========== 【核心】主入口：3帧一处理 ==========
|
||||
public void detectArucoTags(int height, int width, byte[] data, Dictionary dictionary) {
|
||||
isTriggerSuccess = true;
|
||||
Movement.getInstance().setVirtualStickEnableReason(2);
|
||||
|
||||
if (isStartAruco || startFastStick) {
|
||||
LogUtil.log(TAG_LOG, "过滤:" + isStartAruco + startFastStick);
|
||||
if (!isStartAruco && startFastStick) {
|
||||
checkThrowingErrorsTimes++;
|
||||
}
|
||||
return;
|
||||
}
|
||||
// 改为 3 帧一处理（10Hz，接近 DJI 建议上限）
|
||||
int currentFrame = frameCounter++;
|
||||
if (currentFrame % 3 != 0) { // 0处理，1、2跳过
|
||||
return;
|
||||
}
|
||||
if (currentFrame % 30 != 0) { // 0处理，1、2跳过
|
||||
//DroneHelper.getInstance().setGimbalPitchdown();
|
||||
}
|
||||
|
||||
isStartAruco = true;
|
||||
if (lastFuture != null && !lastFuture.isDone()) {
|
||||
lastFuture.cancel(true);
|
||||
}
|
||||
int ultHeight = Movement.getInstance().getUltrasonicHeight();
|
||||
|
||||
if(ultHeight <=10 && setMF==false){
|
||||
//切换mf对焦模式
|
||||
setMF=true;
|
||||
}
|
||||
if(isDoublePayload){
|
||||
if (ultHeight <=5)
|
||||
{
|
||||
LENS_OFFSET_X=-125;
|
||||
LENS_OFFSET_Y=120;
|
||||
}else if(ultHeight<10){
|
||||
LENS_OFFSET_X=-85;
|
||||
LENS_OFFSET_Y=80;
|
||||
} else if (ultHeight<20) {
|
||||
LENS_OFFSET_X=-70;
|
||||
LENS_OFFSET_Y=60;
|
||||
}
|
||||
}else{
|
||||
if (ultHeight <=5)
|
||||
{
|
||||
LENS_OFFSET_X=120;
|
||||
LENS_OFFSET_Y = 100;
|
||||
}else if(ultHeight<10){
|
||||
LENS_OFFSET_X=80;
|
||||
LENS_OFFSET_Y = 80;
|
||||
} else if (ultHeight<20) {
|
||||
LENS_OFFSET_X=60;
|
||||
LENS_OFFSET_Y = 60;
|
||||
}
|
||||
}
|
||||
|
||||
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_BGR2GRAY);
|
||||
|
||||
MatOfInt ids = new MatOfInt();
|
||||
List<Mat> corners = new ArrayList<>();
|
||||
DetectorParameters parameters = new DetectorParameters();
|
||||
ArucoDetector detector = new ArucoDetector(dictionary, parameters);
|
||||
|
||||
detector.detectMarkers(grayImgMat, corners, ids);
|
||||
|
||||
int ultHeight=Movement.getInstance().getUltrasonicHeight();
|
||||
|
||||
// ========== 【新增】高度稳定性监测 ==========
|
||||
if (ultHeight > 0) {
|
||||
if (!isHeightStableMonitoring) {
|
||||
// 开始监测
|
||||
isHeightStableMonitoring = true;
|
||||
lastHeightCheckTime = System.currentTimeMillis();
|
||||
lastUltrasonicHeight = ultHeight;
|
||||
LogUtil.log(TAG_LOG, "port开始监测高度稳定性，基准高度: " + ultHeight + " 分米");
|
||||
} else {
|
||||
// 检查高度变化
|
||||
long currentTime = System.currentTimeMillis();
|
||||
double heightChange = Math.abs(ultHeight - lastUltrasonicHeight);
|
||||
|
||||
if (heightChange > HEIGHT_CHANGE_THRESHOLD) {
|
||||
// 高度有明显变化，重置计时器
|
||||
lastHeightCheckTime = currentTime;
|
||||
lastUltrasonicHeight = ultHeight;
|
||||
// LogUtil.log(TAG_LOG, "高度变化: " + heightChange + " 分米，重置计时器");
|
||||
} else if (currentTime - lastHeightCheckTime > HEIGHT_STABLE_THRESHOLD) {
|
||||
// 45秒内高度没有变动，执行拉高切换
|
||||
LogUtil.log(TAG_LOG, "45秒内高度未变动，执行拉高切换到下视觉");
|
||||
|
||||
Apronmixvalue.getInstance().inchangeTrytimes();
|
||||
|
||||
if(Apronmixvalue.getInstance().getTrytimes()>5){
|
||||
AlternateLandingManager.getInstance().startTaskProcess(null);
|
||||
Movement.getInstance().setAlternate(true);
|
||||
return;
|
||||
}
|
||||
|
||||
// 切换到下视觉降落
|
||||
Apronmixvalue.getInstance().switchthemmodefpv();
|
||||
Apronmixvalue.getInstance().pullup();
|
||||
|
||||
// 重置监测状态
|
||||
isHeightStableMonitoring = false;
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!ids.empty()) {
|
||||
trycount = 0;
|
||||
arucoNotFoundTag = false;
|
||||
|
||||
List<ArucoMarker> smallMarkers = new ArrayList<>();
|
||||
List<ArucoMarker> bigMarkers1_4 = new ArrayList<>();
|
||||
// 【禁用】5号逻辑注释掉，但保留变量避免编译错误
|
||||
// List<ArucoMarker> bigMarker5 = new ArrayList<>();
|
||||
List<ArucoMarker> bigMarker6 = new ArrayList<>();
|
||||
|
||||
int[] idArray = ids.toArray();
|
||||
List<Integer> excludeIds = isDoublePayload ?
|
||||
Arrays.asList(12, 11, 21, 14, 13, 22) :
|
||||
Arrays.asList(12, 11, 21, 20, 19, 25);
|
||||
|
||||
// 【新增】记录所有检测到的码的像素信息
|
||||
StringBuilder detectInfo = new StringBuilder();
|
||||
|
||||
for (int i = 0; i < idArray.length; i++) {
|
||||
int id = idArray[i];
|
||||
Mat corner = corners.get(i);
|
||||
double pixelW = calculatePixelWidth(corner);
|
||||
|
||||
if (id >= 11 && id <= 25 && !excludeIds.contains(id)) {
|
||||
smallMarkers.add(new ArucoMarker(id, corner, 0.03f));
|
||||
detectInfo.append(String.format("小码%d(%.0fpx) ", id, pixelW));
|
||||
} else if (id >= 1 && id <= 4) {
|
||||
bigMarkers1_4.add(new ArucoMarker(id, corner, 0.18f));
|
||||
detectInfo.append(String.format("大码%d(%.0fpx) ", id, pixelW));
|
||||
}
|
||||
// 【禁用】5号二维码逻辑已注释
|
||||
/*
|
||||
else if (id == 5) {
|
||||
bigMarker5.add(new ArucoMarker(5, corner, 0.15f));
|
||||
detectInfo.append(String.format("5号(%.0fpx) ", pixelW));
|
||||
}
|
||||
*/
|
||||
else if (id == 6) {
|
||||
bigMarker6.add(new ArucoMarker(6, corner, 0.24f));
|
||||
detectInfo.append(String.format("6号(%.0fpx) ", pixelW));
|
||||
}
|
||||
}
|
||||
// 输出检测到的所有码及其像素宽度
|
||||
if (detectInfo.length() > 0) {
|
||||
LogUtil.log(TAG_LOG, "【检测到】" + detectInfo.toString());
|
||||
}
|
||||
|
||||
isYawAligned=Apronmixvalue.getInstance().isIsaglinetrue();
|
||||
|
||||
// ========== 【关键修改】阶段0：6号旋转阶段（最高优先级） ==========
|
||||
// 只要看到6号且还没对准，就只用6号旋转，不进入其他任何阶段
|
||||
if (!bigMarker6.isEmpty() && !isYawAligned) {
|
||||
currentLandingMode = 0;
|
||||
processMarker6YawOnly(bigMarker6.get(0), rgbMat.width(), rgbMat.height(), ultHeight);
|
||||
}
|
||||
// ========== 【关键】只有旋转完成后(isYawAligned=true)，才识别其他码 ==========
|
||||
else if (isYawAligned) {
|
||||
// 优先级1：小码模式（高度<25dm且≥3个码）
|
||||
if (!smallMarkers.isEmpty() && ultHeight < 25) {
|
||||
currentLandingMode = 1;
|
||||
processSmallMarkers(smallMarkers, rgbMat.width(), rgbMat.height(), ultHeight);
|
||||
}
|
||||
// 优先级2：1-4号几何中心下降
|
||||
else if (!bigMarkers1_4.isEmpty()) {
|
||||
currentLandingMode = 2;
|
||||
processBigMarkersCenter(bigMarkers1_4, rgbMat.width(), rgbMat.height(), ultHeight);
|
||||
}
|
||||
// 优先级3：6号纯下降（旋转完成后，没看到1-4和小码时）
|
||||
else if (!bigMarker6.isEmpty()) {
|
||||
currentLandingMode = 3;
|
||||
processLandingOnly(bigMarker6.get(0), rgbMat.width(), rgbMat.height(), ultHeight);
|
||||
}
|
||||
// 旋转完成后丢失其他码，但还有6号：继续用6号下降
|
||||
else {
|
||||
handleLostMarker(ultHeight);
|
||||
}
|
||||
} else {
|
||||
LogUtil.log(TAG_LOG, "【等待6号】未检测到6号二维码，无法开始旋转对准");
|
||||
handleLostMarker(ultHeight);
|
||||
}
|
||||
dropTimesTag = true;
|
||||
} else {
|
||||
// 丢失处理（包含旋转阶段保护）
|
||||
handleLostMarker(ultHeight);
|
||||
}
|
||||
grayImgMat.release();
|
||||
rgbMat.release();
|
||||
yuvMat.release();
|
||||
ids.release();
|
||||
if (!corners.isEmpty()) {
|
||||
for (Mat c : corners) if (c != null) c.release();
|
||||
}
|
||||
isStartAruco = false;
|
||||
|
||||
} catch (Exception e) {
|
||||
LogUtil.log(TAG_LOG, "Exception: " + e);
|
||||
isStartAruco = false;
|
||||
}
|
||||
}
|
||||
}, 0, TimeUnit.MILLISECONDS);
|
||||
}
|
||||
|
||||
// ========== 【新增】6号专用旋转阶段（纯旋转，不下降） ==========
|
||||
private void processMarker6YawOnly(ArucoMarker marker, int imgWidth, int imgHeight, int ultHeight) {
|
||||
Mat corner = marker.getConner();
|
||||
Point[] pts = new Point[4];
|
||||
for (int i = 0; i < 4; i++) {
|
||||
double[] p = corner.get(0, i);
|
||||
pts[i] = new Point(p[0], p[1]);
|
||||
}
|
||||
|
||||
// 使用6号的角点计算yaw误差
|
||||
double yawError = calculateYawErrorFromCorners(pts);
|
||||
double absYaw = Math.abs(yawError);
|
||||
|
||||
// 计算6号的像素宽度用于日志
|
||||
double pixelWidth = calculatePixelWidth(corner);
|
||||
|
||||
if (absYaw < 5.0) {
|
||||
// 旋转到位，设置标志，下一帧将进入正常下降流程
|
||||
isYawAligned = true;
|
||||
|
||||
Apronmixvalue.getInstance().setIsaglinetrue(true);
|
||||
|
||||
LogUtil.log(TAG_LOG, String.format("【6号旋转到位】误差=%.1f° pixel=%.0f 进入下降阶段", yawError, pixelWidth));
|
||||
// 发送悬停指令（停止旋转）
|
||||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 0f);
|
||||
} else {
|
||||
// 未对准：计算转速（分段）
|
||||
float yawRate;
|
||||
if (absYaw > 100.0) yawRate = 50.0f;
|
||||
else if (absYaw > 50.0) yawRate = 30.0f;
|
||||
else if (absYaw > 30.0) yawRate = 20.0f;
|
||||
else if (absYaw > 20.0) yawRate = 10.0f;
|
||||
else if (absYaw > 10.0) yawRate = 5.0f;
|
||||
else if (absYaw > 5.0) yawRate = 3.0f;
|
||||
else yawRate = 0.0f;
|
||||
|
||||
// 【注意】如果实际测试还是单向转，把下一行改成：yawRate = yawError > 0 ? -yawRate : yawRate;
|
||||
yawRate = yawError > 0 ? yawRate : -yawRate;
|
||||
|
||||
LogUtil.log(TAG_LOG, String.format("【6号旋转中】误差=%.1f° 转速=%.1f pixel=%.0f ult=%d",
|
||||
yawError, yawRate, pixelWidth, ultHeight));
|
||||
|
||||
// 【关键】只旋转，不水平移动，不下降（vz=0）
|
||||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, yawRate, 0f);
|
||||
}
|
||||
}
|
||||
|
||||
private void processSmallMarkers(List<ArucoMarker> markers, int imgWidth, int imgHeight, int ultHeight) {
|
||||
// 【关键修改】必须检测到3个及以上小码才处理，否则进入丢失保护
|
||||
if (markers == null || markers.size() < 3) {
|
||||
LogUtil.log(TAG_LOG, "【小码数量不足】检测到" + (markers == null ? 0 : markers.size()) + "个，需要≥3个");
|
||||
// 数量不足时，如果有6号就用6号下降，否则悬停
|
||||
return;
|
||||
}
|
||||
|
||||
// 1. 计算几何中心和统计信息
|
||||
double sumX = 0, sumY = 0;
|
||||
int validCount = 0;
|
||||
double totalPixelWidth = 0;
|
||||
|
||||
ArucoMarker target15 = null; // 单挂目标
|
||||
ArucoMarker target17 = null; // 双挂目标
|
||||
|
||||
for (ArucoMarker marker : markers) {
|
||||
Mat corner = marker.getConner();
|
||||
totalPixelWidth += calculatePixelWidth(corner);
|
||||
|
||||
double cx = 0, cy = 0;
|
||||
for (int i = 0; i < 4; i++) {
|
||||
double[] p = corner.get(0, i);
|
||||
cx += p[0];
|
||||
cy += p[1];
|
||||
}
|
||||
cx /= 4.0;
|
||||
cy /= 4.0;
|
||||
sumX += cx;
|
||||
sumY += cy;
|
||||
validCount++;
|
||||
|
||||
int id = marker.getId();
|
||||
if (id == 15) target15 = marker;
|
||||
if (id == 17) target17 = marker;
|
||||
}
|
||||
|
||||
if (validCount == 0) return;
|
||||
|
||||
double avgPixelWidth = totalPixelWidth / validCount;
|
||||
double geoCenterX = sumX / validCount;
|
||||
double geoCenterY = sumY / validCount;
|
||||
double offsetX = geoCenterX - imgWidth / 2.0 - LENS_OFFSET_X;
|
||||
double offsetY = geoCenterY - imgHeight / 2.0 + LENS_OFFSET_Y;
|
||||
|
||||
double absX = Math.abs(offsetX);
|
||||
double absY = Math.abs(offsetY);
|
||||
double z = ultHeight / 10.0;
|
||||
|
||||
// 降落条件判断（连续帧）
|
||||
boolean isSpecialLanding = false;
|
||||
double singleErrX = 0, singleErrY = 0;
|
||||
|
||||
// 单挂模式：检查15号
|
||||
if (!isDoublePayload && target15 != null && ultHeight <= 4) {
|
||||
Mat corner = target15.getConner();
|
||||
double cx = 0, cy = 0;
|
||||
for (int i = 0; i < 4; i++) {
|
||||
double[] p = corner.get(0, i);
|
||||
cx += p[0];
|
||||
cy += p[1];
|
||||
}
|
||||
cx /= 4.0; cy /= 4.0;
|
||||
singleErrX = cx - imgWidth / 2.0 - LENS_OFFSET_X;
|
||||
singleErrY = cy - imgHeight / 2.0 + LENS_OFFSET_Y;
|
||||
|
||||
if (Math.abs(singleErrX) < 60 && Math.abs(singleErrY) < 60) {
|
||||
isSpecialLanding = true;
|
||||
offsetX = singleErrX;
|
||||
offsetY = singleErrY;
|
||||
absX = Math.abs(singleErrX);
|
||||
absY = Math.abs(singleErrY);
|
||||
}
|
||||
}
|
||||
|
||||
// 双挂模式：检查17号
|
||||
if (isDoublePayload && target17 != null && ultHeight <= 4) {
|
||||
Mat corner = target17.getConner();
|
||||
double cx = 0, cy = 0;
|
||||
for (int i = 0; i < 4; i++) {
|
||||
double[] p = corner.get(0, i);
|
||||
cx += p[0];
|
||||
cy += p[1];
|
||||
}
|
||||
cx /= 4.0; cy /= 4.0;
|
||||
singleErrX = cx - imgWidth / 2.0 - LENS_OFFSET_X;
|
||||
singleErrY = cy - imgHeight / 2.0 + LENS_OFFSET_Y;
|
||||
|
||||
if (Math.abs(singleErrX) < 60 && Math.abs(singleErrY) < 60) {
|
||||
isSpecialLanding = true;
|
||||
offsetX = singleErrX;
|
||||
offsetY = singleErrY;
|
||||
absX = Math.abs(singleErrX);
|
||||
absY = Math.abs(singleErrY);
|
||||
}
|
||||
}
|
||||
|
||||
// 连续帧判断
|
||||
int currentCondition = 0;
|
||||
if (isSpecialLanding) {
|
||||
currentCondition = 1;
|
||||
} else if (ultHeight <= 4 && absX < 60 && absY < 60 && validCount >= 8) {
|
||||
currentCondition = 2;
|
||||
}
|
||||
|
||||
if (currentCondition == 0 || currentCondition != lastLandingCondition) {
|
||||
if (consecutiveTriggerCount > 0) {
|
||||
consecutiveTriggerCount = 0;
|
||||
LogUtil.log(TAG_LOG, "【计数器清零】条件变化 last=" + lastLandingCondition + " curr=" + currentCondition);
|
||||
}
|
||||
lastLandingCondition = currentCondition;
|
||||
}
|
||||
|
||||
if (currentCondition > 0) {
|
||||
consecutiveTriggerCount++;
|
||||
if (consecutiveTriggerCount >= TRIGGER_FRAME_THRESHOLD) {
|
||||
startFastStick = true;
|
||||
consecutiveTriggerCount = 0;
|
||||
lastLandingCondition = 0;
|
||||
|
||||
LogUtil.log(TAG_LOG, currentCondition == 1 ?
|
||||
"【降落触发】单双挂精准条件满足" :
|
||||
"【降落触发】几何中心对准，码数量=" + validCount);
|
||||
|
||||
handler.postDelayed(() -> handler.post(runnable), 300);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
double outX = 0, outY = 0, outZ = 0;
|
||||
|
||||
// 分段PID控制（原有逻辑保持不变）
|
||||
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();
|
||||
}
|
||||
}
|
||||
}else if(z <= 0.7){
|
||||
pidControlX.setInputFilterAll((float)offsetX/1200);
|
||||
pidControlY.setInputFilterAll(-(float)offsetY/1200);
|
||||
double pidX = pidControlX.get_pid();
|
||||
double pidY = pidControlY.get_pid();
|
||||
|
||||
if (pidX < 0){
|
||||
outX = (pidX < -0.135) ? (absX < 120 ? 0 : -0.135) : (absX < 120 ? 0 : pidX);
|
||||
}else{
|
||||
outX = (pidX > 0.135) ? (absX < 120 ? 0 : 0.135) : (absX < 120 ? 0 : pidX);
|
||||
}
|
||||
|
||||
if (pidY < 0){
|
||||
outY = (pidY < -0.135) ? (absY < 120 ? 0 : -0.135) : (absY < 120 ? 0 : pidY);
|
||||
}else{
|
||||
outY = (pidY > 0.135) ? (absY < 120 ? 0 : 0.135) : (absY < 120 ? 0 : pidY);
|
||||
}
|
||||
outZ = (absX < 250 && absY < 250) ? -0.1 : 0;
|
||||
|
||||
}else if(z <= 1.0){
|
||||
pidControlX.setInputFilterAll((float)offsetX/1200);
|
||||
pidControlY.setInputFilterAll(-(float)offsetY/1200);
|
||||
double pidX = pidControlX.get_pid();
|
||||
double pidY = pidControlY.get_pid();
|
||||
|
||||
if (pidX < 0){
|
||||
outX = (pidX < -0.145) ? (absX < 120 ? 0 : -0.145) : (absX < 120 ? 0 : pidX);
|
||||
}else{
|
||||
outX = (pidX > 0.145) ? (absX < 120 ? 0 : 0.145) : (absX < 120 ? 0 : pidX);
|
||||
}
|
||||
|
||||
if (pidY < 0){
|
||||
outY = (pidY < -0.175) ? (absY < 120 ? 0 : -0.175) : (absY < 120 ? 0 : pidY);
|
||||
}else{
|
||||
outY = (pidY > 0.175) ? (absY < 120 ? 0 : 0.175) : (absY < 120 ? 0 : pidY);
|
||||
}
|
||||
outZ = (absX < 200 && absY < 200) ? -0.2 : 0;
|
||||
|
||||
}else if(z <= 1.5){
|
||||
pidControlX.setInputFilterAll((float)offsetX/1450);
|
||||
pidControlY.setInputFilterAll(-(float)offsetY/1450);
|
||||
double pidX = pidControlX.get_pid();
|
||||
double pidY = pidControlY.get_pid();
|
||||
|
||||
if (pidX < 0){
|
||||
outX = (pidX < -0.185) ? (absX < 120 ? 0 : -0.185) : (absX < 120 ? 0 : pidX);
|
||||
}else{
|
||||
outX = (pidX > 0.185) ? (absX < 120 ? 0 : 0.185) : (absX < 120 ? 0 : pidX);
|
||||
}
|
||||
if (pidY < 0){
|
||||
outY = (pidY < -0.185) ? (absY < 120 ? 0 : -0.185) : (absY < 120 ? 0 : pidY);
|
||||
}else{
|
||||
outY = (pidY > 0.185) ? (absY < 120 ? 0 : 0.185) : (absY < 120 ? 0 : pidY);
|
||||
}
|
||||
outZ = (absX < 180 && absY < 180) ? -0.4 : 0;
|
||||
|
||||
}else if(z <= 2.0){
|
||||
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.4 : 0;
|
||||
|
||||
}else if(z <= 3.0){
|
||||
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.4 : 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.4 : 0;
|
||||
}
|
||||
|
||||
LogUtil.log(TAG_LOG, String.format("【执行移动】小码 vx=%.3f vy=%.3f vz=%.3f pixel=%.0f ult=%d errX=%.0f errY=%.0f",
|
||||
outX, outY, outZ, avgPixelWidth, ultHeight, absX, absY));
|
||||
DroneHelper.getInstance().moveVxVyYawrateHeight((float)outX, (float)outY, 0, (float)outZ);
|
||||
}
|
||||
|
||||
// ========== 【修改】1-4号几何中心处理（旋转完成后才进入） ==========
|
||||
private void processBigMarkersCenter(List<ArucoMarker> markers, int imgWidth, int imgHeight, int ultHeight) {
|
||||
double sumX = 0, sumY = 0;
|
||||
StringBuilder markerInfo = new StringBuilder();
|
||||
|
||||
for (ArucoMarker marker : markers) {
|
||||
Mat corner = marker.getConner();
|
||||
double cx = 0, cy = 0;
|
||||
for (int i = 0; i < 4; i++) {
|
||||
double[] p = corner.get(0, i);
|
||||
cx += p[0];
|
||||
cy += p[1];
|
||||
}
|
||||
sumX += cx / 4.0;
|
||||
sumY += cy / 4.0;
|
||||
|
||||
double pixelW = calculatePixelWidth(corner);
|
||||
markerInfo.append(String.format("[ID%d:%.0fpx]", marker.getId(), pixelW));
|
||||
}
|
||||
|
||||
double geoCenterX = sumX / markers.size();
|
||||
double geoCenterY = sumY / markers.size();
|
||||
double offsetX = geoCenterX - imgWidth / 2.0;
|
||||
double offsetY = geoCenterY - imgHeight / 2.0;
|
||||
double absX = Math.abs(offsetX);
|
||||
double absY = Math.abs(offsetY);
|
||||
|
||||
double z = ultHeight / 10.0;
|
||||
double outX = 0, outY = 0, outZ = 0;
|
||||
|
||||
if (z >= 5.0) {
|
||||
pidControlX.setInputFilterAll((float)offsetX/300);
|
||||
pidControlY.setInputFilterAll(-(float)offsetY/300);
|
||||
outX = absX < 50 ? 0 : pidControlX.get_pid();
|
||||
outY = absY < 50 ? 0 : pidControlY.get_pid();
|
||||
outX = Math.max(-0.20, Math.min(0.20, outX));
|
||||
outY = Math.max(-0.20, Math.min(0.20, outY));
|
||||
outZ = (absX < 100 && absY < 100) ? -0.4 : 0;
|
||||
}else if(z>=4.0){
|
||||
pidControlX.setInputFilterAll((float)offsetX/600);
|
||||
pidControlY.setInputFilterAll(-(float)offsetY/600);
|
||||
outX = absX < 50 ? 0 : pidControlX.get_pid();
|
||||
outY = absY < 50 ? 0 : pidControlY.get_pid();
|
||||
outX = Math.max(-0.20, Math.min(0.20, outX));
|
||||
outY = Math.max(-0.20, Math.min(0.20, outY));
|
||||
outZ = (absX < 120 && absY < 120) ? -0.4 : 0;
|
||||
}else if(z>=2.0){
|
||||
pidControlX.setInputFilterAll((float)offsetX/900);
|
||||
pidControlY.setInputFilterAll(-(float)offsetY/900);
|
||||
outX = absX < 50 ? 0 : pidControlX.get_pid();
|
||||
outY = absY < 50 ? 0 : pidControlY.get_pid();
|
||||
outX = Math.max(-0.20, Math.min(0.20, outX));
|
||||
outY = Math.max(-0.20, Math.min(0.20, outY));
|
||||
outZ = (absX < 140 && absY < 140) ? -0.4 : 0;
|
||||
}else if(z>=1.0){
|
||||
pidControlX.setInputFilterAll((float)offsetX/1000);
|
||||
pidControlY.setInputFilterAll(-(float)offsetY/1000);
|
||||
outX = absX < 50 ? 0 : pidControlX.get_pid();
|
||||
outY = absY < 50 ? 0 : pidControlY.get_pid();
|
||||
outX = Math.max(-0.20, Math.min(0.20, outX));
|
||||
outY = Math.max(-0.20, Math.min(0.20, outY));
|
||||
outZ = (absX < 150 && absY < 150) ? -0.1 : 0;
|
||||
}else if(z>=0.5){
|
||||
outZ = (absX < 150 && absY < 150) ? -0.1 : 0;
|
||||
}
|
||||
|
||||
LogUtil.log(TAG_LOG, String.format("【执行移动】大码几何中心 vx=%.3f vy=%.3f vz=%.3f ult=%d %s errX=%.0f errY=%.0f",
|
||||
outX, outY, outZ, ultHeight, markerInfo.toString(), absX, absY));
|
||||
DroneHelper.getInstance().moveVxVyYawrateHeight((float)outX, (float)outY, 0f, (float)outZ);
|
||||
}
|
||||
|
||||
// ========== 【修改】6号纯下降模式（旋转完成后使用） ==========
|
||||
private void processLandingOnly(ArucoMarker marker, int imgWidth, int imgHeight, int ultHeight) {
|
||||
Mat corner = marker.getConner();
|
||||
Point[] pts = new Point[4];
|
||||
for (int i = 0; i < 4; i++) {
|
||||
double[] p = corner.get(0, i);
|
||||
pts[i] = new Point(p[0], p[1]);
|
||||
}
|
||||
|
||||
double pixelWidth = calculatePixelWidth(corner);
|
||||
double cx = 0, cy = 0;
|
||||
for (int i = 0; i < 4; i++) {
|
||||
cx += pts[i].x;
|
||||
cy += pts[i].y;
|
||||
}
|
||||
cx /= 4.0;
|
||||
cy /= 4.0;
|
||||
double offsetX = cx - imgWidth / 2.0;
|
||||
double offsetY = cy - imgHeight / 2.0;
|
||||
|
||||
float vz;
|
||||
|
||||
if (ultHeight >= 50) {
|
||||
vz = -0.7f;
|
||||
} else if(ultHeight<=20){
|
||||
vz = 0.0f;
|
||||
}else{
|
||||
vz=-0.3f;
|
||||
}
|
||||
|
||||
LogUtil.log(TAG_LOG, String.format("【执行移动】6号纯下降 vz=%.3f pixel=%.0f ult=%d errX=%.0f errY=%.0f",
|
||||
vz, pixelWidth, ultHeight, Math.abs(offsetX), Math.abs(offsetY)));
|
||||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, vz);
|
||||
}
|
||||
|
||||
private double calculateYawErrorFromCorners(Point[] pts) {
|
||||
double dxTop = pts[1].x - pts[0].x;
|
||||
double dyTop = pts[1].y - pts[0].y;
|
||||
double angleTop = Math.toDegrees(Math.atan2(dyTop, dxTop));
|
||||
double dxBottom = pts[2].x - pts[3].x;
|
||||
double dyBottom = pts[2].y - pts[3].y;
|
||||
double angleBottom = Math.toDegrees(Math.atan2(dyBottom, dxBottom));
|
||||
double yawError = (angleTop + angleBottom) / 2.0;
|
||||
while (yawError > 180) yawError -= 360;
|
||||
while (yawError < -180) yawError += 360;
|
||||
return yawError;
|
||||
}
|
||||
|
||||
// ========== 【修改】handleLostMarker 增加旋转阶段保护 ==========
|
||||
private void handleLostMarker(int ultHeight) {
|
||||
if (!arucoNotFoundTag) {
|
||||
startTime = System.currentTimeMillis();
|
||||
arucoNotFoundTag = true;
|
||||
}
|
||||
endTime = System.currentTimeMillis();
|
||||
long lostDuration = endTime - startTime;
|
||||
|
||||
// 【关键新增】旋转阶段保护：还没对准(isYawAligned=false)时，悬停等待，不下降
|
||||
if (!isYawAligned) {
|
||||
LogUtil.log(TAG_LOG, "【旋转阶段丢失】未对准6号，悬停等待 vz=0 lostDuration=" + lostDuration);
|
||||
// 悬停（不上升也不下降）
|
||||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 0f);
|
||||
// 10秒还没看到6号，去备降点
|
||||
if (lostDuration > 10000) {
|
||||
LogUtil.log(TAG_LOG, "【旋转超时】10秒未检测到6号，飞往备降点");
|
||||
//切换（切换++）
|
||||
|
||||
Apronmixvalue.getInstance().inchangeTrytimes();
|
||||
if(Apronmixvalue.getInstance().getTrytimes()>5){
|
||||
AlternateLandingManager.getInstance().startTaskProcess(null);
|
||||
Movement.getInstance().setAlternate(true);
|
||||
return;
|
||||
}
|
||||
|
||||
Apronmixvalue.getInstance().switchthemmodefpv();
|
||||
Apronmixvalue.getInstance().pullup();
|
||||
|
||||
|
||||
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if(isYawAligned){
|
||||
// 原有逻辑（旋转完成后的丢失处理）
|
||||
if (lostDuration > 1000 && lostDuration <= 12000) {
|
||||
if (ultHeight <= 20) {
|
||||
// 低空丢失：上升
|
||||
LogUtil.log(TAG_LOG, "【执行移动】丢失上升 vz=3.0");
|
||||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 3f);
|
||||
if (dropTimes > 1) {
|
||||
LogUtil.log(TAG_LOG, "超过复降限制,去备降点");
|
||||
|
||||
Apronmixvalue.getInstance().inchangeTrytimes();
|
||||
|
||||
if(Apronmixvalue.getInstance().getTrytimes()>5){
|
||||
AlternateLandingManager.getInstance().startTaskProcess(null);
|
||||
Movement.getInstance().setAlternate(true);
|
||||
return;
|
||||
}
|
||||
Apronmixvalue.getInstance().switchthemmodefpv();
|
||||
Apronmixvalue.getInstance().pullup();
|
||||
|
||||
return;
|
||||
}
|
||||
if (dropTimesTag) {
|
||||
dropTimesTag = false;
|
||||
dropTimes++;
|
||||
LogUtil.log(TAG_LOG, "复降第:" + dropTimes + "次");
|
||||
}
|
||||
} else {
|
||||
// 高空丢失：下降寻找
|
||||
LogUtil.log(TAG_LOG, "【执行移动】丢失下降 vz=-0.3");
|
||||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.3f);
|
||||
}
|
||||
} else if (lostDuration > 8000) {
|
||||
LogUtil.log(TAG_LOG, "判定未识别到二维码,飞往备降点");
|
||||
Apronmixvalue.getInstance().inchangeTrytimes();
|
||||
|
||||
if(Apronmixvalue.getInstance().getTrytimes()>5){
|
||||
AlternateLandingManager.getInstance().startTaskProcess(null);
|
||||
Movement.getInstance().setAlternate(true);
|
||||
return;
|
||||
}
|
||||
Apronmixvalue.getInstance().switchthemmodefpv();
|
||||
Apronmixvalue.getInstance().pullup();
|
||||
|
||||
}
|
||||
}
|
||||
// if(lostDuration > 8000) {
|
||||
// LogUtil.log(TAG_LOG, "判定未识别到二维码,飞往备降点");
|
||||
// AlternateLandingManager.getInstance().startTaskProcess(null);
|
||||
// Movement.getInstance().setAlternate(true);
|
||||
// }
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
public void setDetectedBigMarkers() {
|
||||
startFastStick = false;
|
||||
isStartAruco = false;
|
||||
consecutiveTriggerCount = 0;
|
||||
frameCounter = 0;
|
||||
// 【关键】重置旋转标志，下次降落重新用6号旋转
|
||||
isYawAligned = false;
|
||||
currentLandingMode = 0;
|
||||
lastLandingCondition = 0;
|
||||
}
|
||||
|
||||
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);
|
||||
} else {
|
||||
performNextStep();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
private void performOperation() {
|
||||
LogUtil.log(TAG_LOG, String.format("【执行移动】速降 vz=-4 count=%d/20", handlerCallbackCount));
|
||||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -5);
|
||||
handlerCallbackCount++;
|
||||
}
|
||||
|
||||
private void performNextStep() {
|
||||
canLanding = true;
|
||||
handlerCallbackCount = 0;
|
||||
dropTimes = 0;
|
||||
handler.removeCallbacks(runnable);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
package com.aros.apron.mix;
|
||||
|
||||
import android.os.Handler;
|
||||
import android.os.Looper;
|
||||
|
||||
import androidx.annotation.NonNull;
|
||||
|
||||
import com.aros.apron.entity.Movement;
|
||||
import com.aros.apron.entity.Synchronizedstatus;
|
||||
import com.aros.apron.tools.DroneHelper;
|
||||
import com.aros.apron.tools.LogUtil;
|
||||
import com.google.gson.Gson;
|
||||
|
||||
import dji.v5.common.callback.CommonCallbacks;
|
||||
import dji.v5.common.error.IDJIError;
|
||||
import dji.v5.manager.aircraft.virtualstick.VirtualStickManager;
|
||||
|
||||
public class Apronmixvalue {
|
||||
private Apronmixvalue() {
|
||||
}
|
||||
|
||||
private static class ApronmixvalueHolder {
|
||||
private static final Apronmixvalue INSTANCE = new Apronmixvalue();
|
||||
}
|
||||
|
||||
public static Apronmixvalue getInstance() {
|
||||
return ApronmixvalueHolder.INSTANCE;
|
||||
}
|
||||
|
||||
private volatile boolean isaglinetrue = false; //机头是否对准
|
||||
|
||||
private volatile int trytimes = 0; //复降次数
|
||||
|
||||
public void switchthemmodefpv(){
|
||||
Synchronizedstatus.setAprongim(false);
|
||||
}
|
||||
public void switchthemmodeGime(){
|
||||
Synchronizedstatus.setAprongim(true);
|
||||
}
|
||||
|
||||
public boolean isIsaglinetrue() {
|
||||
return isaglinetrue;
|
||||
}
|
||||
|
||||
public void setIsaglinetrue(boolean isaglinetrue) {
|
||||
this.isaglinetrue = isaglinetrue;
|
||||
}
|
||||
|
||||
public int getTrytimes() {
|
||||
return trytimes;
|
||||
}
|
||||
public void inchangeTrytimes() {
|
||||
this.trytimes++;
|
||||
}
|
||||
public Handler handler = new Handler(Looper.getMainLooper());
|
||||
public void pullup(){
|
||||
Synchronizedstatus.setSwitchtime(false);
|
||||
Aprongim.getInstance().setDropTimes(0);
|
||||
Aprondown.getInstance().setDropTimes(0);
|
||||
|
||||
// 如果高度已经大于 40 分米，直接切换，不用上升
|
||||
if (Movement.getInstance().getUltrasonicHeight() >= 40) {
|
||||
Synchronizedstatus.setSwitchtime(true);
|
||||
return;
|
||||
}
|
||||
|
||||
Runnable runnable = new Runnable() {
|
||||
@Override
|
||||
public void run() {
|
||||
if (Movement.getInstance().getUltrasonicHeight() < 50) {
|
||||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 3f);
|
||||
handler.postDelayed(this, 200);
|
||||
} else {
|
||||
handler.removeCallbacks(this);
|
||||
Synchronizedstatus.setSwitchtime(true);
|
||||
}
|
||||
}
|
||||
};
|
||||
// 开始循环
|
||||
handler.post(runnable);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
Loading…
Reference in New Issue