494 lines
19 KiB
Java
494 lines
19 KiB
Java
package com.aros.apron.tools;
|
||
|
||
|
||
import android.os.Environment;
|
||
import android.os.Handler;
|
||
import android.os.Looper;
|
||
import android.util.Log;
|
||
|
||
import com.aros.apron.constant.AMSConfig;
|
||
import com.aros.apron.entity.ArucoMarker;
|
||
import com.aros.apron.entity.ArucoMarkerDimensions;
|
||
import com.aros.apron.entity.Movement;
|
||
import com.aros.apron.manager.AlternateLandingManager;
|
||
|
||
import org.opencv.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.imgcodecs.Imgcodecs;
|
||
import org.opencv.imgproc.Imgproc;
|
||
import org.opencv.objdetect.ArucoDetector;
|
||
import org.opencv.objdetect.DetectorParameters;
|
||
import org.opencv.objdetect.Dictionary;
|
||
|
||
import java.io.File;
|
||
import java.text.SimpleDateFormat;
|
||
import java.util.ArrayList;
|
||
import java.util.Arrays;
|
||
import java.util.Date;
|
||
import java.util.List;
|
||
import java.util.Locale;
|
||
import java.util.concurrent.Executors;
|
||
import java.util.concurrent.Future;
|
||
import java.util.concurrent.ScheduledExecutorService;
|
||
import java.util.concurrent.TimeUnit;
|
||
|
||
public class ApronArucoDetect {
|
||
|
||
//是否触发识别(如果丢失图传，此值为false)
|
||
private boolean isTriggerSuccess;
|
||
|
||
//没识别到二维码
|
||
private boolean arucoNotFoundTag=false;
|
||
|
||
private boolean isStartAruco = false;
|
||
// public ExecutorService mThreadPool = Executors.newSingleThreadExecutor();
|
||
ScheduledExecutorService executor = Executors.newScheduledThreadPool(1);
|
||
Future<?> lastFuture = null;
|
||
|
||
private String TAG = getClass().getSimpleName();
|
||
Double resultYaw = 0.0;
|
||
//触发去备降点
|
||
private boolean triggerToAlternateLandingPoint;
|
||
long startTime;
|
||
long endTime;
|
||
private int sigleMarkerDetectFailsTimes;
|
||
//复降触发条件
|
||
private boolean dropTimesTag;
|
||
//复降次数
|
||
private int dropTimes=0;
|
||
//是否双挂
|
||
private boolean isDoublePayload;
|
||
|
||
// 【关键修改1】M400下视镜头右后方补偿量（像素，正值向右补偿）
|
||
// 如果还往左偏就调大，往右偏就调小，建议范围 30-80
|
||
private static final double LENS_OFFSET_X = 15.0;
|
||
private static final double LENS_OFFSET_Y = 20.0;
|
||
// 【关键修改2】放宽居中判定阈值（原40，补偿后改为60更合理）
|
||
private static final int CENTER_ERR_MAX = 40; // 居中误差（px）
|
||
|
||
public PIDControl pidControlX = null;
|
||
public PIDControl pidControlY = null;
|
||
|
||
//为了解决降落后不停浆问题，增加记录，开启速降时startFastStick之后的过滤次数
|
||
public int checkThrowingErrorsTimes;
|
||
|
||
public int getCheckThrowingErrorsTimes() {
|
||
return checkThrowingErrorsTimes;
|
||
}
|
||
|
||
public void setCheckThrowingErrorsTimes(int checkThrowingErrorsTimes) {
|
||
this.checkThrowingErrorsTimes = checkThrowingErrorsTimes;
|
||
}
|
||
|
||
public boolean isTriggerSuccess() {
|
||
return isTriggerSuccess;
|
||
}
|
||
|
||
public void setTriggerSuccess(boolean triggerSuccess) {
|
||
isTriggerSuccess = triggerSuccess;
|
||
}
|
||
|
||
public boolean isDoublePayload() {
|
||
return isDoublePayload;
|
||
}
|
||
|
||
public void setDoublePayload(boolean doublePayload) {
|
||
isDoublePayload = doublePayload;
|
||
}
|
||
|
||
|
||
private ApronArucoDetect() {
|
||
// 构造函数里也可以调用，但静态代码块更可靠
|
||
}
|
||
|
||
private static class OpenCVHelperHolder {
|
||
private static final ApronArucoDetect INSTANCE = new ApronArucoDetect();
|
||
|
||
static {
|
||
INSTANCE.init();
|
||
}
|
||
}
|
||
|
||
public static ApronArucoDetect getInstance() {
|
||
return OpenCVHelperHolder.INSTANCE;
|
||
}
|
||
|
||
private static final float SLOW_LAND_SPEED = -0.3f; // 远场慢降
|
||
private static final float SLOW_SUPER_SPEED = -0.1f; // 超近慢降
|
||
|
||
private static final int PIXEL_TRIGGER = 400; // 近场像素阈值
|
||
|
||
public void init() {
|
||
pidControlX = new PIDControl(0.6f, 0.015f, 0.12f, 0.05f, 2.0f, 0.05f);
|
||
pidControlY = new PIDControl(0.6f, 0.015f, 0.12f, 0.05f, 2.0f, 0.05f);
|
||
pidControlX.reset();
|
||
pidControlY.reset();
|
||
|
||
}
|
||
|
||
|
||
public void detectArucoTags(int height, int width, byte[] data, Dictionary dictionary) {
|
||
|
||
//这里说明图传正常
|
||
isTriggerSuccess = true;
|
||
//0未启用 虚拟摇杆启用原因 1异常拉高返航触发 2视觉降落触发 3手动触发
|
||
Movement.getInstance().setVirtualStickEnableReason(2);
|
||
//保证一帧进入同时 保证只有一次速降
|
||
if (isStartAruco || startFastStick) {
|
||
// LogUtil.log(TAG, "过滤:" + isStartAruco + startFastStick);
|
||
if (!isStartAruco && startFastStick) {
|
||
checkThrowingErrorsTimes++;
|
||
}
|
||
return;
|
||
}
|
||
|
||
isStartAruco = true;
|
||
/* 如果上一帧任务还没跑完，直接 cancel */
|
||
if (lastFuture != null && !lastFuture.isDone()) {
|
||
LogUtil.log(TAG, "break---");
|
||
lastFuture.cancel(true);
|
||
}
|
||
|
||
|
||
//LogUtil.log(TAG, "执行了");
|
||
lastFuture = executor.schedule(new Runnable() {
|
||
@Override
|
||
public void run() {
|
||
try {
|
||
/* ---------- 1. YUV → BGR → Gray ---------- */
|
||
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);
|
||
|
||
/* 【OpenCV 4.9+】温和预处理（应对飞行模糊，提升识别率） */
|
||
Mat processedMat = fixedPreprocess(grayImgMat);
|
||
|
||
/* 2. 检测二维码（OpenCV 4.12+） */
|
||
MatOfInt ids = new MatOfInt();
|
||
List<Mat> corners = new ArrayList<>();
|
||
Mat corner6 = new Mat();
|
||
|
||
// 【OpenCV 4.12+】创建 ArucoDetector
|
||
DetectorParameters parameters = new DetectorParameters();
|
||
ArucoDetector detector = new ArucoDetector(dictionary, parameters);
|
||
// 先用预处理图像检测
|
||
detector.detectMarkers(processedMat, corners, ids);
|
||
|
||
// 如果预处理检测不到，回退到原图
|
||
if (ids.empty()) {
|
||
corners.clear();
|
||
detector.detectMarkers(grayImgMat, corners, ids);
|
||
}
|
||
|
||
//b保留6号
|
||
ids = keepOnly6(ids, corners);
|
||
|
||
boolean marker6Found = false;
|
||
if (!ids.empty()) {
|
||
arucoNotFoundTag = false;
|
||
int[] idArray = ids.toArray();
|
||
int ultrasonicHeight = Movement.getInstance().getUltrasonicHeight();
|
||
// double flyingHeight = Movement.getInstance().getElevation();
|
||
|
||
// 如果只有一个而且是6
|
||
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]);
|
||
|
||
// 【关键修改3】降落判断加上镜头偏移补偿（向右补偿）
|
||
double centerX = (points[0].x + points[1].x + points[2].x + points[3].x) / 4.0;
|
||
double errX = Math.abs(centerX - rgbMat.width() / 2.0 + LENS_OFFSET_X);
|
||
// 修改为（向前补偿）：
|
||
double centerY = (points[0].y + points[1].y + points[2].y + points[3].y) / 4.0;
|
||
double errY = Math.abs(centerY - rgbMat.height() / 2.0 - LENS_OFFSET_Y); ;
|
||
|
||
LogUtil.log(TAG, "像素" + (int) pixelWidth +
|
||
" errX=" + (int) errX + " errY=" + (int) errY + " (含偏移" + LENS_OFFSET_X + ")");
|
||
|
||
/* 近场 + 对准 + 像素够 → 立即速降 */
|
||
if (!startFastStick &&
|
||
pixelWidth >= PIXEL_TRIGGER &&
|
||
// flyingHeight <= 1 &&
|
||
ultrasonicHeight <= 4 &&
|
||
errX < CENTER_ERR_MAX &&
|
||
errY < CENTER_ERR_MAX) {
|
||
startFastStick = true;
|
||
handler.post(runnable);
|
||
LogUtil.log(TAG, "6号居中+近场触发速降 pixel=" + (int) pixelWidth +
|
||
" errX=" + (int) errX + " errY=" + (int) errY);
|
||
return;
|
||
}
|
||
}
|
||
if(marker6Found){
|
||
//执行位移修正
|
||
moveOnArucoDetected(new ArucoMarker(1, corner6, 0.24f), rgbMat.width(), rgbMat.height());
|
||
}
|
||
dropTimesTag = true;
|
||
} else {
|
||
LogUtil.log(TAG, "找不到了二维码");
|
||
if (!arucoNotFoundTag) {
|
||
startTime = System.currentTimeMillis();
|
||
arucoNotFoundTag = true;
|
||
}
|
||
endTime = System.currentTimeMillis();
|
||
long lostDuration = endTime - startTime;
|
||
//1s到8s内
|
||
if (lostDuration > 700 && lostDuration <= 8000) {
|
||
if (Movement.getInstance().getUltrasonicHeight()<=30) {
|
||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 0.3f);
|
||
if (dropTimes > Integer.parseInt(AMSConfig.getInstance().getAlternateLandingTimes())) {
|
||
LogUtil.log(TAG, "超过复降限制,去备降点");
|
||
AlternateLandingManager.getInstance().startTaskProcess(null);
|
||
return;
|
||
}
|
||
if (dropTimesTag) {
|
||
dropTimesTag = false;
|
||
dropTimes++;
|
||
LogUtil.log(TAG, "复降第:" + dropTimes + "次");
|
||
}
|
||
} else {
|
||
|
||
LogUtil.log(TAG, "执行位移");
|
||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.3f);
|
||
}
|
||
// 超过8s
|
||
} else if (lostDuration > 8000) {
|
||
LogUtil.log(TAG, "判定未识别到二维码,飞往备降点");
|
||
AlternateLandingManager.getInstance().startTaskProcess(null);
|
||
}
|
||
}
|
||
// 释放资源（安全版）
|
||
processedMat.release();
|
||
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, "Exception" + e); // 第一个日志
|
||
isStartAruco = false;
|
||
}
|
||
}
|
||
}, 0, TimeUnit.MILLISECONDS);
|
||
}
|
||
|
||
|
||
/**
|
||
* 只保留 id==6 的 marker
|
||
* 返回新的 MatOfInt（旧对象已 release），corners 列表同步
|
||
*/
|
||
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);
|
||
|
||
// 1. 先把要保留的角点缓存到临时列表
|
||
List<Mat> tmpCorners = new ArrayList<>(keepIdx.size());
|
||
for (int i : keepIdx) tmpCorners.add(corners.get(i));
|
||
|
||
// 2. 再清空原列表
|
||
corners.clear();
|
||
|
||
// 3. 重建 ids 并回填 corners
|
||
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);
|
||
}
|
||
|
||
|
||
/**
|
||
* 【温和预处理】双边滤波 + CLAHE + 锐化（不做二值化）
|
||
*/
|
||
private Mat fixedPreprocess(Mat src) {
|
||
Mat result = new Mat();
|
||
try {
|
||
// 1. 双边滤波（去噪 + 保边）
|
||
Mat filtered = new Mat();
|
||
Imgproc.bilateralFilter(src, filtered, 9, 75, 75);
|
||
|
||
// 2. CLAHE（对比度增强）
|
||
Mat clahe = new Mat();
|
||
Imgproc.createCLAHE(4.0, new Size(8, 8)).apply(filtered, clahe);
|
||
filtered.release();
|
||
|
||
// 3. 锐化（Unsharp Masking）
|
||
Mat blurred = new Mat();
|
||
Imgproc.GaussianBlur(clahe, blurred, new Size(0, 0), 2.0);
|
||
Core.addWeighted(clahe, 1.5, blurred, -0.5, 0, result);
|
||
|
||
clahe.release();
|
||
blurred.release();
|
||
|
||
return result;
|
||
} catch (Exception e) {
|
||
|
||
LogUtil.log(TAG, "预处理失败: " + e.getMessage());
|
||
src.copyTo(result);
|
||
return result;
|
||
}
|
||
}
|
||
|
||
|
||
/**
|
||
* 61 * 计算两个点之间的欧几里得距离
|
||
* 62 * @param p1 第一个点
|
||
* 63 * @param p2 第二个点
|
||
* 64 * @return 两点之间的距离
|
||
* 65
|
||
*/
|
||
private double calculateDistance(Point p1, Point p2) {
|
||
double dx = p2.x - p1.x;
|
||
double dy = p2.y - p1.y;
|
||
return Math.sqrt(dx * dx + dy * dy);
|
||
}
|
||
|
||
public void setDetectedBigMarkers() {
|
||
startFastStick = false; // 停止速降
|
||
isStartAruco = false; // 允许下一帧再进检测
|
||
}
|
||
|
||
/**
|
||
* 只处理一个 ArucoMarker（6 号码）
|
||
*/
|
||
private void moveOnArucoDetected(ArucoMarker marker, // ← 不再是 List
|
||
int imageWidth,
|
||
int imageHeight) {
|
||
|
||
/* 1. 取角点 */
|
||
Mat corner6 = marker.getConner(); // ← 直接拿，不用 get(0)
|
||
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]);
|
||
}
|
||
|
||
/* 2. 中心 + 误差（带镜头偏移补偿） */
|
||
double cx = (pts[0].x + pts[1].x + pts[2].x + pts[3].x) / 4.0;
|
||
double cy = (pts[0].y + pts[1].y + pts[2].y + pts[3].y) / 4.0;
|
||
Point screenCenter = new Point(imageWidth / 2.0, imageHeight / 2.0);
|
||
|
||
double errX = (cx - screenCenter.x) + LENS_OFFSET_X; // 向右补偿
|
||
double errY = (cy - screenCenter.y) - LENS_OFFSET_Y; // Y方向不变
|
||
|
||
/* 3. PID 微调水平 */
|
||
pidControlX.setInputFilterAll((float) (errX / 700.0));
|
||
pidControlY.setInputFilterAll((float) (-errY/ 700.0));
|
||
float vx = (float) Math.max(-0.25, Math.min(0.25, pidControlX.get_pid()));
|
||
float vy = (float) Math.max(-0.25, Math.min(0.25, pidControlY.get_pid()));
|
||
|
||
/* 4. 远场慢降：像素 < 1500 且 > 2 m */
|
||
double pixelWidth = Math.sqrt(Math.pow(pts[1].x - pts[0].x, 2) +
|
||
Math.pow(pts[1].y - pts[0].y, 2));
|
||
// double flyingHeight = Movement.getInstance().getElevation();
|
||
|
||
int currentHeight = Movement.getInstance().getUltrasonicHeight();
|
||
|
||
float vz;
|
||
if (currentHeight <= 4) {
|
||
vz = 0.0f; // 近场悬停，等速降
|
||
} else if (currentHeight <= 6) {
|
||
vz = SLOW_SUPER_SPEED; // -0.1f，过渡层
|
||
} else {
|
||
vz = SLOW_LAND_SPEED; // -0.3f，远场慢降
|
||
}
|
||
// if (flyingHeight <= 0.2) {
|
||
// vz = 0.0f;
|
||
// LogUtil.log(TAG, "相对高度" + flyingHeight + "强制悬停");
|
||
// }
|
||
|
||
/* 5. 输出 */
|
||
DroneHelper.getInstance().moveVxVyYawrateHeight(vx, vy, 0f, vz);
|
||
|
||
/* 6. 日志（加上偏移提示） */
|
||
LogUtil.log(TAG, "vx" + vx + "vy" + vy + "像素居中 errX=" + (int) errX + "(含偏移" + LENS_OFFSET_X + ") errY=" + (int) errY +
|
||
" pixelW=" + (int) pixelWidth + " vz=" + vz + "ultheight" + Movement.getInstance().getUltrasonicHeight());
|
||
}
|
||
|
||
|
||
public boolean startFastStick;
|
||
|
||
public boolean isStartFastStick() {
|
||
return startFastStick;
|
||
}
|
||
|
||
public void setStartFastStick(boolean startFastStick) {
|
||
this.startFastStick = startFastStick;
|
||
}
|
||
|
||
public boolean canLanding;
|
||
|
||
public boolean isCanLanding() {
|
||
return canLanding;
|
||
}
|
||
|
||
public void setCanLanding(boolean canLanding) {
|
||
//测试重置未识别和识别时间,避免刚触发识别就飞向备降点
|
||
startTime = 0;
|
||
endTime = 0;
|
||
this.canLanding = canLanding;
|
||
|
||
}
|
||
|
||
|
||
private int handlerCallbackCount = 0; // 记录回调次数
|
||
private Handler handler = new Handler(Looper.getMainLooper());
|
||
private Runnable runnable = new Runnable() {
|
||
@Override
|
||
public void run() {
|
||
performOperation();
|
||
if (handlerCallbackCount < 20) {
|
||
handler.postDelayed(this, 50); // 每 50 毫秒执行一次，1 秒内执行 20 次
|
||
} else {
|
||
performNextStep();
|
||
}
|
||
}
|
||
};
|
||
|
||
private void performOperation() {
|
||
LogUtil.log(TAG, "快速下拉中..." + handlerCallbackCount);
|
||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -4);
|
||
handlerCallbackCount++; // 增加计数器
|
||
}
|
||
|
||
private void performNextStep() {
|
||
canLanding = true;
|
||
handlerCallbackCount = 0;
|
||
dropTimes = 0;//手动测试避免多次累加后直接飞往备降点
|
||
LogUtil.log(TAG, "下拉完成：触发下一步自动降落");
|
||
handler.removeCallbacks(runnable); // 防止重复执行
|
||
|
||
}
|
||
|
||
|
||
} |