588 lines
24 KiB
Java
588 lines
24 KiB
Java
package com.aros.apron.tools;
|
||
|
||
import android.os.Handler;
|
||
import android.os.Looper;
|
||
|
||
import com.aros.apron.constant.AMSConfig;
|
||
import com.aros.apron.entity.ArucoMarker;
|
||
import com.aros.apron.entity.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.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 ApronArucoDetect {
|
||
|
||
private static final String TAG = "ApronArucoDetect";
|
||
|
||
// ========== 原有参数 ==========
|
||
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;
|
||
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;
|
||
|
||
// ========== 原有方法 ==========
|
||
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 ApronArucoDetect() {}
|
||
private static class OpenCVHelperHolder {
|
||
private static final ApronArucoDetect INSTANCE = new ApronArucoDetect();
|
||
static { INSTANCE.init(); }
|
||
}
|
||
public static ApronArucoDetect getInstance() { return 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;
|
||
if (!ids.empty()) {
|
||
trycount = 0;
|
||
arucoNotFoundTag = false;
|
||
int[] idArray = ids.toArray();
|
||
int ultrasonicHeight = Movement.getInstance().getUltrasonicHeight();
|
||
|
||
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 > Integer.parseInt(AMSConfig.getInstance().getAlternateLandingTimes())) {
|
||
LogUtil.log(TAG_LOG, "超过复降限制,去备降点");
|
||
AlternateLandingManager.getInstance().startTaskProcess(null);
|
||
Movement.getInstance().setAlternate(true);
|
||
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, "判定未识别到二维码,飞往备降点");
|
||
AlternateLandingManager.getInstance().startTaskProcess(null);
|
||
Movement.getInstance().setAlternate(true);
|
||
}
|
||
}
|
||
|
||
// 释放资源
|
||
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 (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;
|
||
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);
|
||
}
|
||
} |