diff --git a/app/src/main/java/com/aros/apron/mix/Aprondown.java b/app/src/main/java/com/aros/apron/mix/Aprondown.java new file mode 100644 index 00000000..6aabc938 --- /dev/null +++ b/app/src/main/java/com/aros/apron/mix/Aprondown.java @@ -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 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 corners) { + if (ids.empty()) return ids; + int[] idArr = ids.toArray(); + List keepIdx = new ArrayList<>(); + for (int i = 0; i < idArr.length; i++) if (idArr[i] == 6) keepIdx.add(i); + + List 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); + } +} \ No newline at end of file diff --git a/app/src/main/java/com/aros/apron/mix/Aprongim.java b/app/src/main/java/com/aros/apron/mix/Aprongim.java new file mode 100644 index 00000000..66b57941 --- /dev/null +++ b/app/src/main/java/com/aros/apron/mix/Aprongim.java @@ -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 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 smallMarkers = new ArrayList<>(); + List bigMarkers1_4 = new ArrayList<>(); + // 【禁用】5号逻辑注释掉，但保留变量避免编译错误 + // List bigMarker5 = new ArrayList<>(); + List bigMarker6 = new ArrayList<>(); + + int[] idArray = ids.toArray(); + List 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 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 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); + } +} \ No newline at end of file diff --git a/app/src/main/java/com/aros/apron/mix/Apronmixvalue.java b/app/src/main/java/com/aros/apron/mix/Apronmixvalue.java new file mode 100644 index 00000000..0e8fd143 --- /dev/null +++ b/app/src/main/java/com/aros/apron/mix/Apronmixvalue.java @@ -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); + } + + + + + +}