1088 lines
43 KiB
Java
1088 lines
43 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.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;
|
||
|
||
/**
|
||
* 融合视觉降落识别器（独立版本）
|
||
* 集成云台相机和下视相机的识别功能，实现相机切换逻辑
|
||
*
|
||
* 降落策略：
|
||
* 1. 先使用云台相机识别
|
||
* 2. 云台相机识别失败，切换到下视相机
|
||
* 3. 下视相机识别失败，切换回云台相机
|
||
* 4. 最多切换2次，超过则触发备降
|
||
*/
|
||
public class MixedVisionLanding {
|
||
|
||
private static final String TAG_LOG = "MixedVisionLanding";
|
||
|
||
// 单例模式
|
||
private static class SingletonHolder {
|
||
private static final MixedVisionLanding INSTANCE = new MixedVisionLanding();
|
||
static { INSTANCE.init(); }
|
||
}
|
||
|
||
public static MixedVisionLanding getInstance() {
|
||
return SingletonHolder.INSTANCE;
|
||
}
|
||
|
||
// ========== 降落模式 ==========
|
||
public enum LandingMode {
|
||
GIMBAL_CAMERA, // 云台相机
|
||
DOWNWARD_CAMERA // 下视相机
|
||
}
|
||
|
||
// ========== 状态变量 ==========
|
||
private LandingMode currentMode = LandingMode.GIMBAL_CAMERA;
|
||
private int switchCount = 0;
|
||
private static final int MAX_SWITCH_COUNT = 2;
|
||
private boolean isLanding = false;
|
||
private boolean isDoublePayload = false;
|
||
private int startArucoType = 0; // 1执行机库二维码识别 2执行备降点二维码识别
|
||
|
||
// ========== 云台相机参数 ==========
|
||
private static double GIMBAL_LENS_OFFSET_X = 0;
|
||
private static double GIMBAL_LENS_OFFSET_Y = 0;
|
||
private static final int GIMBAL_CENTER_ERR_MAX = 50;
|
||
private static final int GIMBAL_PIXEL_TRIGGER = 360;
|
||
private static final int GIMBAL_TRIGGER_FRAME_THRESHOLD = 2;
|
||
|
||
// ========== 下视相机参数 ==========
|
||
private static double DOWNWARD_LENS_OFFSET_X = 0;
|
||
private static double DOWNWARD_LENS_OFFSET_Y = 0;
|
||
private static final int DOWNWARD_CENTER_ERR_MAX = 30;
|
||
private static final int DOWNWARD_PIXEL_TRIGGER = 360;
|
||
private static final int DOWNWARD_TRIGGER_FRAME_THRESHOLD = 2;
|
||
|
||
// ========== 云台相机状态 ==========
|
||
private boolean gimbalIsTriggerSuccess = false;
|
||
private boolean gimbalArucoNotFoundTag = false;
|
||
private boolean gimbalIsStartAruco = false;
|
||
private boolean gimbalStartFastStick = false;
|
||
private boolean gimbalCanLanding = false;
|
||
private int gimbalConsecutiveTriggerCount = 0;
|
||
private int gimbalFrameCounter = 0;
|
||
private int gimbalDropTimes = 0;
|
||
private boolean gimbalDropTimesTag = false;
|
||
private long gimbalStartTime = 0;
|
||
private long gimbalEndTime = 0;
|
||
private boolean gimbalIsYawAligned = false;
|
||
private int gimbalCurrentLandingMode = 0;
|
||
private int gimbalLastLandingCondition = 0;
|
||
private boolean gimbalSetMF = false;
|
||
|
||
// ========== 下视相机状态 ==========
|
||
private boolean downwardIsTriggerSuccess = false;
|
||
private boolean downwardArucoNotFoundTag = false;
|
||
private boolean downwardIsStartAruco = false;
|
||
private boolean downwardStartFastStick = false;
|
||
private boolean downwardCanLanding = false;
|
||
private int downwardConsecutiveTriggerCount = 0;
|
||
private int downwardFrameCounter = 0;
|
||
private int downwardDropTimes = 0;
|
||
private boolean downwardDropTimesTag = false;
|
||
private long downwardStartTime = 0;
|
||
private long downwardEndTime = 0;
|
||
private boolean downwardIsYawAligned = false;
|
||
|
||
// ========== 执行器 ==========
|
||
private ScheduledExecutorService executor = Executors.newScheduledThreadPool(2);
|
||
private Future<?> lastGimbalFuture = null;
|
||
private Future<?> lastDownwardFuture = null;
|
||
|
||
// ========== PID控制器 ==========
|
||
public PIDControl pidControlX = null;
|
||
public PIDControl pidControlY = null;
|
||
|
||
// ========== Handler ==========
|
||
private Handler handler = new Handler(Looper.getMainLooper());
|
||
private int handlerCallbackCount = 0;
|
||
|
||
// ========== 初始化 ==========
|
||
public void init() {
|
||
LogUtil.log(TAG_LOG, "初始化融合视觉降落识别器");
|
||
pidControlX = new PIDControl(0.7f, 0.02f, 0.2f, 0.05f, 2.5f, 0.1f);
|
||
pidControlY = new PIDControl(0.7f, 0.02f, 0.2f, 0.05f, 2.5f, 0.1f);
|
||
pidControlX.reset();
|
||
pidControlY.reset();
|
||
}
|
||
|
||
// ========== 公共方法 ==========
|
||
public void setDoublePayload(boolean doublePayload) {
|
||
isDoublePayload = doublePayload;
|
||
}
|
||
|
||
public void startLanding() {
|
||
synchronized (this) {
|
||
LogUtil.log(TAG_LOG, "开始融合视觉降落");
|
||
isLanding = true;
|
||
currentMode = LandingMode.GIMBAL_CAMERA;
|
||
switchCount = 0;
|
||
resetGimbalState();
|
||
resetDownwardState();
|
||
}
|
||
}
|
||
|
||
public void stopLanding() {
|
||
synchronized (this) {
|
||
LogUtil.log(TAG_LOG, "停止融合视觉降落");
|
||
isLanding = false;
|
||
startArucoType = 0;
|
||
resetGimbalState();
|
||
resetDownwardState();
|
||
}
|
||
}
|
||
|
||
public LandingMode getCurrentMode() {
|
||
return currentMode;
|
||
}
|
||
|
||
public int getSwitchCount() {
|
||
return switchCount;
|
||
}
|
||
|
||
public boolean isLanding() {
|
||
return isLanding;
|
||
}
|
||
|
||
public int getStartArucoType() {
|
||
return startArucoType;
|
||
}
|
||
|
||
public void setStartArucoType(int type) {
|
||
startArucoType = type;
|
||
}
|
||
|
||
// ========== 云台相机帧处理 ==========
|
||
public void processGimbalFrame(int height, int width, byte[] data, Dictionary dictionary) {
|
||
synchronized (this) {
|
||
if (!isLanding || currentMode != LandingMode.GIMBAL_CAMERA || startArucoType == 0) {
|
||
return;
|
||
}
|
||
|
||
gimbalIsTriggerSuccess = true;
|
||
Movement.getInstance().setVirtualStickEnableReason(2);
|
||
|
||
if (gimbalIsStartAruco || gimbalStartFastStick) {
|
||
return;
|
||
}
|
||
|
||
int currentFrame = gimbalFrameCounter++;
|
||
if (currentFrame % 3 != 0) {
|
||
return;
|
||
}
|
||
|
||
if (currentFrame % 30 != 0) {
|
||
DroneHelper.getInstance().setGimbalPitchdown();
|
||
}
|
||
|
||
gimbalIsStartAruco = true;
|
||
if (lastGimbalFuture != null && !lastGimbalFuture.isDone()) {
|
||
lastGimbalFuture.cancel(true);
|
||
}
|
||
|
||
int ultHeight = Movement.getInstance().getUltrasonicHeight();
|
||
updateGimbalLensOffset(ultHeight);
|
||
|
||
final int finalUltHeight = ultHeight;
|
||
lastGimbalFuture = executor.schedule(() -> {
|
||
try {
|
||
processGimbalFrameInternal(height, width, data, dictionary, finalUltHeight);
|
||
} catch (Exception e) {
|
||
LogUtil.log(TAG_LOG, "云台相机处理异常: " + e);
|
||
synchronized (MixedVisionLanding.this) {
|
||
gimbalIsStartAruco = false;
|
||
}
|
||
handleLandingFailure();
|
||
}
|
||
}, 0, TimeUnit.MILLISECONDS);
|
||
}
|
||
}
|
||
|
||
// ========== 下视相机帧处理 ==========
|
||
public void processDownwardFrame(int height, int width, byte[] data, Dictionary dictionary) {
|
||
synchronized (this) {
|
||
if (!isLanding || currentMode != LandingMode.DOWNWARD_CAMERA || startArucoType == 0) {
|
||
return;
|
||
}
|
||
|
||
downwardIsTriggerSuccess = true;
|
||
Movement.getInstance().setVirtualStickEnableReason(2);
|
||
|
||
if (downwardIsStartAruco || downwardStartFastStick) {
|
||
return;
|
||
}
|
||
|
||
int currentFrame = downwardFrameCounter++;
|
||
if (currentFrame % 2 != 0) {
|
||
return;
|
||
}
|
||
|
||
downwardIsStartAruco = true;
|
||
if (lastDownwardFuture != null && !lastDownwardFuture.isDone()) {
|
||
lastDownwardFuture.cancel(true);
|
||
}
|
||
|
||
int ultHeight = Movement.getInstance().getUltrasonicHeight();
|
||
updateDownwardLensOffset(ultHeight);
|
||
|
||
final int finalUltHeight = ultHeight;
|
||
lastDownwardFuture = executor.schedule(() -> {
|
||
try {
|
||
processDownwardFrameInternal(height, width, data, dictionary, finalUltHeight);
|
||
} catch (Exception e) {
|
||
LogUtil.log(TAG_LOG, "下视相机处理异常: " + e);
|
||
synchronized (MixedVisionLanding.this) {
|
||
downwardIsStartAruco = false;
|
||
}
|
||
handleLandingFailure();
|
||
}
|
||
}, 0, TimeUnit.MILLISECONDS);
|
||
}
|
||
}
|
||
|
||
// ========== 云台相机内部处理 ==========
|
||
private void processGimbalFrameInternal(int height, int width, byte[] data, Dictionary dictionary, int ultHeight) {
|
||
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);
|
||
|
||
if (!ids.empty()) {
|
||
gimbalArucoNotFoundTag = false;
|
||
gimbalDropTimesTag = true;
|
||
|
||
List<ArucoMarker> smallMarkers = new ArrayList<>();
|
||
List<ArucoMarker> bigMarkers1_4 = new ArrayList<>();
|
||
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));
|
||
} 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());
|
||
}
|
||
|
||
// 优先级1：小码模式
|
||
if (!smallMarkers.isEmpty() && ultHeight < 25) {
|
||
gimbalCurrentLandingMode = 1;
|
||
processGimbalSmallMarkers(smallMarkers, rgbMat.width(), rgbMat.height(), ultHeight);
|
||
}
|
||
// 优先级2：1-4号几何中心
|
||
else if (!bigMarkers1_4.isEmpty()) {
|
||
gimbalCurrentLandingMode = 2;
|
||
if (ultHeight > 30 && !gimbalIsYawAligned) {
|
||
processGimbalBigMarkersYawOnly(bigMarkers1_4, rgbMat.width(), rgbMat.height(), ultHeight);
|
||
} else {
|
||
processGimbalBigMarkersCenter(bigMarkers1_4, rgbMat.width(), rgbMat.height(), ultHeight);
|
||
}
|
||
}
|
||
// 优先级3：5/6号纯下降
|
||
else if ((!bigMarker5.isEmpty() || !bigMarker6.isEmpty())) {
|
||
gimbalCurrentLandingMode = 3;
|
||
ArucoMarker target = !bigMarker5.isEmpty() ? bigMarker5.get(0) : bigMarker6.get(0);
|
||
processGimbalLandingOnly(target, rgbMat.width(), rgbMat.height(), ultHeight);
|
||
} else {
|
||
handleGimbalLostMarker(ultHeight);
|
||
}
|
||
} else {
|
||
handleGimbalLostMarker(ultHeight);
|
||
}
|
||
|
||
grayImgMat.release();
|
||
rgbMat.release();
|
||
yuvMat.release();
|
||
ids.release();
|
||
if (!corners.isEmpty()) {
|
||
for (Mat c : corners) if (c != null) c.release();
|
||
}
|
||
gimbalIsStartAruco = false;
|
||
}
|
||
|
||
// ========== 下视相机内部处理 ==========
|
||
private void processDownwardFrameInternal(int height, int width, byte[] data, Dictionary dictionary, int ultHeight) {
|
||
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);
|
||
|
||
// 只保留6号码
|
||
ids = keepOnly6(ids, corners);
|
||
|
||
if (!ids.empty()) {
|
||
downwardArucoNotFoundTag = false;
|
||
downwardDropTimesTag = true;
|
||
|
||
int[] idArray = ids.toArray();
|
||
if (idArray.length == 1 && idArray[0] == 6) {
|
||
Mat 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 + DOWNWARD_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 + DOWNWARD_LENS_OFFSET_Y;
|
||
double errY = Math.abs(centerY - rgbMat.height() / 2.0);
|
||
|
||
LogUtil.log(TAG_LOG, "【下视检测到】6号 像素" + (int) pixelWidth + " errX=" + (int) errX + " errY=" + (int) errY);
|
||
|
||
// 速降判断
|
||
if (!downwardStartFastStick) {
|
||
if (pixelWidth >= DOWNWARD_PIXEL_TRIGGER &&
|
||
ultHeight <= 4 &&
|
||
errX < DOWNWARD_CENTER_ERR_MAX &&
|
||
errY < DOWNWARD_CENTER_ERR_MAX) {
|
||
|
||
downwardConsecutiveTriggerCount++;
|
||
LogUtil.log(TAG_LOG, "下视速降条件满足，累计帧数: " + downwardConsecutiveTriggerCount);
|
||
|
||
if (downwardConsecutiveTriggerCount >= DOWNWARD_TRIGGER_FRAME_THRESHOLD) {
|
||
downwardStartFastStick = true;
|
||
downwardConsecutiveTriggerCount = 0;
|
||
LogUtil.log(TAG_LOG, "【下视触发速降】");
|
||
handler.postDelayed(() -> handler.post(downwardRunnable), 300);
|
||
return;
|
||
}
|
||
} else {
|
||
if (downwardConsecutiveTriggerCount > 0) {
|
||
LogUtil.log(TAG_LOG, "下视速降条件中断，重置累计帧数");
|
||
downwardConsecutiveTriggerCount = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 旋转处理
|
||
if (ultHeight > 30 && !downwardIsYawAligned) {
|
||
double yawError = calculateYawErrorFromCorners(points);
|
||
double absYaw = Math.abs(yawError);
|
||
|
||
if (absYaw < 10.0) {
|
||
downwardIsYawAligned = true;
|
||
LogUtil.log(TAG_LOG, "【下视旋转到位】偏航已对准");
|
||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 0f);
|
||
} else {
|
||
float yawRate = calculateYawRate(yawError, ultHeight);
|
||
LogUtil.log(TAG_LOG, String.format("【下视执行】纯旋转 yawRate=%.1f avgYaw=%.1f", yawRate, yawError));
|
||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, yawRate, 0f);
|
||
}
|
||
} else {
|
||
// 正常修正
|
||
moveOnArucoDetected(new ArucoMarker(1, corner6, 0.24f), rgbMat.width(), rgbMat.height());
|
||
}
|
||
}
|
||
} else {
|
||
handleDownwardLostMarker(ultHeight);
|
||
}
|
||
|
||
grayImgMat.release();
|
||
rgbMat.release();
|
||
yuvMat.release();
|
||
ids.release();
|
||
if (!corners.isEmpty()) {
|
||
for (Mat c : corners) if (c != null) c.release();
|
||
}
|
||
downwardIsStartAruco = false;
|
||
}
|
||
|
||
// ========== 云台相机小码处理 ==========
|
||
private void processGimbalSmallMarkers(List<ArucoMarker> markers, int imgWidth, int imgHeight, int ultHeight) {
|
||
if (markers == null || markers.size() < 3) {
|
||
LogUtil.log(TAG_LOG, "【云台小码数量不足】检测到" + (markers == null ? 0 : markers.size()) + "个");
|
||
return;
|
||
}
|
||
|
||
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 - GIMBAL_LENS_OFFSET_X;
|
||
double offsetY = geoCenterY - imgHeight / 2.0 + GIMBAL_LENS_OFFSET_Y;
|
||
double absX = Math.abs(offsetX);
|
||
double absY = Math.abs(offsetY);
|
||
double z = ultHeight / 10.0;
|
||
|
||
// 特殊降落判断
|
||
boolean isSpecialLanding = false;
|
||
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;
|
||
double singleErrX = cx - imgWidth / 2.0 - GIMBAL_LENS_OFFSET_X;
|
||
double singleErrY = cy - imgHeight / 2.0 + GIMBAL_LENS_OFFSET_Y;
|
||
|
||
if (Math.abs(singleErrX) < 60 && Math.abs(singleErrY) < 60 && ultHeight <= 4) {
|
||
isSpecialLanding = true;
|
||
offsetX = singleErrX; offsetY = singleErrY;
|
||
absX = Math.abs(singleErrX); absY = Math.abs(singleErrY);
|
||
}
|
||
}
|
||
|
||
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;
|
||
double singleErrX = cx - imgWidth / 2.0 - GIMBAL_LENS_OFFSET_X;
|
||
double singleErrY = cy - imgHeight / 2.0 + GIMBAL_LENS_OFFSET_Y;
|
||
|
||
if (Math.abs(singleErrX) < 60 && Math.abs(singleErrY) < 60 && ultHeight <= 4) {
|
||
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 != gimbalLastLandingCondition) {
|
||
if (gimbalConsecutiveTriggerCount > 0) {
|
||
gimbalConsecutiveTriggerCount = 0;
|
||
LogUtil.log(TAG_LOG, "【云台计数器清零】条件变化");
|
||
}
|
||
gimbalLastLandingCondition = currentCondition;
|
||
}
|
||
|
||
if (currentCondition > 0) {
|
||
gimbalConsecutiveTriggerCount++;
|
||
LogUtil.log(TAG_LOG, "【云台计数器累加】count=" + gimbalConsecutiveTriggerCount);
|
||
|
||
if (gimbalConsecutiveTriggerCount >= GIMBAL_TRIGGER_FRAME_THRESHOLD) {
|
||
gimbalStartFastStick = true;
|
||
gimbalConsecutiveTriggerCount = 0;
|
||
gimbalLastLandingCondition = 0;
|
||
LogUtil.log(TAG_LOG, "【云台触发速降】");
|
||
handler.postDelayed(() -> handler.post(gimbalRunnable), 300);
|
||
return;
|
||
}
|
||
}
|
||
|
||
// PID控制
|
||
double outX = 0, outY = 0, outZ = 0;
|
||
if (z <= 0.4) {
|
||
pidControlX.setInputFilterAll((float)offsetX/1750);
|
||
pidControlY.setInputFilterAll(-(float)offsetY/1750);
|
||
outX = Math.max(-0.125, Math.min(0.125, pidControlX.get_pid()));
|
||
outY = Math.max(-0.125, Math.min(0.125, pidControlY.get_pid()));
|
||
} else if (z <= 0.7) {
|
||
pidControlX.setInputFilterAll((float)offsetX/1200);
|
||
pidControlY.setInputFilterAll(-(float)offsetY/1200);
|
||
outX = Math.max(-0.135, Math.min(0.135, pidControlX.get_pid()));
|
||
outY = Math.max(-0.135, Math.min(0.135, pidControlY.get_pid()));
|
||
outZ = (absX < 250 && absY < 250) ? -0.1 : 0;
|
||
} else if (z <= 1.0) {
|
||
pidControlX.setInputFilterAll((float)offsetX/1200);
|
||
pidControlY.setInputFilterAll(-(float)offsetY/1200);
|
||
outX = Math.max(-0.145, Math.min(0.145, pidControlX.get_pid()));
|
||
outY = Math.max(-0.175, Math.min(0.175, pidControlY.get_pid()));
|
||
outZ = (absX < 200 && absY < 200) ? -0.2 : 0;
|
||
} else if (z <= 1.5) {
|
||
pidControlX.setInputFilterAll((float)offsetX/1450);
|
||
pidControlY.setInputFilterAll(-(float)offsetY/1450);
|
||
outX = Math.max(-0.185, Math.min(0.185, pidControlX.get_pid()));
|
||
outY = Math.max(-0.185, Math.min(0.185, pidControlY.get_pid()));
|
||
outZ = (absX < 180 && absY < 180) ? -0.4 : 0;
|
||
} else if (z <= 2.0) {
|
||
pidControlX.setInputFilterAll((float)offsetX/1350);
|
||
pidControlY.setInputFilterAll(-(float)offsetY/1350);
|
||
outX = pidControlX.get_pid();
|
||
outY = 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 = pidControlX.get_pid();
|
||
outY = pidControlY.get_pid();
|
||
outZ = (absX < 200 && absY < 200) ? -0.4 : 0;
|
||
} else {
|
||
pidControlX.setInputFilterAll((float)offsetX/850);
|
||
pidControlY.setInputFilterAll(-(float)offsetY/850);
|
||
outX = pidControlX.get_pid();
|
||
outY = 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", outX, outY, outZ, avgPixelWidth, ultHeight));
|
||
DroneHelper.getInstance().moveVxVyYawrateHeight((float)outX, (float)outY, 0, (float)outZ);
|
||
}
|
||
|
||
// ========== 云台相机大码旋转处理 ==========
|
||
private void processGimbalBigMarkersYawOnly(List<ArucoMarker> markers, int imgWidth, int imgHeight, int ultHeight) {
|
||
double sumYaw = 0;
|
||
int count = 0;
|
||
|
||
for (ArucoMarker marker : markers) {
|
||
Point[] pts = new Point[4];
|
||
Mat corner = marker.getConner();
|
||
for (int i = 0; i < 4; i++) {
|
||
double[] p = corner.get(0, i);
|
||
pts[i] = new Point(p[0], p[1]);
|
||
}
|
||
sumYaw += calculateYawErrorFromCorners(pts);
|
||
count++;
|
||
}
|
||
|
||
double avgYaw = sumYaw / count;
|
||
double absYaw = Math.abs(avgYaw);
|
||
|
||
if (absYaw < 10.0) {
|
||
gimbalIsYawAligned = true;
|
||
LogUtil.log(TAG_LOG, "【云台旋转到位】偏航已对准");
|
||
return;
|
||
}
|
||
|
||
float yawRate = calculateYawRate(avgYaw, ultHeight);
|
||
LogUtil.log(TAG_LOG, String.format("【云台执行】纯旋转 yawRate=%.1f avgYaw=%.1f", yawRate, avgYaw));
|
||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, yawRate, 0f);
|
||
}
|
||
|
||
// ========== 云台相机大码中心处理 ==========
|
||
private void processGimbalBigMarkersCenter(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 = Math.max(-0.20, Math.min(0.20, pidControlX.get_pid()));
|
||
outY = Math.max(-0.20, Math.min(0.20, pidControlY.get_pid()));
|
||
outZ = (absX < 100 && absY < 100) ? -0.4 : 0;
|
||
} else if (z >= 4) {
|
||
pidControlX.setInputFilterAll((float)offsetX/600);
|
||
pidControlY.setInputFilterAll(-(float)offsetY/600);
|
||
outX = Math.max(-0.20, Math.min(0.20, pidControlX.get_pid()));
|
||
outY = Math.max(-0.20, Math.min(0.20, pidControlY.get_pid()));
|
||
outZ = (absX < 120 && absY < 120) ? -0.4 : 0;
|
||
} else if (z >= 2) {
|
||
pidControlX.setInputFilterAll((float)offsetX/900);
|
||
pidControlY.setInputFilterAll(-(float)offsetY/900);
|
||
outX = Math.max(-0.20, Math.min(0.20, pidControlX.get_pid()));
|
||
outY = Math.max(-0.20, Math.min(0.20, pidControlY.get_pid()));
|
||
outZ = (absX < 140 && absY < 140) ? -0.4 : 0;
|
||
}
|
||
|
||
LogUtil.log(TAG_LOG, String.format("【云台执行】大码几何中心 vx=%.3f vy=%.3f vz=%.3f ult=%d %s", outX, outY, outZ, ultHeight, markerInfo.toString()));
|
||
DroneHelper.getInstance().moveVxVyYawrateHeight((float)outX, (float)outY, 0f, (float)outZ);
|
||
}
|
||
|
||
// ========== 云台相机纯下降处理 ==========
|
||
private void processGimbalLandingOnly(ArucoMarker marker, int imgWidth, int imgHeight, int ultHeight) {
|
||
Mat corner = marker.getConner();
|
||
double pixelWidth = calculatePixelWidth(corner);
|
||
|
||
float vz;
|
||
if (ultHeight >= 50) {
|
||
vz = -0.7f;
|
||
} else if (ultHeight <= 15) {
|
||
vz = 0.0f;
|
||
} else {
|
||
vz = 0.3f;
|
||
}
|
||
|
||
LogUtil.log(TAG_LOG, String.format("【云台执行】5/6号纯下降 vz=%.3f pixel=%.0f ult=%d", vz, pixelWidth, ultHeight));
|
||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, vz);
|
||
}
|
||
|
||
// ========== 云台相机丢失处理 ==========
|
||
private void handleGimbalLostMarker(int ultHeight) {
|
||
if (!gimbalArucoNotFoundTag) {
|
||
gimbalStartTime = System.currentTimeMillis();
|
||
gimbalArucoNotFoundTag = true;
|
||
}
|
||
gimbalEndTime = System.currentTimeMillis();
|
||
long lostDuration = gimbalEndTime - gimbalStartTime;
|
||
|
||
if (lostDuration > 1000 && lostDuration <= 12000) {
|
||
if (ultHeight <= 20) {
|
||
LogUtil.log(TAG_LOG, "【云台执行】丢失上升 vz=3.0");
|
||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 3f);
|
||
if (gimbalDropTimes > Integer.parseInt(AMSConfig.getInstance().getAlternateLandingTimes())) {
|
||
LogUtil.log(TAG_LOG, "云台超过复降限制,切换相机");
|
||
handleLandingFailure();
|
||
return;
|
||
}
|
||
if (gimbalDropTimesTag) {
|
||
gimbalDropTimesTag = false;
|
||
gimbalDropTimes++;
|
||
LogUtil.log(TAG_LOG, "云台复降第:" + gimbalDropTimes + "次");
|
||
}
|
||
} else {
|
||
LogUtil.log(TAG_LOG, "【云台执行】丢失下降 vz=-0.3");
|
||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.3f);
|
||
}
|
||
} else if (lostDuration > 8000) {
|
||
LogUtil.log(TAG_LOG, "云台判定未识别到二维码,切换相机");
|
||
handleLandingFailure();
|
||
}
|
||
}
|
||
|
||
// ========== 下视相机丢失处理 ==========
|
||
private void handleDownwardLostMarker(int ultHeight) {
|
||
LogUtil.log(TAG_LOG, "下视找不到了二维码");
|
||
if (!downwardArucoNotFoundTag) {
|
||
downwardStartTime = System.currentTimeMillis();
|
||
downwardArucoNotFoundTag = true;
|
||
}
|
||
downwardEndTime = System.currentTimeMillis();
|
||
long lostDuration = downwardEndTime - downwardStartTime;
|
||
|
||
if (lostDuration > 1000 && lostDuration <= 12000) {
|
||
if (ultHeight <= 20) {
|
||
LogUtil.log(TAG_LOG, "【下视执行】丢失上升 vz=3.0");
|
||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 3f);
|
||
if (downwardDropTimes > Integer.parseInt(AMSConfig.getInstance().getAlternateLandingTimes())) {
|
||
LogUtil.log(TAG_LOG, "下视超过复降限制,切换相机");
|
||
handleLandingFailure();
|
||
return;
|
||
}
|
||
if (downwardDropTimesTag) {
|
||
downwardDropTimesTag = false;
|
||
downwardDropTimes++;
|
||
LogUtil.log(TAG_LOG, "下视复降第:" + downwardDropTimes + "次");
|
||
}
|
||
} else {
|
||
LogUtil.log(TAG_LOG, "【下视执行】丢失下降 vz=-0.3");
|
||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.3f);
|
||
}
|
||
} else if (lostDuration > 8000) {
|
||
LogUtil.log(TAG_LOG, "下视判定未识别到二维码,切换相机");
|
||
handleLandingFailure();
|
||
}
|
||
}
|
||
|
||
// ========== 下视相机移动控制 ==========
|
||
private void moveOnArucoDetected(ArucoMarker marker, int imgWidth, int imgHeight) {
|
||
Mat corner = marker.getConner();
|
||
Point[] points = new Point[4];
|
||
for (int i = 0; i < 4; i++) {
|
||
double[] p = corner.get(0, i);
|
||
points[i] = new Point(p[0], p[1]);
|
||
}
|
||
|
||
double centerX = (points[0].x + points[1].x + points[2].x + points[3].x) / 4.0 + DOWNWARD_LENS_OFFSET_X;
|
||
double centerY = (points[0].y + points[1].y + points[2].y + points[3].y) / 4.0 + DOWNWARD_LENS_OFFSET_Y;
|
||
double errX = centerX - imgWidth / 2.0;
|
||
double errY = centerY - imgHeight / 2.0;
|
||
double absX = Math.abs(errX);
|
||
double absY = Math.abs(errY);
|
||
|
||
int ultHeight = Movement.getInstance().getUltrasonicHeight();
|
||
double z = ultHeight / 10.0;
|
||
|
||
double outX = 0, outY = 0, outZ = 0;
|
||
|
||
if (z <= 0.4) {
|
||
pidControlX.setInputFilterAll((float)errX/1750);
|
||
pidControlY.setInputFilterAll(-(float)errY/1750);
|
||
outX = Math.max(-0.125, Math.min(0.125, pidControlX.get_pid()));
|
||
outY = Math.max(-0.125, Math.min(0.125, pidControlY.get_pid()));
|
||
} else if (z <= 0.7) {
|
||
pidControlX.setInputFilterAll((float)errX/1200);
|
||
pidControlY.setInputFilterAll(-(float)errY/1200);
|
||
outX = Math.max(-0.135, Math.min(0.135, pidControlX.get_pid()));
|
||
outY = Math.max(-0.135, Math.min(0.135, pidControlY.get_pid()));
|
||
outZ = (absX < 250 && absY < 250) ? -0.1 : 0;
|
||
} else if (z <= 1.0) {
|
||
pidControlX.setInputFilterAll((float)errX/1200);
|
||
pidControlY.setInputFilterAll(-(float)errY/1200);
|
||
outX = Math.max(-0.145, Math.min(0.145, pidControlX.get_pid()));
|
||
outY = Math.max(-0.175, Math.min(0.175, pidControlY.get_pid()));
|
||
outZ = (absX < 200 && absY < 200) ? -0.2 : 0;
|
||
} else if (z <= 1.5) {
|
||
pidControlX.setInputFilterAll((float)errX/1450);
|
||
pidControlY.setInputFilterAll(-(float)errY/1450);
|
||
outX = Math.max(-0.185, Math.min(0.185, pidControlX.get_pid()));
|
||
outY = Math.max(-0.185, Math.min(0.185, pidControlY.get_pid()));
|
||
outZ = (absX < 180 && absY < 180) ? -0.4 : 0;
|
||
} else if (z <= 2.0) {
|
||
pidControlX.setInputFilterAll((float)errX/1350);
|
||
pidControlY.setInputFilterAll(-(float)errY/1350);
|
||
outX = pidControlX.get_pid();
|
||
outY = pidControlY.get_pid();
|
||
outZ = (absX < 200 && absY < 200) ? -0.4 : 0;
|
||
} else if (z <= 3.0) {
|
||
pidControlX.setInputFilterAll((float)errX/1250);
|
||
pidControlY.setInputFilterAll(-(float)errY/1250);
|
||
outX = pidControlX.get_pid();
|
||
outY = pidControlY.get_pid();
|
||
outZ = (absX < 200 && absY < 200) ? -0.4 : 0;
|
||
} else {
|
||
pidControlX.setInputFilterAll((float)errX/850);
|
||
pidControlY.setInputFilterAll(-(float)errY/850);
|
||
outX = pidControlX.get_pid();
|
||
outY = pidControlY.get_pid();
|
||
outZ = (absX < 130 && absY < 130) ? -0.4 : 0;
|
||
}
|
||
|
||
LogUtil.log(TAG_LOG, String.format("【下视执行】移动 vx=%.3f vy=%.3f vz=%.3f ult=%d errX=%.0f errY=%.0f", outX, outY, outZ, ultHeight, absX, absY));
|
||
DroneHelper.getInstance().moveVxVyYawrateHeight((float)outX, (float)outY, 0f, (float)outZ);
|
||
}
|
||
|
||
// ========== 降落失败处理（切换相机） ==========
|
||
private void handleLandingFailure() {
|
||
synchronized (this) {
|
||
if (!isLanding) {
|
||
return;
|
||
}
|
||
|
||
switchCount++;
|
||
LogUtil.log(TAG_LOG, "切换相机，当前切换次数: " + switchCount);
|
||
|
||
// 切换到另一个相机
|
||
if (currentMode == LandingMode.GIMBAL_CAMERA) {
|
||
currentMode = LandingMode.DOWNWARD_CAMERA;
|
||
LogUtil.log(TAG_LOG, "切换到下视相机");
|
||
resetDownwardState();
|
||
} else {
|
||
currentMode = LandingMode.GIMBAL_CAMERA;
|
||
LogUtil.log(TAG_LOG, "切换到云台相机");
|
||
resetGimbalState();
|
||
}
|
||
|
||
// 如果达到最大切换次数，触发备降
|
||
if (switchCount >= MAX_SWITCH_COUNT) {
|
||
LogUtil.log(TAG_LOG, "达到最大切换次数，触发备降");
|
||
stopLanding();
|
||
AlternateLandingManager.getInstance().startTaskProcess(null);
|
||
Movement.getInstance().setAlternate(true);
|
||
}
|
||
}
|
||
}
|
||
|
||
// ========== 重置云台相机状态 ==========
|
||
private void resetGimbalState() {
|
||
gimbalIsTriggerSuccess = false;
|
||
gimbalArucoNotFoundTag = false;
|
||
gimbalIsStartAruco = false;
|
||
gimbalStartFastStick = false;
|
||
gimbalCanLanding = false;
|
||
gimbalConsecutiveTriggerCount = 0;
|
||
gimbalFrameCounter = 0;
|
||
gimbalDropTimes = 0;
|
||
gimbalDropTimesTag = false;
|
||
gimbalStartTime = 0;
|
||
gimbalEndTime = 0;
|
||
gimbalIsYawAligned = false;
|
||
gimbalCurrentLandingMode = 0;
|
||
gimbalLastLandingCondition = 0;
|
||
gimbalSetMF = false;
|
||
}
|
||
|
||
// ========== 重置下视相机状态 ==========
|
||
private void resetDownwardState() {
|
||
downwardIsTriggerSuccess = false;
|
||
downwardArucoNotFoundTag = false;
|
||
downwardIsStartAruco = false;
|
||
downwardStartFastStick = false;
|
||
downwardCanLanding = false;
|
||
downwardConsecutiveTriggerCount = 0;
|
||
downwardFrameCounter = 0;
|
||
downwardDropTimes = 0;
|
||
downwardDropTimesTag = false;
|
||
downwardStartTime = 0;
|
||
downwardEndTime = 0;
|
||
downwardIsYawAligned = false;
|
||
}
|
||
|
||
// ========== 更新云台相机镜头偏移 ==========
|
||
private void updateGimbalLensOffset(int ultHeight) {
|
||
if (isDoublePayload) {
|
||
if (ultHeight <= 5) {
|
||
GIMBAL_LENS_OFFSET_X = -120;
|
||
GIMBAL_LENS_OFFSET_Y = 120;
|
||
} else if (ultHeight < 10) {
|
||
GIMBAL_LENS_OFFSET_X = -80;
|
||
GIMBAL_LENS_OFFSET_Y = 80;
|
||
} else if (ultHeight < 20) {
|
||
GIMBAL_LENS_OFFSET_X = -60;
|
||
GIMBAL_LENS_OFFSET_Y = 60;
|
||
}
|
||
} else {
|
||
if (ultHeight <= 5) {
|
||
GIMBAL_LENS_OFFSET_X = 120;
|
||
GIMBAL_LENS_OFFSET_Y = 100;
|
||
} else if (ultHeight < 10) {
|
||
GIMBAL_LENS_OFFSET_X = 80;
|
||
GIMBAL_LENS_OFFSET_Y = 80;
|
||
} else if (ultHeight < 20) {
|
||
GIMBAL_LENS_OFFSET_X = 60;
|
||
GIMBAL_LENS_OFFSET_Y = 60;
|
||
}
|
||
}
|
||
}
|
||
|
||
// ========== 更新下视相机镜头偏移 ==========
|
||
private void updateDownwardLensOffset(int ultHeight) {
|
||
if (ultHeight >= 30) {
|
||
DOWNWARD_LENS_OFFSET_X = 0;
|
||
DOWNWARD_LENS_OFFSET_Y = 0;
|
||
} else if (ultHeight >= 20) {
|
||
DOWNWARD_LENS_OFFSET_X = 0;
|
||
DOWNWARD_LENS_OFFSET_Y = 0;
|
||
} else if (ultHeight >= 10) {
|
||
DOWNWARD_LENS_OFFSET_X = 20;
|
||
DOWNWARD_LENS_OFFSET_Y = 10;
|
||
} else if (ultHeight >= 5) {
|
||
DOWNWARD_LENS_OFFSET_X = 30;
|
||
DOWNWARD_LENS_OFFSET_Y = 20;
|
||
} else {
|
||
DOWNWARD_LENS_OFFSET_X = 55;
|
||
DOWNWARD_LENS_OFFSET_Y = 35;
|
||
}
|
||
}
|
||
|
||
// ========== 工具方法 ==========
|
||
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;
|
||
}
|
||
}
|
||
|
||
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);
|
||
}
|
||
|
||
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 float calculateYawRate(double yawError, int ultHeight) {
|
||
if (ultHeight < 10 || ultHeight > 50) return 0f;
|
||
|
||
double absError = Math.abs(yawError);
|
||
if (absError < 10.0) return 0f;
|
||
|
||
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;
|
||
|
||
return yawError > 0 ? targetRate : -targetRate;
|
||
}
|
||
|
||
private MatOfInt keepOnly6(MatOfInt ids, List<Mat> corners) {
|
||
if (ids.empty()) return ids;
|
||
|
||
int[] idArray = ids.toArray();
|
||
List<Integer> indicesToKeep = new ArrayList<>();
|
||
|
||
for (int i = 0; i < idArray.length; i++) {
|
||
if (idArray[i] == 6) {
|
||
indicesToKeep.add(i);
|
||
}
|
||
}
|
||
|
||
if (indicesToKeep.isEmpty()) {
|
||
ids.release();
|
||
return new MatOfInt();
|
||
}
|
||
|
||
int[] newIds = new int[indicesToKeep.size()];
|
||
List<Mat> newCorners = new ArrayList<>();
|
||
|
||
for (int i = 0; i < indicesToKeep.size(); i++) {
|
||
newIds[i] = 6;
|
||
newCorners.add(corners.get(indicesToKeep.get(i)));
|
||
}
|
||
|
||
corners.clear();
|
||
corners.addAll(newCorners);
|
||
|
||
MatOfInt newIdsMat = new MatOfInt();
|
||
newIdsMat.fromArray(newIds);
|
||
ids.release();
|
||
return newIdsMat;
|
||
}
|
||
|
||
// ========== 云台速降Runnable ==========
|
||
private Runnable gimbalRunnable = new Runnable() {
|
||
@Override
|
||
public void run() {
|
||
LogUtil.log(TAG_LOG, String.format("【云台执行】速降 vz=-5 count=%d/20", handlerCallbackCount));
|
||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -5);
|
||
handlerCallbackCount++;
|
||
if (handlerCallbackCount < 20) {
|
||
handler.postDelayed(this, 50);
|
||
} else {
|
||
gimbalCanLanding = true;
|
||
handlerCallbackCount = 0;
|
||
gimbalDropTimes = 0;
|
||
}
|
||
}
|
||
};
|
||
|
||
// ========== 下视速降Runnable ==========
|
||
private Runnable downwardRunnable = new Runnable() {
|
||
@Override
|
||
public void run() {
|
||
LogUtil.log(TAG_LOG, String.format("【下视执行】速降 vz=-5 count=%d/20", handlerCallbackCount));
|
||
DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -5);
|
||
handlerCallbackCount++;
|
||
if (handlerCallbackCount < 20) {
|
||
handler.postDelayed(this, 50);
|
||
} else {
|
||
downwardCanLanding = true;
|
||
handlerCallbackCount = 0;
|
||
downwardDropTimes = 0;
|
||
}
|
||
}
|
||
};
|
||
}
|