From e3c49008ef8163ea1aa82ff3d7f5fd5efd67e50a Mon Sep 17 00:00:00 2001 From: cxf <2417125293@qq.com> Date: Mon, 15 Jun 2026 17:13:02 +0800 Subject: [PATCH] apriltag --- .../com/aros/apron/activity/MainActivity.kt | 80 +++- .../java/com/aros/apron/base/BaseManager.java | 18 +- .../com/aros/apron/callback/MqttCallBack.java | 27 +- .../com/aros/apron/manager/FlightManager.java | 119 ++++- .../com/aros/apron/manager/GimbalManager.java | 2 + .../aros/apron/manager/MissionV3Manager.java | 1 + .../apron/manager/PayloadWidgetManager.java | 7 +- .../apron/manager/PayloadlightManager.java | 2 +- .../WayLineExecutingInterruptManager.java | 2 +- .../com/aros/apron/tools/AprilTagPort.java | 446 ++++++++++++++++++ .../aros/apron/tools/ApronArucodownmany.java | 50 +- .../com/example/nativec/ApriltagDetector.java | 334 +++++++++++++ 12 files changed, 1001 insertions(+), 87 deletions(-) create mode 100644 app/src/main/java/com/aros/apron/tools/AprilTagPort.java create mode 100644 app/src/main/java/com/example/nativec/ApriltagDetector.java diff --git a/app/src/main/java/com/aros/apron/activity/MainActivity.kt b/app/src/main/java/com/aros/apron/activity/MainActivity.kt index 32c163c9..c3eb15df 100644 --- a/app/src/main/java/com/aros/apron/activity/MainActivity.kt +++ b/app/src/main/java/com/aros/apron/activity/MainActivity.kt @@ -49,6 +49,7 @@ import com.aros.apron.manager.WirelessLinkManager import com.aros.apron.mix.Aprondown import com.aros.apron.mix.Aprongim import com.aros.apron.tools.AlternateArucoDetect +import com.aros.apron.tools.AprilTagPort import com.aros.apron.tools.ApronArucoDetect import com.aros.apron.tools.ApronArucoDetectPort import com.aros.apron.tools.ApronArucodownmany @@ -58,6 +59,7 @@ import com.aros.apron.tools.MqttManager import com.aros.apron.tools.PreferenceUtils import com.aros.apron.tools.SimplePortScanner import com.dji.wpmzsdk.manager.WPMZManager +import com.example.nativec.ApriltagDetector import com.google.gson.Gson import dji.sdk.keyvalue.key.CameraKey import dji.sdk.keyvalue.key.DJIKey @@ -823,6 +825,49 @@ open class MainActivity : BaseActivity() { GimbalManager.getInstance().setmode() PayloadWidgetManager.getInstance().initPayloadInfo() + // ===== AprilTag 初始化 ===== + // 相机内参（Plumb Bob 标定结果） + ApriltagDetector.getInstance().setCameraParams( + 669.256846, // fx + 669.564155, // fy + 489.4201049, // cx + 358.3523993 // cy + ) + + // 每个 tag 的物理尺寸（米） + val tagSizeMap = mapOf( + 248 to 0.167, 247 to 0.167, + 246 to 0.071, 244 to 0.071, + 242 to 0.063, 241 to 0.061, + 245 to 0.084, + 250 to 0.187, 249 to 0.187 + ) + ApriltagDetector.getInstance().setTagSizeMap(tagSizeMap) + ApriltagDetector.getInstance().setTagSize(0.16) // fallback + + // 每个 tag 离降落点的偏移（米）：[dx右, dy前] + val tagOffsetMap = mapOf( + 248 to doubleArrayOf(-0.262, 0.052), + 247 to doubleArrayOf( 0.267, 0.052), + 246 to doubleArrayOf(-0.052, 0.0), + 244 to doubleArrayOf( 0.056, 0.0), + 242 to doubleArrayOf(-0.262, -0.194), + 241 to doubleArrayOf( 0.267, -0.192), + 245 to doubleArrayOf( 0.0, -0.404), + 250 to doubleArrayOf(-0.262, -0.483), + 249 to doubleArrayOf( 0.267, -0.480) + ) + ApriltagDetector.getInstance().setTagOffsets(tagOffsetMap) + + // 初始化 C 层检测器 + // init(家族名, 纠错bit, 降采样, 高斯模糊sigma, 线程数) + ApriltagDetector.getInstance().init("tag36h11", 2, 2.0, 0.0, 4) + + + + + + if (PreferenceUtils.getInstance().lteEnable) { // LogUtil.log("qwq","lteEnable"+PreferenceUtils.getInstance().lteEnable) MLTEManager.getInstance().initLTEManager() @@ -1093,7 +1138,7 @@ open class MainActivity : BaseActivity() { @SuppressLint("SuspiciousIndentation") private fun initFpvStream() { cameraManager.addFrameListener( - ComponentIndexType.FPV, + ComponentIndexType.PORT_1, ICameraStreamManager.FrameFormat.YUV420_888 ) { frameData, _, _, width, height, _ -> Movement.getInstance().isVtx = true @@ -1113,11 +1158,17 @@ open class MainActivity : BaseActivity() { } if (startArucoType == 1) { - ApronArucoDetect.getInstance()?.detectArucoTags( - height, - width, + +// ApronArucodownmany.getInstance()?.detectArucoTags( +// height, +// width, +// frameData, +// dictionary +// ) + AprilTagPort.getInstance().processFrame( frameData, - dictionary + width, + height ) } else if (startArucoType == 2) { AlternateArucoDetect.getInstance()?.detectArucoTags( @@ -1127,12 +1178,12 @@ open class MainActivity : BaseActivity() { dictionary ) } else if (startArucoType == 3) { - ApronArucoDetect.getInstance()?.detectForceTriggerTags( - height, - width, - frameData, - dictionary - ) +// ApronArucodownmany.getInstance()?.detectForceTriggerTags( +// height, +// width, +// frameData, +// dictionary +// ) } } finally { Synchronizedstatus.setIsruningframe(false) @@ -1249,7 +1300,7 @@ open class MainActivity : BaseActivity() { LogUtil.log(TAG, "取消降落,识别机库二维码") if (PreferenceUtils.getInstance().cameraLocationType == 3) { Handler().postDelayed(Runnable { - if (!ApronArucoDetect.getInstance().isTriggerSuccess) { + if (!ApronArucodownmany.getInstance().isTriggerSuccess) { LogUtil.log(TAG, "图传异常:飞往备降点") //测试图传丢失 AlternateLandingManager.getInstance().startTaskProcess(null) @@ -1280,6 +1331,8 @@ open class MainActivity : BaseActivity() { ApronArucoDetectPort.getInstance().setDetectedBigMarkers() Aprongim.getInstance().setDetectedBigMarkers() Aprondown.getInstance().setDetectedBigMarkers() + ApronArucodownmany.getInstance().setDetectedBigMarkers() + AprilTagPort.getInstance().reset() DroneHelper.getInstance().setGimbalPitchDegree() @@ -1298,6 +1351,8 @@ open class MainActivity : BaseActivity() { ApronArucoDetectPort.getInstance().setDetectedBigMarkers() Aprongim.getInstance().setDetectedBigMarkers() Aprondown.getInstance().setDetectedBigMarkers() + ApronArucodownmany.getInstance().setDetectedBigMarkers() + AprilTagPort.getInstance().reset() DroneHelper.getInstance().setGimbalPitchDegree() @@ -1371,6 +1426,7 @@ open class MainActivity : BaseActivity() { ApronArucoDetectPort.getInstance().setStartAruco(false); Aprongim.getInstance().setStartAruco(false); Aprondown.getInstance().setStartAruco(false); + ApronArucodownmany.getInstance().setStartAruco(false); KeyManager.getInstance().performAction( diff --git a/app/src/main/java/com/aros/apron/base/BaseManager.java b/app/src/main/java/com/aros/apron/base/BaseManager.java index cefc2252..8ea298bf 100644 --- a/app/src/main/java/com/aros/apron/base/BaseManager.java +++ b/app/src/main/java/com/aros/apron/base/BaseManager.java @@ -86,9 +86,11 @@ public abstract class BaseManager { * @param entity */ public void sendMsg2Server(MessageDown entity) { - // 延迟 500ms 发送，避开媒体/日志上传等突发流量高峰 - mainHandler.postDelayed(() -> { + LogUtil.log(TAG, "sendMsg2Server 入口, method=" + entity.getMethod() + ", tid=" + entity.getTid()); + // 直接在主线程发送（Paho 内部有独立发送线程，不会阻塞主线程） + mainHandler.post(() -> { try { + LogUtil.log(TAG, "sendMsg2Server 执行, connected=" + MqttManager.getInstance().mqttAndroidClient.isConnected() + ", tid=" + entity.getTid()); if (MqttManager.getInstance().mqttAndroidClient.isConnected()) { MessageReply messageReply = new MessageReply(); messageReply.setBid(entity.getBid()); @@ -100,15 +102,17 @@ public abstract class BaseManager { messageReply.setData(data); MqttMessage mqttMessage = new MqttMessage(new Gson().toJson(messageReply).getBytes("UTF-8")); mqttMessage.setQos(1); - MqttManager.getInstance().mqttAndroidClient.publish(AMSConfig.UP_UAV_SERVICES_REPLY, mqttMessage); + org.eclipse.paho.client.mqttv3.IMqttDeliveryToken token = + MqttManager.getInstance().mqttAndroidClient.publish(AMSConfig.UP_UAV_SERVICES_REPLY, mqttMessage); + LogUtil.log(TAG, "回复已提交, tid=" + entity.getTid() + ", msgId=" + token.getMessageId() + ", method=" + entity.getMethod()); } else { - LogUtil.log(TAG, "回复失败：mqtt 未连接"); + LogUtil.log(TAG, "回复失败：mqtt 未连接, tid=" + entity.getTid()); } } catch (Exception e) { e.printStackTrace(); - LogUtil.log(TAG, "回复异常：" + e.toString()); + LogUtil.log(TAG, "回复异常：" + e.toString() + ", tid=" + entity.getTid()); } - }, 500); + }); } final Handler mainHandler = new Handler(Looper.getMainLooper()); @@ -652,7 +656,7 @@ public abstract class BaseManager { public boolean getGimbalAndCameraEnabled() { - if (!PreferenceUtils.getInstance().getNeedTriggerApronArucoLand() && !PreferenceUtils.getInstance().getNeedTriggerAlterArucoLand() && Movement.getInstance().getGoHomeState() != 1 && Movement.getInstance().getGoHomeState() != 2) { + if (!PreferenceUtils.getInstance().getNeedTriggerApronArucoLand() && !PreferenceUtils.getInstance().getNeedTriggerAlterArucoLand() && Movement.getInstance().getGoHomeState() != 2) { return true; } else { LogUtil.log(TAG, "降落时不允许操作云台/相机/虚拟摇杆"); diff --git a/app/src/main/java/com/aros/apron/callback/MqttCallBack.java b/app/src/main/java/com/aros/apron/callback/MqttCallBack.java index 7b006a3b..0f8801a2 100644 --- a/app/src/main/java/com/aros/apron/callback/MqttCallBack.java +++ b/app/src/main/java/com/aros/apron/callback/MqttCallBack.java @@ -38,6 +38,7 @@ import com.aros.apron.mix.Aprondown; import com.aros.apron.mix.Aprongim; import com.aros.apron.tools.ApronArucoDetect; import com.aros.apron.tools.ApronArucoDetectPort; +import com.aros.apron.tools.ApronArucodownmany; import com.aros.apron.tools.Generakmztools; import com.aros.apron.tools.LogUtil; import com.aros.apron.tools.MqttManager; @@ -138,7 +139,7 @@ public class MqttCallBack extends BaseManager implements MqttCallbackExtended { } else { switch (message.getMethod()) { case Constant.PILOT_ON: -// LogUtil.log(TAG, "收到：遥控器是否开机" + jsonString); + // LogUtil.log(TAG, "收到：遥控器是否开机" + jsonString); SystemManager.getInstance().checkRemoteControlPowerStatus(message); break; case Constant.AIRCRAFT_ON: @@ -215,7 +216,7 @@ public class MqttCallBack extends BaseManager implements MqttCallbackExtended { LogUtil.log(TAG, "收到：返航" + jsonString); //自动返航 (如果调用方法失败了 这个设置就有一个问题但是为了方便看懂我就放这里了没放成功的回调里面) Movement.getInstance().setMode_code(9); - if (!Movement.getInstance().isAlternate() && !ApronArucoDetectPort.getInstance().isStartAruco() && !ApronArucoDetect.getInstance().isStartAruco() && !Aprongim.getInstance().isStartAruco() && !Aprondown.getInstance().isStartAruco()) { + if (!Movement.getInstance().isAlternate() && !ApronArucoDetectPort.getInstance().isStartAruco() && !ApronArucoDetect.getInstance().isStartAruco() && !Aprongim.getInstance().isStartAruco() && !Aprondown.getInstance().isStartAruco()&& !ApronArucodownmany.getInstance().isStartAruco()) { // if(Movement.getInstance().getElevation()<15){ // // }else{ @@ -324,7 +325,7 @@ public class MqttCallBack extends BaseManager implements MqttCallbackExtended { break; case Constant.DRONE_CONTROL: LogUtil.log(TAG, " " + jsonString); - if (!Movement.getInstance().isAlternate() && !ApronArucoDetectPort.getInstance().isStartAruco() && !ApronArucoDetect.getInstance().isStartAruco() && !Aprongim.getInstance().isStartAruco() && !Aprondown.getInstance().isStartAruco()) { + if (!Movement.getInstance().isAlternate() && !ApronArucoDetectPort.getInstance().isStartAruco() && !ApronArucoDetect.getInstance().isStartAruco() && !Aprongim.getInstance().isStartAruco() && !Aprondown.getInstance().isStartAruco() && !ApronArucodownmany.getInstance().isStartAruco()) { resetVirtualStickHeartbeat(); StickManager.getInstance().sendVirtualStickAdvancedParam(message); }else{ @@ -359,7 +360,7 @@ public class MqttCallBack extends BaseManager implements MqttCallbackExtended { break; case Constant.CAMERA_SCREEN_DRAG: LogUtil.log(TAG, "收到：负载控制—画面拖动控制" + jsonString); - if (ApronArucoDetect.getInstance().isStartAruco() || ApronArucoDetectPort.getInstance().isStartAruco() || Aprongim.getInstance().isStartAruco() || Aprondown.getInstance().isStartAruco()) { + if (ApronArucoDetect.getInstance().isStartAruco() || ApronArucoDetectPort.getInstance().isStartAruco() || Aprongim.getInstance().isStartAruco() || Aprondown.getInstance().isStartAruco()&& !ApronArucodownmany.getInstance().isStartAruco()) { sendFailMsg2Server(message,"自动降落不可以动负载"); sendEvent2Server("自动降落不可以动负载", 1); return; @@ -369,7 +370,7 @@ public class MqttCallBack extends BaseManager implements MqttCallbackExtended { break; case Constant.CAMERA_AIM: LogUtil.log(TAG, "收到：负载控制—双击成为 AIM" + jsonString); - if (ApronArucoDetect.getInstance().isStartAruco() || ApronArucoDetectPort.getInstance().isStartAruco() || Aprongim.getInstance().isStartAruco() || Aprondown.getInstance().isStartAruco()) { + if (ApronArucoDetect.getInstance().isStartAruco() || ApronArucoDetectPort.getInstance().isStartAruco() || Aprongim.getInstance().isStartAruco() || Aprondown.getInstance().isStartAruco()&& !ApronArucodownmany.getInstance().isStartAruco()) { sendFailMsg2Server(message,"自动降落不可以动负载"); sendEvent2Server("自动降落不可以动负载", 1); return; @@ -379,7 +380,7 @@ public class MqttCallBack extends BaseManager implements MqttCallbackExtended { break; case Constant.CAMERA_FOCAL_LENGTH_SET: LogUtil.log(TAG, "收到：负载控制—变焦" + jsonString); - if (ApronArucoDetect.getInstance().isStartAruco() || ApronArucoDetectPort.getInstance().isStartAruco() || Aprongim.getInstance().isStartAruco() || Aprondown.getInstance().isStartAruco()) { + if (ApronArucoDetect.getInstance().isStartAruco() || ApronArucoDetectPort.getInstance().isStartAruco() || Aprongim.getInstance().isStartAruco() || Aprondown.getInstance().isStartAruco()&& !ApronArucodownmany.getInstance().isStartAruco()) { sendFailMsg2Server(message,"自动降落不可以动负载"); sendEvent2Server("自动降落不可以动负载", 1); return; @@ -389,7 +390,7 @@ public class MqttCallBack extends BaseManager implements MqttCallbackExtended { break; case Constant.GIMBAL_RESET: LogUtil.log(TAG, "收到：负载控制—重置云台" + jsonString); - if (ApronArucoDetect.getInstance().isStartAruco() || ApronArucoDetectPort.getInstance().isStartAruco() || Aprongim.getInstance().isStartAruco() || Aprondown.getInstance().isStartAruco()) { + if (ApronArucoDetect.getInstance().isStartAruco() || ApronArucoDetectPort.getInstance().isStartAruco() || Aprongim.getInstance().isStartAruco() || Aprondown.getInstance().isStartAruco()&& !ApronArucodownmany.getInstance().isStartAruco()) { sendFailMsg2Server(message,"自动降落不可以动负载"); sendEvent2Server("自动降落不可以动负载", 1); return; @@ -399,7 +400,7 @@ public class MqttCallBack extends BaseManager implements MqttCallbackExtended { break; case Constant.CAMERA_LOOK_AT: LogUtil.log(TAG, "收到：负载控制—Look At" + jsonString); - if (ApronArucoDetect.getInstance().isStartAruco() || ApronArucoDetectPort.getInstance().isStartAruco() || Aprongim.getInstance().isStartAruco() || Aprondown.getInstance().isStartAruco()) { + if (ApronArucoDetect.getInstance().isStartAruco() || ApronArucoDetectPort.getInstance().isStartAruco() || Aprongim.getInstance().isStartAruco() || Aprondown.getInstance().isStartAruco()&& !ApronArucodownmany.getInstance().isStartAruco()) { sendFailMsg2Server(message,"自动降落不可以动负载"); sendEvent2Server("自动降落不可以动负载", 1); return; @@ -456,7 +457,7 @@ public class MqttCallBack extends BaseManager implements MqttCallbackExtended { break; case Constant.CAMERA_FRAME_ZOOM: LogUtil.log(TAG, "收到：框选变焦" + jsonString); - if (ApronArucoDetect.getInstance().isStartAruco() || ApronArucoDetectPort.getInstance().isStartAruco() || Aprongim.getInstance().isStartAruco() || Aprondown.getInstance().isStartAruco()) { + if (ApronArucoDetect.getInstance().isStartAruco() || ApronArucoDetectPort.getInstance().isStartAruco() || Aprongim.getInstance().isStartAruco() || Aprondown.getInstance().isStartAruco()&& !ApronArucodownmany.getInstance().isStartAruco()) { sendFailMsg2Server(message,"自动降落不可以动负载"); sendEvent2Server("自动降落不可以动负载", 1); return; @@ -1034,7 +1035,13 @@ public class MqttCallBack extends BaseManager implements MqttCallbackExtended { @Override public void deliveryComplete(IMqttDeliveryToken token) { - + try { + LogUtil.log(TAG, "消息送达确认, msgId=" + token.getMessageId() + + ", complete=" + token.isComplete() + + ", topics=" + java.util.Arrays.toString(token.getTopics())); + } catch (Exception e) { + LogUtil.log(TAG, "deliveryComplete 异常: " + e); + } } String[] topics = new String[]{ diff --git a/app/src/main/java/com/aros/apron/manager/FlightManager.java b/app/src/main/java/com/aros/apron/manager/FlightManager.java index da8eab48..04a3913b 100644 --- a/app/src/main/java/com/aros/apron/manager/FlightManager.java +++ b/app/src/main/java/com/aros/apron/manager/FlightManager.java @@ -22,6 +22,7 @@ import com.aros.apron.mix.Aprongim; import com.aros.apron.tools.AlternateArucoDetect; import com.aros.apron.tools.ApronArucoDetect; import com.aros.apron.tools.ApronArucoDetectPort; +import com.aros.apron.tools.ApronArucodownmany; import com.aros.apron.tools.DroneHelper; import com.aros.apron.tools.FlyToPointProgressScheduler; import com.aros.apron.tools.Gpsdistance; @@ -54,6 +55,7 @@ import dji.sdk.keyvalue.value.camera.CameraVideoStreamSourceType; import dji.sdk.keyvalue.value.common.Attitude; import dji.sdk.keyvalue.value.common.ComponentIndexType; import dji.sdk.keyvalue.value.common.EmptyMsg; +import dji.sdk.keyvalue.value.common.IntValueConfig; import dji.sdk.keyvalue.value.common.LocationCoordinate2D; import dji.sdk.keyvalue.value.common.LocationCoordinate3D; import dji.sdk.keyvalue.value.common.Velocity3D; @@ -187,7 +189,7 @@ public class FlightManager extends BaseManager { public void onDeviceStatusUpdate(DJIDeviceStatus from, DJIDeviceStatus to) { if (to != null && !TextUtils.isEmpty(to.description())) { LogUtil.log(TAG, "setPlaneMessage" + to.description()); - sendEvent2Server("PlaneMessage"+to.description(),1); + sendEvent2Server("PlaneMessage" + to.description(), 1); Movement.getInstance().setPlaneMessage(to.description()); pushFlightAttitude(); } @@ -256,7 +258,7 @@ public class FlightManager extends BaseManager { if (!isFlying) { RTHTrackerManager.getInstance().reset(); - }else{ + } else { RTHTrackerManager.getInstance().startReporting(); } @@ -359,12 +361,11 @@ public class FlightManager extends BaseManager { Movement.getInstance().setHome_distance(distance); - Movement.getInstance().setHeight(GpsUtils.egm96Altitude((Movement.getInstance().getRtk_takeoff_altitude() + Movement.getInstance().getElevation()), newValue.getLatitude(), newValue.getLongitude())); - // Movement.getInstance().setHeight(Movement.getInstance().getRtk_takeoff_altitude() + Movement.getInstance().getElevation()); + // Movement.getInstance().setHeight(Movement.getInstance().getRtk_takeoff_altitude() + Movement.getInstance().getElevation()); Movement.getInstance().setLatitude(newValue.getLatitude()); Movement.getInstance().setLongitude(newValue.getLongitude()); @@ -431,7 +432,7 @@ public class FlightManager extends BaseManager { if (newValue != null) { Movement.getInstance().setGps_number(newValue); Movement.getInstance().setRtk_number(newValue); - // LogUtil.log(TAG,"number"+newValue); + // LogUtil.log(TAG,"number"+newValue); pushFlightAttitude(); } } @@ -524,6 +525,48 @@ public class FlightManager extends BaseManager { } }); + + //限高 + KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyHeightLimitRange), this, new CommonCallbacks.KeyListener() { + @Override + public void onValueChange(@Nullable IntValueConfig intValueConfig, @Nullable IntValueConfig t1) { + if (t1 != null) { + if (t1.getMax() > 500) { + KeyManager.getInstance().setValue(createKey(FlightControllerKey.KeyHeightLimit), t1.getMax(), new CommonCallbacks.CompletionCallback() { + @Override + public void onSuccess() { + LogUtil.log(TAG, "限高设置成功" + t1.getMax()); + sendEvent2Server("限高设置成功" + t1.getMax(), 1); + } + + @Override + public void onFailure(@NonNull IDJIError idjiError) { + LogUtil.log(TAG, "限高设置失败" + t1.getMax()); + sendEvent2Server("限高设置失败" + t1.getMax(), 1); + } + }); + } else { + KeyManager.getInstance().setValue(createKey(FlightControllerKey.KeyHeightLimit), t1.getMax(), new CommonCallbacks.CompletionCallback() { + @Override + public void onSuccess() { + LogUtil.log(TAG, "限高设置成功" + 500); + sendEvent2Server("限高设置成功" + 500, 1); + } + + @Override + public void onFailure(@NonNull IDJIError idjiError) { + LogUtil.log(TAG, "限高设置失败" + 500); + sendEvent2Server("限高设置失败" + 500, 2); + } + }); + } + LogUtil.log(TAG, "限高最高" + t1.getMax()); + sendEvent2Server("限高最高" + t1.getMax(), 1); + } + } + }); + + //飞行器状态 (未找到类似上云api的枚举格式，先用FlightMode替代) KeyManager.getInstance().listen(createKey(FlightControllerKey.KeyFlightMode), this, new CommonCallbacks.KeyListener() { @Override @@ -598,7 +641,7 @@ public class FlightManager extends BaseManager { Movement.getInstance().setFlightmode(0); break; case FORCE_LANDING: - LogUtil.log(TAG,"强制降落触发"); + LogUtil.log(TAG, "强制降落触发"); Movement.getInstance().setMode_code(11); break; case POI: @@ -966,7 +1009,6 @@ public class FlightManager extends BaseManager { flyingHeight > FLYING_HEIGHT_THRESHOLD && !sendOpenCabinDoorMsg; - if (!PreferenceUtils.getInstance().getTriggerToAlternatePoint() && isReturningHome && isDistanceAndHeightValid && !isDebugMode) { LogUtil.log(TAG, "返航距离:" + distance + "---当前高度:" + flyingHeight); @@ -988,12 +1030,12 @@ public class FlightManager extends BaseManager { MediaDataCenter.getInstance().getCameraStreamManager().setVisionAssistViewDirection(VisionAssistDirection.DOWN, new CommonCallbacks.CompletionCallback() { @Override public void onSuccess() { - LogUtil.log(TAG,"下视开启成功"); + LogUtil.log(TAG, "下视开启成功"); } @Override public void onFailure(@NonNull IDJIError idjiError) { - LogUtil.log(TAG,"下视开启失败"); + LogUtil.log(TAG, "下视开启失败"); } }); @@ -1005,45 +1047,56 @@ public class FlightManager extends BaseManager { KeyManager.getInstance().setValue(KeyTools.createKey(FlightControllerKey.KeyLEDsSettings), leDsSettings, new CommonCallbacks.CompletionCallback() { @Override public void onSuccess() { - LogUtil.log(TAG,"夜航灯关闭成功"); + LogUtil.log(TAG, "夜航灯关闭成功"); } @Override public void onFailure(@NonNull IDJIError idjiError) { - LogUtil.log(TAG,"夜航灯关闭失败"); + LogUtil.log(TAG, "夜航灯关闭失败"); } }); KeyManager.getInstance().setValue(KeyTools.createKey(FlightAssistantKey.KeyBottomAuxiliaryLightMode), AuxiliaryLightMode.OFF, new CommonCallbacks.CompletionCallback() { @Override public void onSuccess() { - LogUtil.log(TAG,"下视辅助灯开启成功"); + LogUtil.log(TAG, "下视辅助灯开启成功"); } @Override public void onFailure(@NonNull IDJIError idjiError) { - LogUtil.log(TAG,"夜航灯关闭失败"); + LogUtil.log(TAG, "夜航灯关闭失败"); } }); //DroneHelper.getInstance().setGimbalPitchDegree(); //将镜头设置为自动对焦 //DroneHelper.getInstance().setCameraFocusMode(); + //归中 + + //云台设置 +// new android.os.Handler().postDelayed(() -> { +// GimbalManager.getInstance().gimbalReset(); +// }, 500); +// //广角 +// new android.os.Handler().postDelayed(() -> { +// CameraManager.getInstance().resumeLensToWideISOManual(); +// }, 500); + new android.os.Handler().postDelayed(() -> { DroneHelper.getInstance().setGimbalPitchDegree(); - }, 500); + }, 600); new android.os.Handler().postDelayed(() -> { DroneHelper.getInstance().setCameraFocusMode(); - }, 500); + }, 600); new android.os.Handler().postDelayed(() -> { setkuaim(); - }, 500); + }, 600); new android.os.Handler().postDelayed(() -> { setCameraExposureMode(); - }, 500); + }, 600); new android.os.Handler().postDelayed(() -> { EventBus.getDefault().post("REFRESH_VIDEO_SOURCE"); }, 2000); @@ -1144,6 +1197,7 @@ public class FlightManager extends BaseManager { LogUtil.log(TAG, "相机未连接"); } } + //快门优先 private void setCameraExposureMode() { Boolean cameraConnect = KeyManager.getInstance().getValue(KeyTools.createKey(CameraKey. @@ -1164,6 +1218,7 @@ public class FlightManager extends BaseManager { LogUtil.log(TAG, "相机未连接"); } } + //设置成 public void setCameraExposureModePROGRAM() { Boolean cameraConnect = KeyManager.getInstance().getValue(KeyTools.createKey(CameraKey. @@ -1199,6 +1254,7 @@ public class FlightManager extends BaseManager { isGimbalReset = true; } } + // 检查是否满足开始视觉识别降落的条件 private void checkAndStartVisionLanding() { boolean shouldStartVisionLanding = (PreferenceUtils.getInstance().getLandType() == 2); @@ -1208,6 +1264,7 @@ public class FlightManager extends BaseManager { checkLandingConditions(); } } + private static final double FLYING_HEIGHT_THRESHOLD_MAX = 20; private static final double FLYING_HEIGHT_THRESHOLD_MAX_ALTERNATE = 10; private static final double FLYING_HEIGHT_THRESHOLD_MIN = -2; @@ -1227,7 +1284,7 @@ public class FlightManager extends BaseManager { // 节流打印相对高度和超声波高度(约每秒1次) if (++heightLogCounter >= HEIGHT_LOG_INTERVAL) { - LogUtil.log(TAG, "[高度监控] 相对高度=" + Movement.getInstance().getElevation() + "m, 超声波高度=" + Movement.getInstance().getUltrasonicHeight() + "dm"+"限高"+KeyManager.getInstance().getValue(KeyTools.createKey(FlightControllerKey.KeyHeightLimitRange))); + LogUtil.log(TAG, "[高度监控] 相对高度=" + Movement.getInstance().getElevation() + "m, 超声波高度=" + Movement.getInstance().getUltrasonicHeight() + "dm" + "限高" + KeyManager.getInstance().getValue(KeyTools.createKey(FlightControllerKey.KeyHeightLimitRange))); heightLogCounter = 0; } @@ -1261,6 +1318,11 @@ public class FlightManager extends BaseManager { //当电池电量大于40时，允许复降次数设置为10次 isSendDetect = true; + //广角 + new android.os.Handler().postDelayed(() -> { + CameraManager.getInstance().resumeLensToWideISOManual(); + }, 500); + if (Movement.getInstance().getBattery_a_capacity_percent() > 40) { AMSConfig.getInstance().setAlternateLandingTimes(10 + ""); } @@ -1353,7 +1415,7 @@ public class FlightManager extends BaseManager { if (PreferenceUtils.getInstance().getNeedTriggerAlterArucoLand()) { return !isTriggerLanding && (isFlying || isMotorsOn) && AlternateArucoDetect.getInstance().isCanLanding(); } else { - return !isTriggerLanding && (isFlying || isMotorsOn) && (ApronArucoDetect.getInstance().isCanLanding() || ApronArucoDetectPort.getInstance().isCanLanding()|| Aprongim.getInstance().isCanLanding()|| Aprondown.getInstance().isCanLanding()); + return !isTriggerLanding && (isFlying || isMotorsOn) && (ApronArucoDetect.getInstance().isCanLanding() || ApronArucoDetectPort.getInstance().isCanLanding() || ApronArucodownmany.getInstance().isCanLanding() || Aprongim.getInstance().isCanLanding() || Aprondown.getInstance().isCanLanding()); } } @@ -1374,10 +1436,16 @@ public class FlightManager extends BaseManager { sendCloseCabinDoorMsg = false; forceTriggerDetection = false; MainActivity.setStartArucoType(0); + + ApronArucoDetect.getInstance().setCanLanding(false); ApronArucoDetectPort.getInstance().setCanLanding(false); Aprondown.getInstance().setCanLanding(false); Aprongim.getInstance().setCanLanding(false); + ApronArucodownmany.getInstance().setCanLanding(false); + + + // 发布事件，通知其他组件停止Aruco检测 EventBus.getDefault().post(FLAG_STOP_ARUCO); @@ -1386,9 +1454,9 @@ public class FlightManager extends BaseManager { if (Movement.getInstance().isIstakeoffex() == true && !Objects.equals(Movement.getInstance().getTakeoff_status(), "wayline_cancel")) { - if(Movement.getInstance().isAlternate()){ + if (Movement.getInstance().isAlternate()) { Movement.getInstance().setTakeoff_status("wayline_failed"); - }else{ + } else { Movement.getInstance().setTakeoff_status("task_finish"); } new Handler(Looper.getMainLooper()).postDelayed(new Runnable() { @@ -1399,9 +1467,9 @@ public class FlightManager extends BaseManager { }, 1000); sendEvent2Server("一键起飞已发送task_finish", 1); } else if (Movement.getInstance().isIstakeoffex() == true && Objects.equals(Movement.getInstance().getTakeoff_status(), "wayline_cancel")) { - if(Movement.getInstance().isAlternate()){ + if (Movement.getInstance().isAlternate()) { Movement.getInstance().setTakeoff_status("wayline_failed"); - }else{ + } else { Movement.getInstance().setTakeoff_status("wayline_failed"); } new Handler(Looper.getMainLooper()).postDelayed(new Runnable() { @@ -1440,7 +1508,7 @@ public class FlightManager extends BaseManager { //Movement.getInstance().setMissionFinish(true); // }, 1000); //原有那个替换 - if(PreferenceUtils.getInstance().getMissionType()==0){ + if (PreferenceUtils.getInstance().getMissionType() == 0) { Movement.getInstance().setMissionFinish1(true); } @@ -1485,8 +1553,7 @@ public class FlightManager extends BaseManager { } - - },1000); + }, 1000); } diff --git a/app/src/main/java/com/aros/apron/manager/GimbalManager.java b/app/src/main/java/com/aros/apron/manager/GimbalManager.java index eb8b8075..a87b47b1 100644 --- a/app/src/main/java/com/aros/apron/manager/GimbalManager.java +++ b/app/src/main/java/com/aros/apron/manager/GimbalManager.java @@ -16,6 +16,7 @@ import com.aros.apron.entity.Movement; import com.aros.apron.mix.Aprongim; import com.aros.apron.tools.ApronArucoDetect; import com.aros.apron.tools.ApronArucoDetectPort; +import com.aros.apron.tools.ApronArucodownmany; import com.aros.apron.tools.LogUtil; import com.aros.apron.tools.PreferenceUtils; import com.google.gson.Gson; @@ -61,6 +62,7 @@ public class GimbalManager extends BaseManager { public void initGimbalInfo() { ApronArucoDetect.getInstance().setDoublePayload(PreferenceUtils.getInstance().getCameraLocationType() == 2); + ApronArucodownmany.getInstance().setDoublePayload(PreferenceUtils.getInstance().getCameraLocationType() == 2); ApronArucoDetectPort.getInstance().setDoublePayload(PreferenceUtils.getInstance().getCameraLocationType() == 2); diff --git a/app/src/main/java/com/aros/apron/manager/MissionV3Manager.java b/app/src/main/java/com/aros/apron/manager/MissionV3Manager.java index dfc3b70c..49fe4078 100644 --- a/app/src/main/java/com/aros/apron/manager/MissionV3Manager.java +++ b/app/src/main/java/com/aros/apron/manager/MissionV3Manager.java @@ -7,6 +7,7 @@ import android.os.Environment; import android.os.Handler; import android.os.Looper; import android.text.TextUtils; +import android.util.Log; import androidx.annotation.NonNull; import androidx.annotation.Nullable; diff --git a/app/src/main/java/com/aros/apron/manager/PayloadWidgetManager.java b/app/src/main/java/com/aros/apron/manager/PayloadWidgetManager.java index 04fe4a5f..3c2c8b3b 100644 --- a/app/src/main/java/com/aros/apron/manager/PayloadWidgetManager.java +++ b/app/src/main/java/com/aros/apron/manager/PayloadWidgetManager.java @@ -78,11 +78,12 @@ public class PayloadWidgetManager extends BaseManager { if (payloadManager != null) { IPayloadManager iPayloadManager = payloadManager.get(PayloadIndexType.PORT_2); if (iPayloadManager != null) { + LogUtil.log(TAG,"左侧负载基础信息注册成功"); //基础信息 PayloadCenter.getInstance().getPayloadManager().get(PayloadIndexType.PORT_2).addPayloadBasicInfoListener(new PayloadBasicInfoListener() { @Override public void onPayloadBasicInfoUpdate(PayloadBasicInfo info) { - //LogUtil.log(TAG, "左侧负载基础信息:" + info.toString()); + // LogUtil.log(TAG, "左侧负载基础信息:" + info.toString()); // XcFileLog.getInstace().w(TAG, "左侧负载基础信息:" + info.toString()); //cachedBasicInfo = info; //startPsdkWidgetReport(); @@ -91,9 +92,11 @@ public class PayloadWidgetManager extends BaseManager { PayloadCenter.getInstance().getPayloadManager().get(PayloadIndexType.PORT_2).addPayloadDataListener(new PayloadDataListener() { @Override public void onDataFromPayloadUpdate(byte[] data) { + String cleanData = new String(data, java.nio.charset.StandardCharsets.UTF_8) .replaceAll("[^\\x20-\\x7E\\u4e00-\\u9fa5]", ""); - // LogUtil.log(TAG, "左侧负载数据信息:" + cleanData); + LogUtil.log(TAG, "左侧负载数据信息:" + cleanData); + // XcFileLog.getInstace().w(TAG, "左侧负载数据信息:" + cleanData); diff --git a/app/src/main/java/com/aros/apron/manager/PayloadlightManager.java b/app/src/main/java/com/aros/apron/manager/PayloadlightManager.java index 7fdef1cc..989326b0 100644 --- a/app/src/main/java/com/aros/apron/manager/PayloadlightManager.java +++ b/app/src/main/java/com/aros/apron/manager/PayloadlightManager.java @@ -106,7 +106,7 @@ public class PayloadlightManager extends BaseManager { //关闭 widgetValue.setValue(0); - widgetValue.setIndex(3); + widgetValue.setIndex(2); widgetValue.setType(WidgetType.SWITCH); iPayloadManager.setWidgetValue(widgetValue, new CommonCallbacks.CompletionCallback() { diff --git a/app/src/main/java/com/aros/apron/manager/WayLineExecutingInterruptManager.java b/app/src/main/java/com/aros/apron/manager/WayLineExecutingInterruptManager.java index 2cc38016..b7c1bf8e 100644 --- a/app/src/main/java/com/aros/apron/manager/WayLineExecutingInterruptManager.java +++ b/app/src/main/java/com/aros/apron/manager/WayLineExecutingInterruptManager.java @@ -58,7 +58,7 @@ public class WayLineExecutingInterruptManager extends BaseManager { } }); - if (Movement.getInstance().getElevation() < 90) { + if (Movement.getInstance().getElevation() < 100) { raiseTheReturnFlight(); sendEvent2Server( "航线中断:拉高后返航"+ Movement.getInstance().getElevation(),2); } else { diff --git a/app/src/main/java/com/aros/apron/tools/AprilTagPort.java b/app/src/main/java/com/aros/apron/tools/AprilTagPort.java new file mode 100644 index 00000000..bf43969f --- /dev/null +++ b/app/src/main/java/com/aros/apron/tools/AprilTagPort.java @@ -0,0 +1,446 @@ +package com.aros.apron.tools; + +import android.os.Handler; +import android.os.Looper; + +import com.aros.apron.constant.AMSConfig; +import com.aros.apron.entity.Movement; +import com.aros.apron.manager.AlternateLandingManager; +import com.example.nativec.ApriltagDetector; +import com.example.nativec.ApriltagDetection; +import com.example.nativec.ApriltagPose; +import com.example.nativec.DetectionResult; + +import java.util.List; +import java.util.concurrent.Executors; +import java.util.concurrent.Future; +import java.util.concurrent.ScheduledExecutorService; +import java.util.concurrent.TimeUnit; + +/** + * AprilTag 视觉降落桥接类（v1.0）。 + * + *

与 Aruco 方案的区别

+ *
    + *
  • AprilTag 直接输出3D位姿（米），无需像素→米的经验映射
    • + *
  • 解码带纠错，对光照/模糊更鲁棒
    • + *
  • 支持多 tag 同时检测，自动选最优
    • + *
+ * + *

使用

+ * 
{@code
+ *   // 在相机帧回调里调用
+ *   AprilTagPort.getInstance().processFrame(yuvData, width, height);
+ * }
+ */ +public class AprilTagPort { + + private static final String TAG = "AprilTagPort"; + + // ========== 统一 PID 控制参数（高阻尼防过冲，速度够用） ========== + private static final float PID_XY_KP = 0.5f; + private static final float PID_XY_KI = 0.015f; + private static final float PID_XY_KD = 0.15f; // 降阻尼，防噪声放大 + private static final float PID_XY_IMAX = 0.25f; + private static final float MAX_HORIZ_SPEED = 1.2f; // 水平 m/s + private static final float MAX_YAW_RATE = 25.0f; // 旋转 °/s + private static final float MAX_DESCENT = -0.5f; // 最快下降 m/s + private static final float MIN_DESCENT = -0.05f; // 最慢下降 m/s + // 位姿误差阈值 + private static final double POSE_ERROR_THRESHOLD = 0.05; + + // ========== 状态 ========== + private volatile boolean isProcessing; + private boolean isStartAprilTag; + private boolean startFastStick; + private boolean canLanding; + private boolean aprilTagNotFound; + private long lostStartTime; + private int dropTimes; + private boolean dropTimesTag; + private int frameCounter; + + // ========== 统一 PID 控制器 + 平滑滤波器 ========== + private PIDControl pidX; + private PIDControl pidY; + private double smoothX, smoothY, smoothZ, smoothYaw; // EMA平滑后的位姿 + private boolean poseInitialized; // 首次初始化标志 + private static final double EMA_ALPHA = 0.3; // 平滑系数 (0=最强, 1=不过滤) + ScheduledExecutorService executor = Executors.newScheduledThreadPool(1); + Future lastFuture; + + // ========== 降落触发后速降 ========== + private int fastDescendCount; + private Handler handler = new Handler(Looper.getMainLooper()); + private Runnable fastDescendRunnable; + + // ========== 单例 ========== + private AprilTagPort() {} + + private static class Holder { + private static final AprilTagPort INSTANCE = new AprilTagPort(); + static { INSTANCE.init(); } + } + + public static AprilTagPort getInstance() { + return Holder.INSTANCE; + } + + // ========== 初始化 ========== + private void init() { + pidX = new PIDControl(PID_XY_KP, PID_XY_KI, PID_XY_KD, PID_XY_IMAX, 2.5f, 0.1f); + pidY = new PIDControl(PID_XY_KP, PID_XY_KI, PID_XY_KD, PID_XY_IMAX, 2.5f, 0.1f); + pidX.reset(); + pidY.reset(); + } + + // ========== 公开方法 ========== + + public boolean isProcessing() { return isProcessing; } + public boolean isStartAprilTag() { return isStartAprilTag; } + public void setStartAprilTag(boolean v) { isStartAprilTag = v; } + public boolean isCanLanding() { return canLanding; } + public void setCanLanding(boolean v) { + canLanding = v; + lostStartTime = 0; + } + public boolean isStartFastStick() { return startFastStick; } + public void setStartFastStick(boolean v) { startFastStick = v; } + + /** + * 重置状态，准备下一次降落。 + */ + public void reset() { + startFastStick = false; + isStartAprilTag = false; + canLanding = false; + aprilTagNotFound = false; + lostStartTime = 0; + dropTimes = 0; + dropTimesTag = false; + frameCounter = 0; + fastDescendCount = 0; + pidX.reset(); + pidY.reset(); + poseInitialized = false; + smoothX = 0; smoothY = 0; smoothZ = 0; smoothYaw = 0; + if (handler != null && fastDescendRunnable != null) { + handler.removeCallbacks(fastDescendRunnable); + } + } + + // ================================================================ + // 核心：处理一帧 YUV 图像 + // ================================================================ + + /** + * 处理一帧 YUV 图像，检测 AprilTag 并控制无人机。 + * + * @param yuvData NV21 / YUV420_888 帧数据 + * @param width 图像宽度（像素） + * @param height 图像高度（像素） + */ + public void processFrame(byte[] yuvData, int width, int height) { + // 过滤：正在处理中 或 已触发速降 + if (isStartAprilTag || startFastStick) { + return; + } + + // 每3帧处理一次（~10Hz） + int frame = frameCounter++; + if (frame % 2 != 0) { + return; + } + + isStartAprilTag = true; + Movement.getInstance().setVirtualStickEnableReason(2); + + if (lastFuture != null && !lastFuture.isDone()) { + lastFuture.cancel(true); + } + + int ultrasonicHeight = Movement.getInstance().getUltrasonicHeight(); + + lastFuture = executor.schedule(() -> { + try { + isProcessing = true; + + // ── 调用 AprilTag 检测 + 位姿估计 ── + ApriltagDetector detector = ApriltagDetector.getInstance(); + DetectionResult result = detector.process(yuvData, width, height); + + if (!result.hasAnyPose()) { + // 没检测到 tag 或位姿不可用 + handleLostTag(ultrasonicHeight); + isProcessing = false; + isStartAprilTag = false; + return; + } + + // tag 找到了 + aprilTagNotFound = false; + lostStartTime = 0; + dropTimesTag = true; + + // ── 联合 PnP：收集所有 tag 的降落点位姿，加权平均 ── + List allPoses = result.getLandingPoses(); + if (allPoses.isEmpty()) { + allPoses = result.getSinglePoses(); + } + + if (allPoses.isEmpty()) { + handleLostTag(ultrasonicHeight); + isProcessing = false; + isStartAprilTag = false; + return; + } + + // 按 1/error 加权平均所有位姿 + double totalWeight = 0; + double fusedX = 0, fusedY = 0, fusedZ = 0, fusedYaw = 0; + for (ApriltagPose p : allPoses) { + double w = (p.error > 0) ? 1.0 / p.error : 10.0; + totalWeight += w; + fusedX += p.x() * w; + fusedY += p.y() * w; + fusedZ += p.z() * w; + fusedYaw += p.yawDegrees() * w; + } + fusedX /= totalWeight; + fusedY /= totalWeight; + fusedZ /= totalWeight; + fusedYaw /= totalWeight; + + // ── EMA 平滑滤波，防速度乱跳 ── + if (!poseInitialized) { + smoothX = fusedX; smoothY = fusedY; + smoothZ = fusedZ; smoothYaw = fusedYaw; + poseInitialized = true; + } else { + smoothX = EMA_ALPHA * fusedX + (1 - EMA_ALPHA) * smoothX; + smoothY = EMA_ALPHA * fusedY + (1 - EMA_ALPHA) * smoothY; + smoothZ = EMA_ALPHA * fusedZ + (1 - EMA_ALPHA) * smoothZ; + smoothYaw = EMA_ALPHA * fusedYaw + (1 - EMA_ALPHA) * smoothYaw; + } + + // 平均误差 + double avgErr = 0; + for (ApriltagPose p : allPoses) { + avgErr += p.error; + } + avgErr /= allPoses.size(); + + LogUtil.log(TAG, String.format( + "【联合PnP %d tag】raw=(%.3f,%.3f,%.2f,%.1f°) smooth=(%.3f,%.3f,%.2f,%.1f°) err=%.4f ult=%d", + allPoses.size(), fusedX, fusedY, fusedZ, fusedYaw, + smoothX, smoothY, smoothZ, smoothYaw, avgErr, ultrasonicHeight)); + + // 位姿误差过大 → 不可信 + if (avgErr > POSE_ERROR_THRESHOLD) { + LogUtil.log(TAG, "位姿误差过大(" + String.format("%.4f", avgErr) + ")，悬停"); + DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 0f); + isProcessing = false; + isStartAprilTag = false; + return; + } + + // ── 高度：纯AprilTag ── + // （备用：超声波兜底。需要时取消注释） + // double ultraM = ultrasonicHeight / 10.0; + // double altM = (ultrasonicHeight > 0 && ultrasonicHeight <= 50) + // ? ultraM : smoothZ; + double altM = smoothZ; + + // ── 统一 PID：2m以上边转边降，2m以下只平移 ── + double absX = Math.abs(smoothX); + double absY = Math.abs(smoothY); + double absYaw = Math.abs(smoothYaw); + + boolean aboveTwo = altM > 2.0; + boolean yawAligned = absYaw <= 5.0; + + // ── X/Y 水平 PID：动态缩放 ── + double scaleXY = Math.max(altM * 0.6, 0.3); + double normX = smoothX / scaleXY; + double normY = smoothY / scaleXY; + + double speedLimit = MAX_HORIZ_SPEED; + if (altM < 1.0) speedLimit = 0.2; + else if (altM < 2.0) speedLimit = 0.5; + else if (altM < 3.0) speedLimit = 0.8; + float maxV = (float) speedLimit; + + pidX.setInputFilterAll((float) normX); + pidY.setInputFilterAll((float) normY); + float vx = clamp(pidX.get_pid(), maxV); + float vy = clamp(pidY.get_pid(), maxV); + if (absX < 0.02) vx = 0f; + if (absY < 0.02) vy = 0f; + + // ── Yaw：2m以上边转边降，确保到2m时已转好，2m以下不转 ── + float yawRate = 0f; + if (aboveTwo && !yawAligned) { + boolean nearTwo = altM <= 3.0; // 2-3m紧急修正区 + if (nearTwo) { + // 接近2m：全力转，确保到2m时对准 + yawRate = (absYaw > 10.0) ? 40.0f : 8.0f; + } else { + // 3m以上：从容修正 + if (absYaw > 60.0) yawRate = 35.0f; + else if (absYaw > 30.0) yawRate = 25.0f; + else if (absYaw > 15.0) yawRate = 15.0f; + else if (absYaw > 8.0) yawRate = 8.0f; + else yawRate = 5.0f; + } + yawRate = (smoothYaw > 0) ? -yawRate : yawRate; + } + + // ── 垂直速度：一路降到底 ── + float baseDescent = mapHeightToDescent((float) altM); + float alignFactor = 1.0f - clamp01( + (float) Math.max(absX / 0.3, absY / 0.3)); + alignFactor = Math.max(0.15f, alignFactor); + float vz = baseDescent * alignFactor; + + LogUtil.log(TAG, String.format( + "【统一PID】vx=%.3f vy=%.3f yaw=%.1f vz=%.2f | err=(%.3f,%.3f,%.1f°) alt=%.2fm tag=%d", + vx, vy, yawRate, vz, smoothX, smoothY, smoothYaw, altM, allPoses.size())); + + DroneHelper.getInstance().moveVxVyYawrateHeight(vx, vy, yawRate, vz); + + isProcessing = false; + isStartAprilTag = false; + + } catch (Exception e) { + LogUtil.log(TAG, "Exception: " + e.getMessage()); + isProcessing = false; + isStartAprilTag = false; + } + }, 0, TimeUnit.MILLISECONDS); + } + + // ================================================================ + // 工具方法 + // ================================================================ + + /** 限幅到 [-max, max] */ + private static float clamp(float val, float max) { + if (val > max) return max; + if (val < -max) return -max; + return val; + } + + /** 限幅到 [0, 1] */ + private static float clamp01(float val) { + if (val > 1f) return 1f; + if (val < 0f) return 0f; + return val; + } + + /** 高度→基础下降速度：连续函数，真正动态 */ + private static float mapHeightToDescent(float altM) { + // vz = -(altM / 25.0)，线性映射: 0m→0, 50m→-2.0 + float vz = -(altM / 25.0f); + if (vz > MIN_DESCENT) vz = MIN_DESCENT; // 不低于 -0.05 + if (vz < -2.0f) vz = -2.0f; // 不高于 -2.0 + return vz; + } + + // ================================================================ + // 丢失处理 + // ================================================================ + + private void handleLostTag(int ultrasonicHeight) { + if (!aprilTagNotFound) { + lostStartTime = System.currentTimeMillis(); + aprilTagNotFound = true; + } + + long lostDuration = System.currentTimeMillis() - lostStartTime; + double ultraHeightM = ultrasonicHeight / 10.0; + + if (lostDuration > 1000 && lostDuration <= 12000) { + if (ultrasonicHeight <= 20) { + // 低空丢失 → 上升 + LogUtil.log(TAG, "【丢失】低空上升 vz=3.0 lost=" + lostDuration + "ms"); + DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 3f); + + if (dropTimes > Integer.parseInt(AMSConfig.getInstance().getAlternateLandingTimes())) { + LogUtil.log(TAG, "超过复降限制，去备降点"); + AlternateLandingManager.getInstance().startTaskProcess(null); + Movement.getInstance().setAlternate(true); + return; + } + if (dropTimesTag) { + dropTimesTag = false; + dropTimes++; + LogUtil.log(TAG, "复降第" + dropTimes + "次"); + } + } else { + // 高空丢失 → 慢降寻找 + LogUtil.log(TAG, "【丢失】下降寻找 vz=-0.3 lost=" + lostDuration + "ms"); + DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.3f); + } + } else if (lostDuration > 8000) { + LogUtil.log(TAG, "判定未识别到 AprilTag，飞往备降点"); + AlternateLandingManager.getInstance().startTaskProcess(null); + Movement.getInstance().setAlternate(true); + } + } + + // ================================================================ + // 速降逻辑 + // ================================================================ + + private Runnable getFastDescendRunnable() { + return new Runnable() { + @Override + public void run() { + if (fastDescendCount < 18) { + LogUtil.log(TAG, "【速降】第" + fastDescendCount + "次 vz=-6"); + DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -6); + fastDescendCount++; + handler.postDelayed(this, 50); + } else { + // 速降完成 + canLanding = true; + fastDescendCount = 0; + dropTimes = 0; + handler.removeCallbacks(this); + } + } + }; + } + + // ================================================================ + // 调试：检测并打印 tag 信息 + // ================================================================ + + /** + * 调试用：只检测 tag，不控制飞机。 + * 在 logcat 里筛选 "AprilTagPort" 查看输出。 + */ + public void debugDetect(byte[] yuvData, int width, int height) { + ApriltagDetector detector = ApriltagDetector.getInstance(); + if (!detector.isInitialized()) { + LogUtil.log(TAG, "检测器未初始化"); + return; + } + DetectionResult result = detector.process(yuvData, width, height); + LogUtil.log(TAG, "===== 调试帧 " + width + "x" + height + " ====="); + LogUtil.log(TAG, "检测到 " + result.tagCount() + " 个 tag"); + for (ApriltagDetection det : result.getDetections()) { + LogUtil.log(TAG, String.format( + " ID=%d hamming=%d margin=%.1f center=(%.0f,%.0f)", + det.id, det.hamming, det.decisionMargin, det.c[0], det.c[1])); + } + if (result.hasAnyPose()) { + ApriltagPose best = result.getBestLandingPose(); + if (best != null) { + LogUtil.log(TAG, "最佳降落点位姿: " + best.toString()); + } + } else { + LogUtil.log(TAG, "无可用位姿"); + } + } +} diff --git a/app/src/main/java/com/aros/apron/tools/ApronArucodownmany.java b/app/src/main/java/com/aros/apron/tools/ApronArucodownmany.java index ec6d9737..e43d1488 100644 --- a/app/src/main/java/com/aros/apron/tools/ApronArucodownmany.java +++ b/app/src/main/java/com/aros/apron/tools/ApronArucodownmany.java @@ -80,9 +80,9 @@ public class ApronArucodownmany { */ private double getMarkerOffsetX(int id) { switch (id) { - case 7: return 110; // 往右110像素 + case 7: return 120; // 往右110像素 case 8: return 0; // 不动 - case 9: return 110; // 往右110像素 + case 9: return 120; // 往右110像素 case 10: return 0; // 不动 default: return 0; } @@ -96,8 +96,8 @@ public class ApronArucodownmany { switch (id) { case 7: return 0; // 不动 case 8: return 0; // 不动 - case 9: return -110; // 往上110像素 - case 10: return -110; // 往上110像素 + case 9: return -120; // 往上110像素 + case 10: return -120; // 往上110像素 default: return 0; } } @@ -107,11 +107,11 @@ public class ApronArucodownmany { */ private int getMarkerLandErrXThreshold(int id) { switch (id) { - case 7: return 30; - case 8: return 30; - case 9: return 30; - case 10: return 30; - default: return 30; + case 7: return 35; + case 8: return 35; + case 9: return 35; + case 10: return 35; + default: return 35; } } @@ -120,11 +120,11 @@ public class ApronArucodownmany { */ private int getMarkerLandErrYThreshold(int id) { switch (id) { - case 7: return 30; - case 8: return 30; - case 9: return 30; - case 10: return 30; - default: return 30; + case 7: return 40; + case 8: return 40; + case 9: return 40; + case 10: return 40; + default: return 40; } } @@ -596,7 +596,7 @@ public class ApronArucodownmany { } double pixelWidth = calculateDistance(markerPts[0], markerPts[1]); - if (absX < thresholdX && absY < thresholdY && pixelWidth >= 200) { + if (absX < thresholdX && absY < thresholdY && pixelWidth >= 90) { landingCounters[lockedMarkerId]++; } else { landingCounters[lockedMarkerId] = 0; @@ -624,21 +624,15 @@ public class ApronArucodownmany { // ===== 位移修正：只用锁定的码 ===== if (ultHeight <= 4) { - // 低于4dm：温柔对准，不下降 - float vx = 0f, vy = 0f; + // 低于4dm：PID连续控制（跟4-20dm和6号一致），避免离散台阶冲过头 + pidControlX.setInputFilterAll((float) (errX / 1750.0)); + pidControlY.setInputFilterAll((float) (-errY / 1750.0)); - if (absX > 120) vx = errX > 0 ? 0.135f : -0.135f; - else if (absX > 80) vx = errX > 0 ? 0.09f : -0.09f; - else if (absX > 60) vx = errX > 0 ? 0.07f : -0.07f; - else if (absX > 30) vx = errX > 0 ? 0.05f : -0.05f; - - if (absY > 120) vy = errY > 0 ? 0.135f : -0.135f; - else if (absY > 80) vy = errY > 0 ? 0.09f : -0.09f; - else if (absY > 60) vy = errY > 0 ? 0.07f : -0.07f; - else if (absY > 30) vy = errY > 0 ? 0.05f : -0.05f; + float vx = (float) Math.max(-0.135, Math.min(0.135, pidControlX.get_pid())); + float vy = (float) Math.max(-0.135, Math.min(0.135, pidControlY.get_pid())); DroneHelper.getInstance().moveVxVyYawrateHeight(vx, vy, 0f, 0f); - LogUtil.log(TAG_LOG, "【执行移动】锁定ID=" + lockedMarkerId + " vx=" + vx + " vy=" + vy); + LogUtil.log(TAG_LOG, "【执行移动PID】锁定ID=" + lockedMarkerId + " vx=" + vx + " vy=" + vy); return; } @@ -932,7 +926,7 @@ public class ApronArucodownmany { Point screenCenter = new Point(imageWidth / 2.0, imageHeight / 2.0); double errX = (cx - screenCenter.x); - double errY = (cy - screenCenter.y); + double errY = (cy - screenCenter.y)+50; int currentHeight = Movement.getInstance().getUltrasonicHeight(); double scaleFactor; diff --git a/app/src/main/java/com/example/nativec/ApriltagDetector.java b/app/src/main/java/com/example/nativec/ApriltagDetector.java new file mode 100644 index 00000000..b7608f4a --- /dev/null +++ b/app/src/main/java/com/example/nativec/ApriltagDetector.java @@ -0,0 +1,334 @@ +package com.example.nativec; + +import android.util.Log; + +import com.aros.apron.tools.LogUtil; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.Map; + +/** + * AprilTag 检测器 — JNI 的完整 Java 封装。 + * + *

设计原则

+ *
    + *
  • 纯工具类，不依赖 Android UI，任何线程可用
    • + *
  • 管理 JNI 生命周期（init → process → destroy）
    • + *
  • 线程安全：init/destroy 加锁，process 支持并发读
    • + *
  • 相机内参、tag 尺寸在 Java 端配置，JNI 只管算法
    • + *
  • 畸变校正由调用方在上游处理（JNI 输入已校正图像）
    • + *
+ * + *

使用

+ * 
{@code
+ *   ApriltagDetector d = ApriltagDetector.getInstance();
+ *   d.setCameraParams(800, 800, 320, 240);
+ *   d.setTagSize(0.16);  // 16cm tag
+ *   d.init("tag36h11", 2, 2.0, 0.0, 4);
+ *
+ *   // 每帧
+ *   DetectionResult r = d.process(yuvFrame, 640, 480);
+ *   ApriltagPose pose = r.getBestLandingPose();
+ *
+ *   // 退出
+ *   d.destroy();
+ * }
+ */ +public class ApriltagDetector { + + private static final String TAG = "ApriltagDetector"; + + private static volatile ApriltagDetector instance; + + /** 获取单例 */ + public static ApriltagDetector getInstance() { + if (instance == null) { + synchronized (ApriltagDetector.class) { + if (instance == null) { + instance = new ApriltagDetector(); + } + } + } + return instance; + } + + private ApriltagDetector() {} // 禁止外部 new + + // ── 相机内参 ── + private double fx = 800, fy = 800, cx = 320, cy = 240; + + // ── 标签参数 ── + private String tagFamily = "tag36h11"; + private int errorBits = 2; + private double decimate = 2.0; + private double sigma = 0.0; + private int nthreads = 4; + private double defaultTagSize = 0.16; // 默认标签尺寸（米） + private Map tagSizeMap; // 不同 ID 不同尺寸（可选） + private Map tagOffsetMap; // 不同 ID 离降落点的偏移（可选） + + // ── 状态 ── + private boolean initialized = false; + private final Object lock = new Object(); + + // ── 统计 ── + private long frameCount = 0; + private long totalDetectTimeMs = 0; + + // ================================================================ + // 配置（init 之前设置） + // ================================================================ + + /** + * 设置相机内参（必须）。 + * 从 Android CameraCharacteristics.LENS_INTRINSIC_CALIBRATION 获取。 + * + * @param fx x 方向焦距（像素） + * @param fy y 方向焦距（像素） + * @param cx 主点 x（像素），一般为 imageWidth/2 + * @param cy 主点 y（像素），一般为 imageHeight/2 + */ + public void setCameraParams(double fx, double fy, double cx, double cy) { + this.fx = fx; this.fy = fy; + this.cx = cx; this.cy = cy; + } + + /** 获取相机内参（数组 [fx, fy, cx, cy]） */ + public double[] getCameraParams() { + return new double[]{fx, fy, cx, cy}; + } + + /** + * 设置默认标签物理尺寸（所有 tag 一样大时用这个）。 + * + * @param meters 标签边长（米），例如 0.16 表示 16cm + */ + public void setTagSize(double meters) { + this.defaultTagSize = meters; + } + + /** + * 为不同 ID 设置不同物理尺寸。 + * 平台上有大小 tag 混用时才需要，没设的 ID 会 fallback 到 {@link #setTagSize}。 + * + * 
{@code
+     *   Map sizes = new HashMap<>();
+     *   sizes.put(0, 0.16);  // tag0 是 16cm
+     *   sizes.put(1, 0.08);  // tag1 是 8cm
+     *   sizes.put(2, 0.08);
+     *   detector.setTagSizeMap(sizes);
+     * }
+ */ + public void setTagSizeMap(Map map) { + this.tagSizeMap = (map != null) ? new HashMap<>(map) : null; + } + + /** 查询某个 tag ID 的实际尺寸（米） */ + public double getTagSize(int tagId) { + if (tagSizeMap != null && tagSizeMap.containsKey(tagId)) { + return tagSizeMap.get(tagId); + } + return defaultTagSize; + } + + /** 获取默认尺寸（米） */ + public double getDefaultTagSize() { return defaultTagSize; } + + /** + * 设置每个 tag 离降落点的物理偏移。 + * + * 坐标系：X 右、Y 前（tag 到降落点的距离）。 + * 例如 tag 在降落点左 5cm、后 10cm → put(id, new double[]{-0.05, 0.10}) + * + * 设了之后，DetectionResult 里的 getLandingPose() 会自动算好， + * 不用再手动加减。 + */ + public void setTagOffsets(Map offsets) { + this.tagOffsetMap = (offsets != null) ? new HashMap<>(offsets) : null; + } + + /** 查某个 tag 的偏移量，没设返回 (0, 0) */ + public double[] getTagOffset(int tagId) { + if (tagOffsetMap != null && tagOffsetMap.containsKey(tagId)) { + return tagOffsetMap.get(tagId); + } + return new double[]{0.0, 0.0}; + } + + /** + * 修改检测参数（可在 init 之前或之后，下次 init 生效）。 + */ + public void setDetectParams(String tagFamily, int errorBits, + double decimate, double sigma, int nthreads) { + this.tagFamily = tagFamily; + this.errorBits = errorBits; + this.decimate = decimate; + this.sigma = sigma; + this.nthreads = nthreads; + } + + // ================================================================ + // 生命周期 + // ================================================================ + + /** + * 初始化 C 层检测器。 + * 可重复调用以切换参数（内部自动销毁旧的）。 + * + * @throws IllegalStateException 库加载失败 + * @throws IllegalArgumentException 参数非法 + */ + public boolean init() { + return init(tagFamily, errorBits, decimate, sigma, nthreads); + } + + public boolean init(String family, int errBits, double dec, + double sig, int threads) { + synchronized (lock) { + this.tagFamily = family; + this.errorBits = errBits; + this.decimate = dec; + this.sigma = sig; + this.nthreads = threads; + + boolean ok = ApriltagNative.apriltag_init(family, errBits, dec, sig, threads); + if (ok) { + initialized = true; + LogUtil.log(TAG, String.format( + "init OK: %s, dec=%.1f, sigma=%.1f, threads=%d, defaultTagSize=%.2fm, " + + "cam=(%.0f,%.0f,%.0f,%.0f)", + family, dec, sig, threads, defaultTagSize, fx, fy, cx, cy)); + } else { + initialized = false; + LogUtil.log(TAG, "init FAILED: " + family); + } + return ok; + } + } + + /** 释放 native 资源 */ + public void destroy() { + synchronized (lock) { + ApriltagNative.destroy(); + initialized = false; + Log.i(TAG, "destroyed"); + } + } + + public boolean isInitialized() { + synchronized (lock) { + return initialized && ApriltagNative.isDetectorReady(); + } + } + + // ================================================================ + // 核心：每帧处理 + // ================================================================ + + /** + * 处理一帧 NV21 YUV 图像：检测 + 所有 tag 位姿估计。 + * + * @param yuvData NV21 格式帧数据 + * @param width 图像宽度 + * @param height 图像高度 + * @return 完整结果（检测列表 + 各 tag 位姿），不会 null + */ + public DetectionResult process(byte[] yuvData, int width, int height) { + long t0 = System.currentTimeMillis(); + + // ── 检测 ── + ArrayList detections; + synchronized (lock) { + if (!initialized) { + Log.w(TAG, "process called before init"); + return new DetectionResult(new ArrayList<>(), width, height); + } + detections = ApriltagNative.apriltag_detect_yuv(yuvData, width, height); + } + + DetectionResult result = new DetectionResult(detections, width, height); + + // ── 对每个检测结果做单标签位姿 + 降落点位姿 ── + for (int i = 0; i < detections.size(); i++) { + ApriltagDetection det = detections.get(i); + try { + double size = getTagSize(det.id); + ApriltagPose pose = ApriltagNative.estimate_pose( + det, size, fx, fy, cx, cy); + result.setSinglePose(i, pose); + + // tag 位姿 + 偏移 = 降落点位姿 + double[] off = getTagOffset(det.id); + result.setLandingPose(i, pose.toLandingPose(off[0], off[1])); + + } catch (Exception e) { + Log.w(TAG, "pose estimation failed for tag id=" + det.id + + ": " + e.getMessage()); + } + } + + // ── 统计 ── + long elapsed = System.currentTimeMillis() - t0; + synchronized (lock) { + frameCount++; + totalDetectTimeMs += elapsed; + } + + return result; + } + + // ================================================================ + // 便捷方法 + // ================================================================ + + /** + * 快速处理一帧，只取最佳位姿（不包含检测列表）。 + * + * @return 位姿，或 null（无可用的） + */ + public ApriltagPose processFast(byte[] yuvData, int width, int height) { + DetectionResult r = process(yuvData, width, height); + return r.getBestPose(); + } + + /** + * 处理一帧，同时写入调试信息到 StringBuilder。 + */ + public DetectionResult processDebug(byte[] yuvData, int width, int height, + StringBuilder debug) { + DetectionResult r = process(yuvData, width, height); + debug.append("tags=").append(r.tagCount()); + debug.append(", jointPose=").append(r.hasJointPose() ? "Y" : "N"); + debug.append(", bestSingle=").append(r.getBestSinglePose() != null ? "Y" : "N"); + ApriltagPose best = r.getBestPose(); + if (best != null) { + debug.append(String.format(", t=(%.3f,%.3f,%.3f) err=%.4f", + best.x(), best.y(), best.z(), best.error)); + } + return r; + } + + // ================================================================ + // 信息 + // ================================================================ + + public String getVersion() { + return ApriltagNative.getVersion(); + } + + public String getFamilyName() { + return ApriltagNative.getFamilyName(); + } + + public long getFrameCount() { + synchronized (lock) { return frameCount; } + } + + /** 平均检测耗时（毫秒） */ + public double getAverageDetectTimeMs() { + synchronized (lock) { + return frameCount > 0 ? (double) totalDetectTimeMs / frameCount : 0; + } + } +}