融合降落
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