diff --git a/app/release/output-metadata.json b/app/release/output-metadata.json index e2995943..89616d83 100644 --- a/app/release/output-metadata.json +++ b/app/release/output-metadata.json @@ -16,5 +16,22 @@ "outputFile": "app-release.apk" } ], - "elementType": "File" + "elementType": "File", + "baselineProfiles": [ + { + "minApi": 28, + "maxApi": 30, + "baselineProfiles": [ + "baselineProfiles/1/app-release.dm" + ] + }, + { + "minApi": 31, + "maxApi": 2147483647, + "baselineProfiles": [ + "baselineProfiles/0/app-release.dm" + ] + } + ], + "minSdkVersionForDexing": 24 } \ No newline at end of file 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 b135f06c..67fbf07b 100644 --- a/app/src/main/java/com/aros/apron/activity/MainActivity.kt +++ b/app/src/main/java/com/aros/apron/activity/MainActivity.kt @@ -14,6 +14,8 @@ import android.view.WindowManager import android.widget.Button import android.widget.Toast import android.widget.TextView +import com.aros.apron.tools.ToastUtil +import dji.sdk.keyvalue.key.RemoteControllerKey import androidx.annotation.RequiresApi import androidx.constraintlayout.widget.ConstraintLayout import androidx.core.view.GravityCompat @@ -134,7 +136,7 @@ open class MainActivity : BaseActivity() { var isAppStarted: Boolean = false var streamReceive: Boolean = false - var isscousse: Boolean=false; + var isscousse: Boolean = false; private var instance: MainActivity? = null fun getInstance(): MainActivity? { @@ -653,10 +655,11 @@ open class MainActivity : BaseActivity() { StickManager.getInstance().enableVirtualStick1() } -// btn_test3?.setOnClickListener { -// StreamManager.getInstance().startstream() -// -// } + btn_test3 = findViewById(R.id.btn_test3) + btn_test3?.setOnClickListener { + // DJI按钮方式急停测试 + //FlightManager.getInstance().emergencyHoverByPauseButton(null); + } initClickListener() @@ -819,7 +822,9 @@ open class MainActivity : BaseActivity() { GeoidManager.getInstance().init(this) MissionV3Manager.getInstance().initMissionManager() enableStream() + initFpvStream() + startVtxHeartbeat() SpeakerManager.getInstance().addMegaphoneInfoListener() GimbalManager.getInstance().setmode() @@ -847,15 +852,15 @@ open class MainActivity : BaseActivity() { // 每个 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), + 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), + 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) + 249 to doubleArrayOf(0.267, -0.480) ) ApriltagDetector.getInstance().setTagOffsets(tagOffsetMap) @@ -889,7 +894,7 @@ open class MainActivity : BaseActivity() { 2 -> StreamManager.getInstance().startLiveWithCustom() else -> StreamManager.getInstance().startLiveWithCustom() } - // SimplePortScanner.getInstance().startScan() + // SimplePortScanner.getInstance().startScan() }, 5000) LogUtil.log(TAG, "推流类型:" + PreferenceUtils.getInstance().customStreamType) @@ -944,7 +949,7 @@ open class MainActivity : BaseActivity() { 2 -> StreamManager.getInstance().startLiveWithCustom() else -> StreamManager.getInstance().startLiveWithCustom() } - // SimplePortScanner.getInstance().startScan() + // SimplePortScanner.getInstance().startScan() }, 5000) LogUtil.log(TAG, "推流类型:" + PreferenceUtils.getInstance().customStreamType) @@ -1002,7 +1007,7 @@ open class MainActivity : BaseActivity() { 2 -> StreamManager.getInstance().startLiveWithCustom() else -> StreamManager.getInstance().startLiveWithCustom() } - // SimplePortScanner.getInstance().startScan() + // SimplePortScanner.getInstance().startScan() }, 5000) LogUtil.log(TAG, "推流类型:" + PreferenceUtils.getInstance().customStreamType) @@ -1043,9 +1048,9 @@ open class MainActivity : BaseActivity() { try { Synchronizedstatus.setIsruningframe(true) - if(!isscousse){ - isscousse=true; - LogUtil.log(TAG,"mix视频帧回调了") + if (!isscousse) { + isscousse = true; + LogUtil.log(TAG, "mix视频帧回调了") } if (startArucoType == 1) { @@ -1102,8 +1107,6 @@ open class MainActivity : BaseActivity() { try { Synchronizedstatus.setIsruningframe(true) - - if (startArucoType == 1) { Aprondown.getInstance()?.detectArucoTags( height, @@ -1138,7 +1141,7 @@ open class MainActivity : BaseActivity() { @SuppressLint("SuspiciousIndentation") private fun initFpvStream() { cameraManager.addFrameListener( - ComponentIndexType.PORT_1, + ComponentIndexType.FPV, ICameraStreamManager.FrameFormat.YUV420_888 ) { frameData, _, _, width, height, _ -> Movement.getInstance().isVtx = true @@ -1152,24 +1155,34 @@ open class MainActivity : BaseActivity() { try { Synchronizedstatus.setIsruningframe(true) - if(!isscousse){ - isscousse=true; - LogUtil.log(TAG,"port视频帧回调了") + if (!isscousse) { + isscousse = true; + LogUtil.log(TAG, "port视频帧回调了") } if (startArucoType == 1) { + + ApronArucoDetect.getInstance()?.detectArucoTags( + height, + width, + frameData, + dictionary + ) + + // ApronArucodownmany.getInstance()?.detectArucoTags( // height, // width, // frameData, // dictionary // ) - AprilTagPort.getInstance().processFrame( - frameData, - width, - height - ) + +// AprilTagPort.getInstance().processFrame( +// frameData, +// width, +// height +// ) } else if (startArucoType == 2) { AlternateArucoDetect.getInstance()?.detectArucoTags( height, @@ -1178,12 +1191,23 @@ 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) @@ -1212,9 +1236,9 @@ open class MainActivity : BaseActivity() { try { Synchronizedstatus.setIsruningframe(true) - if(!isscousse){ - isscousse=true; - LogUtil.log(TAG,"fpv视频帧回调了") + if (!isscousse) { + isscousse = true; + LogUtil.log(TAG, "fpv视频帧回调了") } if (startArucoType == 1) { @@ -1300,7 +1324,7 @@ open class MainActivity : BaseActivity() { LogUtil.log(TAG, "取消降落,识别机库二维码") if (PreferenceUtils.getInstance().cameraLocationType == 3) { Handler().postDelayed(Runnable { - if (!ApronArucodownmany.getInstance().isTriggerSuccess) { + if (!AprilTagPort.getInstance().isTriggerSuccess) { LogUtil.log(TAG, "图传异常:飞往备降点") //测试图传丢失 AlternateLandingManager.getInstance().startTaskProcess(null) @@ -1309,7 +1333,10 @@ open class MainActivity : BaseActivity() { } else if (PreferenceUtils.getInstance().cameraLocationType == 4 || PreferenceUtils.getInstance().cameraLocationType == 5) { Handler().postDelayed(Runnable { if (!Aprongim.getInstance().isTriggerSuccess) { - LogUtil.log(TAG, "图传异常:飞往备降点"+ Movement.getInstance().isVtx) + LogUtil.log( + TAG, + "图传异常:飞往备降点" + Movement.getInstance().isVtx + ) //测试图传丢失 AlternateLandingManager.getInstance().startTaskProcess(null) } @@ -1371,7 +1398,7 @@ open class MainActivity : BaseActivity() { LogUtil.log(TAG, "取消降落,识别备降点二维码") if (PreferenceUtils.getInstance().cameraLocationType == 3) { Handler().postDelayed(Runnable { - if (!ApronArucoDetect.getInstance().isTriggerSuccess) { + if (!AprilTagPort.getInstance().isTriggerSuccess) { LogUtil.log(TAG, "图传异常:飞往备降点") //测试图传丢失 AlternateLandingManager.getInstance().startTaskProcess(null) 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 255f59d2..40ce34b8 100644 --- a/app/src/main/java/com/aros/apron/base/BaseManager.java +++ b/app/src/main/java/com/aros/apron/base/BaseManager.java @@ -62,7 +62,7 @@ public abstract class BaseManager { mqttMessage.setQos(1); 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()); + //LogUtil.log(TAG, "回复已提交, tid=" + entity.getTid() + ", msgId=" + token.getMessageId() + ", method=" + entity.getMethod()); } else { LogUtil.log(TAG, "回复失败：mqtt 未连接, tid=" + entity.getTid()); } @@ -237,7 +237,7 @@ public abstract class BaseManager { if( Movement.getInstance().getTask_media_count()!=0){ LogUtil.log(TAG ,"getTask_media_count"+Movement.getInstance().getTask_media_count()); } - LogUtil.log(TAG ,"QWQsendFlightTaskProgress2Server"); + // LogUtil.log(TAG ,"QWQsendFlightTaskProgress2Server"); try { if (MqttManager.getInstance().mqttAndroidClient.isConnected()) { // 必须加 final，否则内部类（回调）里访问不了 @@ -613,7 +613,9 @@ public abstract class BaseManager { public boolean getGimbalAndCameraEnabled() { - if (!PreferenceUtils.getInstance().getNeedTriggerApronArucoLand() && !PreferenceUtils.getInstance().getNeedTriggerAlterArucoLand() && 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/MqttActionCallBack.java b/app/src/main/java/com/aros/apron/callback/MqttActionCallBack.java index 3a4d5765..76fc3552 100644 --- a/app/src/main/java/com/aros/apron/callback/MqttActionCallBack.java +++ b/app/src/main/java/com/aros/apron/callback/MqttActionCallBack.java @@ -5,6 +5,7 @@ import android.os.Looper; import com.aros.apron.constant.AMSConfig; import com.aros.apron.tools.LogUtil; +import com.aros.apron.tools.MqttManager; import com.aros.apron.tools.ToastUtil; import org.eclipse.paho.android.service.MqttAndroidClient; @@ -68,7 +69,11 @@ public class MqttActionCallBack implements IMqttActionListener { @Override public void run() { try { - mqttAndroidClient.connect(options, null, MqttActionCallBack.this); + // 使用MqttManager当前持有的client，避免使用已失效的旧引用导致"Invalid ClientHandle" + MqttAndroidClient currentClient = MqttManager.getInstance().mqttAndroidClient; + if (currentClient != null && !currentClient.isConnected()) { + currentClient.connect(MqttManager.getInstance().mMqttConnectOptions, null, MqttActionCallBack.this); + } } catch (MqttException e) { LogUtil.log(TAG, "mqtt重连异常:" + e.toString()); } 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 8109d648..7bae299e 100644 --- a/app/src/main/java/com/aros/apron/callback/MqttCallBack.java +++ b/app/src/main/java/com/aros/apron/callback/MqttCallBack.java @@ -24,6 +24,7 @@ import com.aros.apron.manager.FlightManager; import com.aros.apron.manager.FlyToPointManager; import com.aros.apron.manager.GimbalManager; import com.aros.apron.manager.MLTEManager; +import com.aros.apron.manager.MediaManager; import com.aros.apron.manager.MissionV3Manager; import com.aros.apron.manager.OSDManager; import com.aros.apron.manager.PayloadlightManager; @@ -249,6 +250,8 @@ public class MqttCallBack extends BaseManager implements MqttCallbackExtended { LogUtil.log(TAG, "收到：服务端响应TaskFail" + jsonString); ApronExecutionStatus.getInstance().setServerReplyTaskFail(true); break; + + case Constant.TAKEOFF_TO_POINT: // //1.检查图传是否连接 // MissionDataBean data = new Gson().fromJson(new Gson().toJson(message.getData()), MissionDataBean.class); @@ -680,6 +683,23 @@ public class MqttCallBack extends BaseManager implements MqttCallbackExtended { case Constant.HANGXIANTEST: MLTEManager.getInstance().test(); break; + case Constant.RESTART_RTSP: + LogUtil.log(TAG, "收到：重启视频推流" + jsonString); + //重启视频推流 + StreamManager.getInstance().restart(message); + break; + case Constant.RTMP_PUSH: + LogUtil.log(TAG, "收到：RTMP推流" + jsonString); + StreamManager.getInstance().startLiveWithRTMP(message); + break; + case Constant.STOP_RTMP: + LogUtil.log(TAG, "收到：停止RTMP推流" + jsonString); + StreamManager.getInstance().stopRTMP(message); + break; + case Constant.FLY_TO_LAND_POINT: + LogUtil.log(TAG,"收到一键备降点任务"+jsonString); + break; + @@ -1036,9 +1056,9 @@ 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())); +// LogUtil.log(TAG, "消息送达确认, msgId=" + token.getMessageId() +// + ", complete=" + token.isComplete() +// + ", topics=" + java.util.Arrays.toString(token.getTopics())); } catch (Exception e) { LogUtil.log(TAG, "deliveryComplete 异常: " + e); } diff --git a/app/src/main/java/com/aros/apron/constant/Constant.java b/app/src/main/java/com/aros/apron/constant/Constant.java index 15c13ef7..ec04fb55 100644 --- a/app/src/main/java/com/aros/apron/constant/Constant.java +++ b/app/src/main/java/com/aros/apron/constant/Constant.java @@ -366,6 +366,23 @@ public class Constant { * 航线测试 */ public static final String HANGXIANTEST="hangxiantest"; + + + public static final String RESTART_RTSP="restart_rtsp"; + + public static final String FLY_TO_LAND_POINT="fly_to_land_point"; + + /** + * RTMP推流 + */ + public static final String RTMP_PUSH="push_rtmp"; + + public static final String STOP_RTMP="stop_rtmp"; + + + + + } diff --git a/app/src/main/java/com/aros/apron/entity/CurrentWayline.java b/app/src/main/java/com/aros/apron/entity/CurrentWayline.java index 4624d20b..acb77d46 100644 --- a/app/src/main/java/com/aros/apron/entity/CurrentWayline.java +++ b/app/src/main/java/com/aros/apron/entity/CurrentWayline.java @@ -2,7 +2,9 @@ package com.aros.apron.entity; import java.util.ArrayList; +import java.util.LinkedList; import java.util.List; +import java.util.Queue; import dji.sdk.wpmz.value.mission.WaylineExecuteWaypoint; import dji.sdk.wpmz.value.mission.WaylineWaypoint; @@ -41,4 +43,46 @@ public class CurrentWayline { public void setRouteWaypoints(List waypoints) { this.routeWaypoints = waypoints; } + + /** 解析后的航点列表（含经纬度、高度、悬停时间、到达/离开状态） */ + private List parsedWaypoints = new ArrayList<>(); + + /** 航点队列，用于FIFO处理：peek=当前目标，poll=处理完成出队 */ + private final Queue waypointQueue = new LinkedList<>(); + + public List getParsedWaypoints() { + return parsedWaypoints; + } + + public void setParsedWaypoints(List parsedWaypoints) { + this.parsedWaypoints = parsedWaypoints; + // 同步初始化队列：清空旧数据，新航点全部入队 + waypointQueue.clear(); + if (parsedWaypoints != null) { + waypointQueue.addAll(parsedWaypoints); + } + } + + /** 获取队列（直接操作，用于peek/poll） */ + public Queue getWaypointQueue() { + return waypointQueue; + } + + /** 查看当前待处理的航点（不出队），队列空返回null */ + public ParsedWaypoint peekNextWaypoint() { + return waypointQueue.peek(); + } + + /** 当前航点处理完成，出队 */ + public void pollNextWaypoint() { + waypointQueue.poll(); + } + + /** 根据航点序号查找 */ + public ParsedWaypoint getParsedWaypoint(int index) { + if (index >= 0 && index < parsedWaypoints.size()) { + return parsedWaypoints.get(index); + } + return null; + } } diff --git a/app/src/main/java/com/aros/apron/entity/MessageDown.java b/app/src/main/java/com/aros/apron/entity/MessageDown.java index f947362a..ea376f19 100644 --- a/app/src/main/java/com/aros/apron/entity/MessageDown.java +++ b/app/src/main/java/com/aros/apron/entity/MessageDown.java @@ -56,6 +56,7 @@ public class MessageDown { public static class Data { private int result; private String url; + private String rtmp_url; private int video_quality; private int ideo_quality; private AlternateLandPoint alternate_land_point; @@ -632,6 +633,14 @@ public class MessageDown { this.url = url; } + public String getRtmp_url() { + return rtmp_url; + } + + public void setRtmp_url(String rtmp_url) { + this.rtmp_url = rtmp_url; + } + public int getVideo_quality() { return video_quality; } diff --git a/app/src/main/java/com/aros/apron/entity/Movement.java b/app/src/main/java/com/aros/apron/entity/Movement.java index ab5c1f17..47182adc 100644 --- a/app/src/main/java/com/aros/apron/entity/Movement.java +++ b/app/src/main/java/com/aros/apron/entity/Movement.java @@ -56,7 +56,11 @@ public class Movement { private int landingPower;//降落电量所需百分比 private int lowBatteryRTHState;//智能低电量返航状态 0未触发智能低电量返航 1触发智能低电量返航，飞行器正在倒计时 2执行智能低电量返航 3智能低电量返航被取消 private String missionName;//当前正在执行的航线名 - private int currentWaypointIndex = 0;//当前航点下标 + private int currentWaypointIndex = 0;//当前航点下标（DJI回调） + private int targetWaypointIndex = 0;//GPS检测目标航点下标（自研） + + + private boolean planeWing;//飞机是否在飞 private boolean isMotorsOn;//电机是否起转 @@ -2525,6 +2529,14 @@ public class Movement { this.currentWaypointIndex = currentWaypointIndex; } + public int getTargetWaypointIndex() { + return targetWaypointIndex; + } + + public void setTargetWaypointIndex(int targetWaypointIndex) { + this.targetWaypointIndex = targetWaypointIndex; + } + public String getMissionName() { return missionName; } diff --git a/app/src/main/java/com/aros/apron/manager/CameraManager.java b/app/src/main/java/com/aros/apron/manager/CameraManager.java index 1179ebc3..064e7df4 100644 --- a/app/src/main/java/com/aros/apron/manager/CameraManager.java +++ b/app/src/main/java/com/aros/apron/manager/CameraManager.java @@ -747,7 +747,6 @@ public class CameraManager extends BaseManager { KeyConnection, ComponentIndexType.PORT_1)); if (isConnect != null && isConnect && getGimbalAndCameraEnabled()) { if (message != null) { - int cameraMode = message.getData().getCamera_mode(); // 新增：如果当前已经是该模式，直接返回成功 if (cameraMode == Movement.getInstance().getCamera_mode()) { @@ -1466,24 +1465,24 @@ public class CameraManager extends BaseManager { @Override public void onSuccess() { // 切换到红外视频源时，自动设置调色盘为铁红色（6） - if (type == 3) { - KeyManager.getInstance().setValue( - KeyTools.createCameraKey(CameraKey.KeyThermalPalette, - ComponentIndexType.PORT_1, - CameraLensType.CAMERA_LENS_THERMAL), - CameraThermalPalette.find(6), - new CommonCallbacks.CompletionCallback() { - @Override - public void onSuccess() { - LogUtil.log(TAG, "红外切换成功，已设置调色盘为铁红"); - } - - @Override - public void onFailure(@NonNull IDJIError error) { - LogUtil.log(TAG, "设置红外调色盘失败:" + getIDJIErrorMsg(error)); - } - }); - } +// if (type == 3) { +// KeyManager.getInstance().setValue( +// KeyTools.createCameraKey(CameraKey.KeyThermalPalette, +// ComponentIndexType.PORT_1, +// CameraLensType.CAMERA_LENS_THERMAL), +// CameraThermalPalette.find(6), +// new CommonCallbacks.CompletionCallback() { +// @Override +// public void onSuccess() { +// LogUtil.log(TAG, "红外切换成功，已设置调色盘为铁红"); +// } +// +// @Override +// public void onFailure(@NonNull IDJIError error) { +// LogUtil.log(TAG, "设置红外调色盘失败:" + getIDJIErrorMsg(error)); +// } +// }); +// } sendMsg2Server(message); } 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 1f7713dd..e8c6fcf8 100644 --- a/app/src/main/java/com/aros/apron/manager/FlightManager.java +++ b/app/src/main/java/com/aros/apron/manager/FlightManager.java @@ -14,8 +14,10 @@ import androidx.annotation.Nullable; import com.aros.apron.base.BaseManager; import com.aros.apron.constant.AMSConfig; import com.aros.apron.entity.ApronExecutionStatus; +import com.aros.apron.entity.CurrentWayline; import com.aros.apron.entity.MessageDown; import com.aros.apron.entity.Movement; +import com.aros.apron.entity.ParsedWaypoint; import com.aros.apron.activity.MainActivity; import com.aros.apron.mix.Aprondown; import com.aros.apron.mix.Aprongim; @@ -47,6 +49,7 @@ import dji.sdk.keyvalue.key.FlightControllerKey; import dji.sdk.keyvalue.key.GimbalKey; import dji.sdk.keyvalue.key.KeyTools; import dji.sdk.keyvalue.key.ProductKey; +import dji.sdk.keyvalue.key.RemoteControllerKey; import dji.sdk.keyvalue.key.RtkMobileStationKey; import dji.sdk.keyvalue.value.camera.CameraExposureCompensation; import dji.sdk.keyvalue.value.camera.CameraExposureMode; @@ -351,9 +354,10 @@ public class FlightManager extends BaseManager { if (newValue.getAltitude() != null) { Movement.getInstance().setElevation(newValue.getAltitude()); Movement.getInstance().setTask_height(newValue.getAltitude()); - } + + double distance = LocationUtils.getDistance(String.valueOf(Movement.getInstance().getHomepoint_longitude()), String.valueOf(Movement.getInstance().getHomepoint_latitude()), String.valueOf(newValue.getLongitude()), @@ -370,6 +374,67 @@ public class FlightManager extends BaseManager { Movement.getInstance().setLatitude(newValue.getLatitude()); Movement.getInstance().setLongitude(newValue.getLongitude()); pushFlightAttitude(); + + + //航点到达/离开检测：队列FIFO，peek队头，处理完poll出队 + // 进入：距离≤1m 且 速度≈0；离开：主动(速度>0.2且距离>1m) 或 兜底(hoverTime+1s) + ParsedWaypoint pw = CurrentWayline.getInstance().peekNextWaypoint(); + if (pw != null) { + final ParsedWaypoint finalPw = pw; + final int targetIdx = finalPw.getIndex(); + + double dist = Gpsdistance.calculateDistance( + newValue.getLatitude(), newValue.getLongitude(), + finalPw.getLatitude(), finalPw.getLongitude()); + double speed = Movement.getInstance().getHorizontal_speed(); + + // 进入：距离≤1m 且 速度为0 且 未到达 → 发"进入"并启动兜底定时器 + if (dist <= 1.0 && Math.abs(speed) < 0.1 && !finalPw.isReached()) { + finalPw.markReached(); + sendWaypointReachOrLeave("0", String.valueOf(targetIdx)); + LogUtil.log(TAG, "到达航点[" + targetIdx + "] 距离=" + dist + "m 速度=" + speed + "m/s 悬停=" + finalPw.getHoverTime() + "s"); + // 兜底：hoverTime+1s 后若仍未离开则强制离开并出队 + new Handler(Looper.getMainLooper()).postDelayed(() -> { + if (!finalPw.isLeft()) { + finalPw.markLeft(); + sendWaypointReachOrLeave("1", String.valueOf(targetIdx)); + LogUtil.log(TAG, "离开航点[" + targetIdx + "] 兜底超时(hoverTime+1s)"); + CurrentWayline.getInstance().pollNextWaypoint(); + Movement.getInstance().setTargetWaypointIndex(targetIdx + 1); + } + }, (finalPw.getHoverTime() + 1) * 1000L); + } + // 主动离开：已到达未离开 且 速度>0.2 且 距离>1m → 发"离开"并出队 + if (finalPw.isReached() && !finalPw.isLeft() && speed > 0.2 && dist > 1.0) { + finalPw.markLeft(); + sendWaypointReachOrLeave("1", String.valueOf(targetIdx)); + LogUtil.log(TAG, "离开航点[" + targetIdx + "] 距离=" + dist + "m 速度=" + speed + "m/s"); + CurrentWayline.getInstance().pollNextWaypoint(); + Movement.getInstance().setTargetWaypointIndex(targetIdx + 1); + } + } + + + + + + + + + + + + + + + + + + + + + + // ========== 判断是否到达FlyTo目标点 ========== double targetLat = Movement.getInstance().getFlyto_target_latitude(); @@ -1288,11 +1353,15 @@ public class FlightManager extends BaseManager { heightLogCounter = 0; } - if (isFlying && (Movement.getInstance().getElevation() < 15 && Movement.getInstance().getUltrasonicHeight() < 50 || forceTriggerDetection) && !isSendDetect) { - double flyingHeight = Movement.getInstance().getElevation(); + // 超声波卡住(≥50dm)时，飞控高度 <5m 也允许触发 + double currElevation = Movement.getInstance().getElevation(); + int currUlt = Movement.getInstance().getUltrasonicHeight(); + boolean altOk = (currElevation < 15 && currUlt < 50) // 正常：两个传感器一致 + || (currElevation < 5); // 兜底：飞控高度够低，跳过超声波 + if (isFlying && (altOk || forceTriggerDetection) && !isSendDetect) { double thresholdMin = triggerToAlternatePoint ? FLYING_HEIGHT_THRESHOLD_MIN_ALTERNATE : FLYING_HEIGHT_THRESHOLD_MIN; - if (flyingHeight > thresholdMin || forceTriggerDetection) { + if (currElevation > thresholdMin || forceTriggerDetection) { boolean shouldTriggerDetection; @@ -1339,7 +1408,7 @@ public class FlightManager extends BaseManager { PreferenceUtils.getInstance().setNeedTriggerAlterArucoLand(false); PreferenceUtils.getInstance().setNeedTriggerApronArucoLand(true); LogUtil.log(TAG, "开始识别机库二维码,椭球高度:" + Movement.getInstance().getElevation() + "米" + "--超声波高度:" + Movement.getInstance().getUltrasonicHeight() + "分米"); - sendEvent2Server("开始视觉降落", 1); + sendEvent2Server("开始视觉降落"+Movement.getInstance().getCapacity_percent(), 1); } //PerceptionManager.getInstance().setPerceptionEnable(false); @@ -1665,22 +1734,28 @@ public class FlightManager extends BaseManager { * 紧急悬停 */ public void emergencyHover(MessageDown message) { - + FlightMode flightMode = KeyManager.getInstance().getValue(createKey(FlightControllerKey.KeyFlightMode)); if (flightMode != null) { switch (flightMode) { case GO_HOME: + KeyManager.getInstance().performAction(createKey(FlightControllerKey.KeyStopGoHome), new CommonCallbacks.CompletionCallbackWithParam() { @Override public void onSuccess(EmptyMsg emptyMsg) { - LogUtil.log(TAG, "紧急悬停，取消返航成功"); + //只有在取消返航时才能显示取消返航失败 + Movement.getInstance().setMode_code(3); + isGimbalReset = false; sendMsg2Server(message); resetAircrftLandingStatus(); } @Override public void onFailure(@NonNull IDJIError error) { + LogUtil.log(TAG, "取消返航执行失败" + error); + //isGimbalReset = false; sendFailMsg2Server(message, "紧急悬停，取消返航失败:" + getIDJIErrorMsg(error)); + resetAircrftLandingStatus(); } }); break; @@ -1770,5 +1845,66 @@ public class FlightManager extends BaseManager { } }); + // ★ 急停：清理视觉降落定时器，解除 isStartAruco 入口 guard，确保下次可重新触发 + ApronArucoDetect.getInstance().stopAndReset(); + ApronArucoDetectPort.getInstance().stopAndReset(); + Aprongim.getInstance().stopAndReset(); + Aprondown.getInstance().stopAndReset(); + ApronArucodownmany.getInstance().stopAndReset(); + + + } + + + +// public void emergencyHoverByPauseButton(MessageDown message) { +// LogUtil.log(TAG, "紧急悬停(DJI按钮方式)：setValue KeyPauseButtonDown=true"); +// KeyManager.getInstance().setValue(createKey(RemoteControllerKey.KeyPauseButtonDown), true, new CommonCallbacks.CompletionCallback() { +// @Override +// public void onSuccess() { +// LogUtil.log(TAG, "紧急悬停(DJI按钮方式) 成功"); +// resetAircrftLandingStatus(); +// } +// +// @Override +// public void onFailure(@NonNull IDJIError error) { +// LogUtil.log(TAG, "紧急悬停(DJI按钮方式) 失败: " + getIDJIErrorMsg(error)); +// } +// }); +// } + + + + + /** + * 发送航点进入/离开到服务端，格式与350demo WaypointEventSender一致。 + * @param state "0"=进入 "1"=离开 + * @param index 航点序号 + */ + private void sendWaypointReachOrLeave(String state, String index) { + try { + if (MqttManager.getInstance().mqttAndroidClient != null + && MqttManager.getInstance().mqttAndroidClient.isConnected()) { + com.google.gson.JsonObject msg = new com.google.gson.JsonObject(); + msg.addProperty("msg_type", 60203); + msg.addProperty("result", 1); + msg.addProperty("waypointActionState", state); + msg.addProperty("waypointIndex", index); + + org.eclipse.paho.client.mqttv3.MqttMessage mqttMessage = + new org.eclipse.paho.client.mqttv3.MqttMessage( + new Gson().toJson(msg).getBytes("UTF-8")); + mqttMessage.setQos(1); + + MqttManager.getInstance().mqttAndroidClient.publish( + AMSConfig.UP_UAV_SERVICES_REPLY, + mqttMessage); + LogUtil.log(TAG, "航点事件发送: " + ("0".equals(state) ? "进入" : "离开") + " index=" + index); + } else { + LogUtil.log(TAG, "航点事件发送失败：MQTT未连接"); + } + } catch (Exception e) { + LogUtil.log(TAG, "航点事件发送异常: " + e); + } } } diff --git a/app/src/main/java/com/aros/apron/manager/LTEManager.java b/app/src/main/java/com/aros/apron/manager/LTEManager.java index 619f4c12..daba296c 100644 --- a/app/src/main/java/com/aros/apron/manager/LTEManager.java +++ b/app/src/main/java/com/aros/apron/manager/LTEManager.java @@ -72,7 +72,7 @@ public class LTEManager extends BaseManager { @Override public void onLTELinkInfoUpdate(LTELinkInfo t1) { if (t1 != null) { - LogUtil.log(TAG, "信号质量" + t1.getLinkQualityLevel()); + // LogUtil.log(TAG, "信号质量" + t1.getLinkQualityLevel()); if(t1.getLinkQualityLevel()!=null){ Movement.getInstance().setQuality_4g(t1.getLinkQualityLevel().getLteGndSingalBar().value()+ ""); Movement.getInstance().setGnd_quality_4g(t1.getLinkQualityLevel().getOcuSyncSignalQualityLevel().value()+ ""); diff --git a/app/src/main/java/com/aros/apron/manager/MediaManager.java b/app/src/main/java/com/aros/apron/manager/MediaManager.java index 3f6e4a86..e52ea034 100644 --- a/app/src/main/java/com/aros/apron/manager/MediaManager.java +++ b/app/src/main/java/com/aros/apron/manager/MediaManager.java @@ -3,6 +3,7 @@ package com.aros.apron.manager; import android.os.Build; import android.os.Environment; import android.os.Handler; +import android.os.Looper; import android.text.TextUtils; import android.util.Log; import androidx.annotation.NonNull; @@ -19,6 +20,7 @@ import com.amazonaws.services.s3.model.ProgressListener; import com.amazonaws.services.s3.model.PutObjectRequest; import com.aros.apron.base.BaseManager; import com.aros.apron.entity.ApronExecutionStatus; +import com.aros.apron.entity.MessageDown; import com.aros.apron.entity.Movement; import com.aros.apron.tools.LogUtil; import com.aros.apron.tools.PreferenceUtils; @@ -36,6 +38,7 @@ import java.util.ArrayList; import java.util.HashSet; import java.util.List; import java.util.Set; +import java.util.concurrent.atomic.AtomicBoolean; import dji.sdk.keyvalue.key.FlightControllerKey; import dji.sdk.keyvalue.key.KeyTools; @@ -62,8 +65,8 @@ import io.reactivex.rxjava3.schedulers.Schedulers; public class MediaManager extends BaseManager { - private final String TAG = "MediaManager"; - private final String mediaFileDir = "/apronPic"; + private final String TAG = "MediaManager"; + private final String mediaFileDir = "/apronPic"; private MediaFileListState mState = null; private List mediaFiles = new ArrayList<>(); @@ -74,6 +77,10 @@ public class MediaManager extends BaseManager { /* ===== 下载失败重试计数 ===== */ private int downloadFailTimes = 0; private static final int MAX_DOWNLOAD_RETRY = 3; + + /* ===== 统一主线程Handler ===== */ + private final Handler mainHandler = new Handler(Looper.getMainLooper()); + private MediaManager() { } @@ -85,289 +92,354 @@ public class MediaManager extends BaseManager { return MediaManagerHolder.INSTANCE; } - public void init() { + public void init() { Boolean isConnect = KeyManager.getInstance().getValue(KeyTools.createKey(FlightControllerKey.KeyConnection)); if (isConnect != null && isConnect) { MediaFileListDataSource source = new MediaFileListDataSource.Builder().setIndexType(ComponentIndexType.PORT_1).build(); MediaDataCenter.getInstance().getMediaManager().setMediaFileDataSource(source); MediaDataCenter.getInstance().getMediaManager().addMediaFileListStateListener(new MediaFileListStateListener() { @Override - public void onUpdate(MediaFileListState mediaFileListState) { - mState = mediaFileListState; - LogUtil.log(TAG, "当前媒体文件状态：" + mediaFileListState.name()); + public void onUpdate(MediaFileListState state) { + mState = state; + LogUtil.log(TAG, "【状态监听】state=" + state.name() + " | isPulling=" + isPulling + " | retryCycle=" + retryCycle); + sendEvent2Server("媒体文件状态"+mState,1); + if (state == MediaFileListState.UP_TO_DATE && isPulling) { + LogUtil.log(TAG, "【状态监听】UP_TO_DATE 到达，开始处理文件列表"); + processFileList(); + } } }); } } + /* ================================================================= + * enablePlayback / 拉列表 — 两层重试：DJI方式(3次) → 自有方式(20次) + * DJI方式：enable → IDLE调pull/UPDATING只等 → 等20s UP_TO_DATE + * 自有方式：enable → 2s后第一次pull → 20s后第二次pull → 检查数据 + * 每步都挂超时兜底，SDK不给回调也不卡死 + * ================================================================= */ + private int enterPlayBackFailTimes; - private boolean isEnablePlayback; - private volatile boolean isPlaybackEnabling; // 防止并发调用 - private Handler playbackTimeoutHandler = new Handler(android.os.Looper.getMainLooper()); - private Runnable playbackTimeoutRunnable = null; - private static final int PLAYBACK_ENABLE_TIMEOUT_MS = 15000; // 进入媒体模式超时 15 秒 + private volatile boolean isPlaybackEnabling; + private static final int PULL_TIMEOUT_S = 20; + private int retryCycle = 0; + private boolean isPulling; + private Runnable pullTimeoutRunnable; + + // ★ 两层重试计数器 + private static final int DJI_MAX_RETRIES = 3; + private static final int OWN_MAX_RETRIES = 20; + private int djiRetries = 0; + private int ownRetries = 0; + private boolean useOwnMethod = false; public void enablePlayback() { + LogUtil.log(TAG, "【enablePlayback】调用 | isPlaybackEnabling=" + isPlaybackEnabling + + " | useOwnMethod=" + useOwnMethod + " | dji=" + djiRetries + "/" + DJI_MAX_RETRIES + + " | own=" + ownRetries + "/" + OWN_MAX_RETRIES); if (isPlaybackEnabling) { - LogUtil.log(TAG, "媒体模式已在启用中，跳过重复调用"); + LogUtil.log(TAG, "【enablePlayback】已在启用中，跳过"); return; } - boolean isFirstEntry = !isPlaybackEnabling; - isPlaybackEnabling = true; - // 停止端口扫描和 RTSP 刷新，关闭 RTSP 推流，确保媒体上传稳定 - // StreamManager.getInstance().stopStreamRefreshTimer(); - //com.aros.apron.tools.SimplePortScanner.getInstance().stopScan(); - StreamManager.getInstance().stopstream(); - - // 仅首次调用时清理状态，重试时保留计数器 - if (isFirstEntry) { - - uploadedFileNames.clear(); - bucketChecked = false; - downloadFailTimes = 0; - enterPlayBackFailTimes = 0; - isEnablePlayback = false; - pullMediaFileListFromCameraFailTimes = 0; - updatingWaitCount = 0; - pullqwq = false; - pullStartTime = System.currentTimeMillis(); - LogUtil.log(TAG, "已清空上传文件集合"); + // ★ DJI方式3次耗尽 → 降级自有方式 + if (!useOwnMethod && djiRetries >= DJI_MAX_RETRIES) { + LogUtil.log(TAG, "【enablePlayback】DJI方式" + DJI_MAX_RETRIES + "次均失败 → 降级自有方式"); + useOwnMethod = true; + ownRetries = 0; } - MediaDataCenter.getInstance().getMediaManager().enable(new CommonCallbacks.CompletionCallback() { - @Override - public void onSuccess() { - LogUtil.log(TAG, "进入媒体模式成功"); - isPlaybackEnabling = false; // 成功后释放锁 - new Handler().postDelayed(new Runnable() { - @Override - public void run() { - MediaFileListDataSource source = new - MediaFileListDataSource.Builder().setIndexType(ComponentIndexType.PORT_1).build(); - MediaDataCenter.getInstance().getMediaManager().setMediaFileDataSource(source); - - new Handler().postDelayed(new Runnable() { - @Override - public void run() { - pullMediaFileListFromCamera(); - } - }, 3000); - } - }, 3000); - } - - @Override - public void onFailure(@NonNull IDJIError idjiError) { - LogUtil.log(TAG, "第" + enterPlayBackFailTimes + "次进入媒体模式失败:" + new Gson().toJson(idjiError)); - if (!isEnablePlayback) { - new Handler().postDelayed(new Runnable() { - @Override - public void run() { - if (enterPlayBackFailTimes < 10) { - enterPlayBackFailTimes++; - isPlaybackEnabling = false; // 释放锁 - LogUtil.log(TAG, "第" + enterPlayBackFailTimes + "次重试进入媒体模式"); - retryEnablePlayback(); - } else { - isPlaybackEnabling = false; // 失败放弃后释放锁 - ApronExecutionStatus.getInstance().setAircraftWaitShutDown(true); - sendEvent2Server("媒体模式进入失败:关机",1); - } - } - }, 1500); - } - } - }); - } - - /** 重试进入媒体模式（不清理状态，保留计数器） */ - private void retryEnablePlayback() { - isPlaybackEnabling = true; - MediaDataCenter.getInstance().getMediaManager().enable(new CommonCallbacks.CompletionCallback() { - @Override - public void onSuccess() { - LogUtil.log(TAG, "进入媒体模式成功（重试）"); - isPlaybackEnabling = false; - new Handler().postDelayed(new Runnable() { - @Override - public void run() { - MediaFileListDataSource source = new - MediaFileListDataSource.Builder().setIndexType(ComponentIndexType.PORT_1).build(); - MediaDataCenter.getInstance().getMediaManager().setMediaFileDataSource(source); - - new Handler().postDelayed(new Runnable() { - @Override - public void run() { - pullMediaFileListFromCamera(); - } - }, 3000); - } - }, 3000); - } - - @Override - public void onFailure(@NonNull IDJIError idjiError) { - LogUtil.log(TAG, "第" + enterPlayBackFailTimes + "次进入媒体模式失败:" + new Gson().toJson(idjiError)); - if (!isEnablePlayback) { - new Handler().postDelayed(new Runnable() { - @Override - public void run() { - if (enterPlayBackFailTimes < 10) { - enterPlayBackFailTimes++; - isPlaybackEnabling = false; - LogUtil.log(TAG, "第" + enterPlayBackFailTimes + "次重试进入媒体模式"); - retryEnablePlayback(); - } else { - isPlaybackEnabling = false; - ApronExecutionStatus.getInstance().setAircraftWaitShutDown(true); - sendEvent2Server("媒体模式进入失败:关机",1); - } - } - }, 1500); - } - } - }); - } - - private int pullMediaFileListFromCameraFailTimes; - - private int updatingWaitCount = 0; - private static final int MAX_UPDATING_WAIT = 15; // 最多等待15秒，无文件时快速跳过 - private boolean pullqwq = false; - private boolean isPullMediaFileListFromCameraSuccess; - private long pullStartTime = 0; // 记录整个拉取流程开始时间 - private static final int MAX_PULL_DURATION = 25; // 整个拉取流程最多25秒，超时强制关机 - - private void pullMediaFileListFromCamera() { - // 全局超时检查：防止状态机异常导致无限循环 - long elapsed = (System.currentTimeMillis() - pullStartTime) / 1000; - if (elapsed >= MAX_PULL_DURATION) { - LogUtil.log(TAG, "拉取流程总耗时 " + elapsed + "s，超时强制关机"); + // ★ 自有方式次数耗尽 → 放弃，发关机兜底 + if (useOwnMethod && ownRetries >= OWN_MAX_RETRIES) { + LogUtil.log(TAG, "【enablePlayback】自有方式" + OWN_MAX_RETRIES + "次也失败，彻底放弃 → 发关机"); + sendEvent2Server("媒体列表拉取彻底失败(DJI3+自有20)", 2); ApronExecutionStatus.getInstance().setAircraftWaitShutDown(true); - sendEvent2Server("媒体文件拉取超时", 2); disablePlayback(); return; } - MediaFileListState currentState = MediaDataCenter.getInstance().getMediaManager().getMediaFileListState(); - LogUtil.log(TAG, "当前状态：" + currentState + "，准备拉取文件列表（已耗时" + elapsed + "s）"); + isPlaybackEnabling = true; - if (currentState == MediaFileListState.IDLE) { - LogUtil.log(TAG, "状态为IDLE，开始拉取文件列表"); - MediaDataCenter.getInstance().getMediaManager().pullMediaFileListFromCamera(new PullMediaFileListParam.Builder().count(-1).build(), new CommonCallbacks.CompletionCallback() { - @Override - public void onSuccess() { - LogUtil.log(TAG, "拉取请求已接受，等待状态变为UP_TO_DATE"); - pullMediaFileListFromCameraFailTimes = 0; - } - - @Override - public void onFailure(@NonNull IDJIError idjiError) { - LogUtil.log(TAG, "拉取请求失败: " + new Gson().toJson(idjiError)); - if (pullMediaFileListFromCameraFailTimes < 5) { - pullMediaFileListFromCameraFailTimes++; - LogUtil.log(TAG, "第" + pullMediaFileListFromCameraFailTimes + "次重试..."); - } else { - ApronExecutionStatus.getInstance().setAircraftWaitShutDown(true); - sendEvent2Server("拉取媒体文件失败",2); - disablePlayback(); - LogUtil.log(TAG, "发送关闭无人机"); - } - } - }); - // 不依赖回调续命，无条件调度下一轮轮询 - new Handler().postDelayed(MediaManager.this::pullMediaFileListFromCamera, 1000); - } else if (currentState == MediaFileListState.UP_TO_DATE) { - // 状态为UP_TO_DATE，获取文件列表数据 - LogUtil.log(TAG, "状态为UP_TO_DATE，获取文件列表数据"); - try { - // 确保获取文件列表数据 - List rawList = MediaDataCenter.getInstance().getMediaManager().getMediaFileListData().getData(); - - // 检查文件列表是否为空 - if (rawList == null || rawList.isEmpty()) { - LogUtil.log(TAG, "文件列表为空，重试拉取"); - // 状态已经是UP_TO_DATE时，空列表可能确实无文件，快速重试2次后放弃 - if (pullMediaFileListFromCameraFailTimes < 2) { - pullMediaFileListFromCameraFailTimes++; - LogUtil.log(TAG, "第" + pullMediaFileListFromCameraFailTimes + "次重试..."); - new Handler().postDelayed(MediaManager.this::pullMediaFileListFromCamera, 3000); - } else { - LogUtil.log(TAG, "UP_TO_DATE状态文件列表持续为空，确认无文件"); - ApronExecutionStatus.getInstance().setAircraftWaitShutDown(true); - sendEvent2Server("拉取媒体文件失败",2); - disablePlayback(); - LogUtil.log(TAG, "发送关闭无人机"); - } - return; - } - - LogUtil.log(TAG, "原始文件列表数量: " + rawList.size()); - - // 过滤已上传文件 - mediaFiles = new ArrayList<>(); - for (MediaFile mf : rawList) { - if (!uploadedFileNames.contains(mf.getFileName())) { - mediaFiles.add(mf); - } else { - LogUtil.log(TAG, "跳过已上传文件: " + mf.getFileName()); - } - } - // 修复：在过滤后设置任务媒体计数 - Movement.getInstance().setTask_media_count(mediaFiles.size()); - LogUtil.log(TAG, "过滤后文件数量: " + mediaFiles.size()); - if(PreferenceUtils.getInstance().getMissionType()==0){ - sendFlightTaskProgress2Server(); - } - - - if (mediaFiles.isEmpty()) { - LogUtil.log(TAG, "所有文件均已上传，直接清理"); - downLoadMediaFileIndex = 0; - // 提前设置关机标志，让 aircraftStoredReply 能立即回复成功 - ApronExecutionStatus.getInstance().setAircraftWaitShutDown(true); - removeAllFiles(); - return; - } - - if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.O) { - pullOriginalMediaFileFromCamera(); - } - } catch (Exception e) { - LogUtil.log(TAG, "获取文件列表数据失败: " + e.getMessage()); - // 发生异常时快速重试2次 - if (pullMediaFileListFromCameraFailTimes < 2) { - pullMediaFileListFromCameraFailTimes++; - LogUtil.log(TAG, "第" + pullMediaFileListFromCameraFailTimes + "次重试..."); - new Handler().postDelayed(MediaManager.this::pullMediaFileListFromCamera, 2000); - } else { - LogUtil.log(TAG, "重试次数达到上限，拉取失败"); - ApronExecutionStatus.getInstance().setAircraftWaitShutDown(true); - sendEvent2Server("拉取媒体文件失败",2); - disablePlayback(); - LogUtil.log(TAG, "发送关闭无人机"); - } - } + if (!useOwnMethod) { + djiRetries++; + LogUtil.log(TAG, "【DJI方式】第" + djiRetries + "/" + DJI_MAX_RETRIES + "次"); } else { - // 其他状态（如UPDATING），等待状态变化，不要重复调用pullMediaFileListFromCamera - LogUtil.log(TAG, "状态为" + currentState + "，等待状态变化... (count=" + updatingWaitCount + ")"); - updatingWaitCount++; + ownRetries++; + LogUtil.log(TAG, "【自有方式】第" + ownRetries + "/" + OWN_MAX_RETRIES + "次"); + } - // 增加超时处理，避免无限等待 - if (updatingWaitCount >= MAX_UPDATING_WAIT) { - LogUtil.log(TAG, "等待状态变化超时，强制关机"); - ApronExecutionStatus.getInstance().setAircraftWaitShutDown(true); - sendEvent2Server("媒体文件状态更新超时",2); - disablePlayback(); - LogUtil.log(TAG, "发送关闭无人机"); - return; + // 首次进入清理状态 + if (!useOwnMethod && djiRetries == 1) { + uploadedFileNames.clear(); + bucketChecked = false; + downloadFailTimes = 0; + enterPlayBackFailTimes = 0; + LogUtil.log(TAG, "【enablePlayback】首次进入，清理状态"); + } + + // ★ 只在推流活跃时才停，避免无流时产生"直播未启动"的无效日志 + if (MediaDataCenter.getInstance().getLiveStreamManager().isStreaming()) { + StreamManager.getInstance().stopstream(); + } + + // ★ SDK enable 可能不给回调 → 15s 超时兜底，防止 isPlaybackEnabling 锁死 + final java.util.concurrent.atomic.AtomicBoolean enableDone = new java.util.concurrent.atomic.AtomicBoolean(false); + Runnable enableTimeout = () -> { + if (enableDone.compareAndSet(false, true)) { + LogUtil.log(TAG, "【enablePlayback】⏰ SDK enable 15s 无回调！强制重置"); + isPlaybackEnabling = false; + retryAfterDisable("SDK enable无回调"); + } + }; + mainHandler.postDelayed(enableTimeout, 15000); + + MediaDataCenter.getInstance().getMediaManager().enable(new CommonCallbacks.CompletionCallback() { + @Override + public void onSuccess() { + if (!enableDone.compareAndSet(false, true)) return; // 超时已触发，忽略 + mainHandler.removeCallbacks(enableTimeout); + LogUtil.log(TAG, "【enablePlayback】SDK enable 成功 | method=" + (useOwnMethod ? "自有" : "DJI")); + isPlaybackEnabling = false; + + mainHandler.postDelayed(() -> { + MediaFileListDataSource source = new MediaFileListDataSource.Builder() + .setIndexType(ComponentIndexType.PORT_1).build(); + MediaDataCenter.getInstance().getMediaManager().setMediaFileDataSource(source); + + if (!useOwnMethod) { + // ★ DJI方式：走状态驱动流程 + LogUtil.log(TAG, "【DJI方式】6s延迟到，开始 pullMediaFileListFromCamera"); + mainHandler.postDelayed(() -> pullMediaFileListFromCamera(), 3000); + } else { + // ★ 自有方式：enable成功后2s就拉，不等状态 + LogUtil.log(TAG, "【自有方式】enable成功，2s后第一次拉取"); + mainHandler.postDelayed(() -> forcePullInOwnMode(), 2000); + } + }, 3000); } - new Handler().postDelayed(MediaManager.this::pullMediaFileListFromCamera, 1000); + @Override + public void onFailure(@NonNull IDJIError e) { + if (!enableDone.compareAndSet(false, true)) return; // 超时已触发，忽略 + mainHandler.removeCallbacks(enableTimeout); + LogUtil.log(TAG, "【enablePlayback】SDK enable 失败: " + e.description() + + " | method=" + (useOwnMethod ? "自有" : "DJI")); + isPlaybackEnabling = false; + retryAfterDisable("enable失败"); + } + }); + } + + /** + * ★ 自有方式：enable 成功 → 2s后第一次拉 → 等20s → 第二次拉 → 检查数据 + * SDK 可能不给任何回调，每步都挂超时兜底 + */ + private void forcePullInOwnMode() { + LogUtil.log(TAG, "【自有方式】2s后第一次拉取 | ownRetries=" + ownRetries); + isPulling = true; + + MediaFileListState state = MediaDataCenter.getInstance().getMediaManager().getMediaFileListState(); + LogUtil.log(TAG, "【自有方式】当前SDK状态=" + state); + + // ★ 第一次拉：不管状态直接调 SDK pull + MediaDataCenter.getInstance().getMediaManager().pullMediaFileListFromCamera( + new PullMediaFileListParam.Builder().count(-1).build(), + new CommonCallbacks.CompletionCallback() { + @Override + public void onSuccess() { + LogUtil.log(TAG, "【自有方式】第一次pull onSuccess → 20s后第二次拉"); + } + @Override + public void onFailure(@NonNull IDJIError e) { + LogUtil.log(TAG, "【自有方式】第一次pull onFailure: " + e.description() + " → 继续等20s"); + } + }); + + // ★ SDK 可能不给回调 → 20s后不管结果直接第二次拉 + mainHandler.postDelayed(() -> { + LogUtil.log(TAG, "【自有方式】20s到，开始第二次拉取"); + MediaDataCenter.getInstance().getMediaManager().pullMediaFileListFromCamera( + new PullMediaFileListParam.Builder().count(-1).build(), + new CommonCallbacks.CompletionCallback() { + @Override + public void onSuccess() { + LogUtil.log(TAG, "【自有方式】第二次pull onSuccess → 检查数据"); + if (isPulling) checkDataInOwnMode(); + } + @Override + public void onFailure(@NonNull IDJIError e) { + LogUtil.log(TAG, "【自有方式】第二次pull onFailure: " + e.description()); + if (isPulling) checkDataInOwnMode(); + } + }); + + // ★ 第二次拉也可能不给回调，再挂 20s 兜底 + mainHandler.postDelayed(() -> { + if (isPulling) { + LogUtil.log(TAG, "【自有方式】第二次pull 20s兜底超时，强制检查数据"); + checkDataInOwnMode(); + } + }, PULL_TIMEOUT_S * 1000L); + }, PULL_TIMEOUT_S * 1000L); + } + + /** + * ★ 自有方式：不管 SDK 状态，直接拿数据尝试处理 + */ + private void checkDataInOwnMode() { + if (!isPulling) return; + isPulling = false; + + try { + List data = MediaDataCenter.getInstance().getMediaManager() + .getMediaFileListData().getData(); + LogUtil.log(TAG, "【自有方式】SDK返回文件数=" + (data != null ? data.size() : 0) + + " | state=" + MediaDataCenter.getInstance().getMediaManager().getMediaFileListState()); + if (data != null && !data.isEmpty()) { + // 拉到了！重置所有计数器，走正常处理流程 + LogUtil.log(TAG, "【自有方式】✅ 成功拉到" + data.size() + "个文件，切回正常流程"); + djiRetries = 0; + ownRetries = 0; + retryCycle = 0; + useOwnMethod = false; + processFileList(); + } else { + LogUtil.log(TAG, "【自有方式】无数据，退出重进"); + retryAfterDisable("自有方式无数据"); + } + } catch (Exception e) { + LogUtil.log(TAG, "【自有方式】获取数据异常: " + e.getMessage()); + retryAfterDisable("自有方式获取数据异常"); } } + private void retryAfterDisable(String reason) { + retryCycle++; + LogUtil.log(TAG, "【retry】" + reason + " → 先disable再enable | method=" + (useOwnMethod ? "自有" : "DJI") + + " | retryCycle=" + retryCycle); + disablePlaybackAndThen(() -> { + LogUtil.log(TAG, "【retry】disable完成，3s后enablePlayback"); + mainHandler.postDelayed(() -> enablePlayback(), 3000); + }); + } + + private void pullMediaFileListFromCamera() { + // ★ 防重复post：先取消旧的超时 + if (pullTimeoutRunnable != null) { + mainHandler.removeCallbacks(pullTimeoutRunnable); + LogUtil.log(TAG, "【pullList】取消旧超时"); + } + isPulling = true; + + // ★ 定义一个统一的超时runnable（SDK不给回调的兜底） + pullTimeoutRunnable = () -> { + LogUtil.log(TAG, "【pullList】⏰ 20s超时触发！当前state=" + + MediaDataCenter.getInstance().getMediaManager().getMediaFileListState() + + " | djiRetries=" + djiRetries + " → 退出重进"); + isPulling = false; + retryAfterDisable("媒体文件拉取超时(SDK无回调)"); + }; + // 启动20s看门狗 + mainHandler.postDelayed(pullTimeoutRunnable, PULL_TIMEOUT_S * 1000L); + LogUtil.log(TAG, "【pullList】20s超时看门狗已启动"); + + MediaFileListState state = MediaDataCenter.getInstance().getMediaManager().getMediaFileListState(); + LogUtil.log(TAG, "【pullList】当前SDK状态=" + state + " | isPulling=" + isPulling + " | djiRetries=" + djiRetries); + + // 状态已是 UP_TO_DATE：数据已就绪，直接处理 + if (state == MediaFileListState.UP_TO_DATE) { + LogUtil.log(TAG, "【pullList】状态已是UP_TO_DATE，数据已就绪，直接处理"); + processFileList(); + return; + } + + // ★ DJI方式：IDLE 才调 pull，UPDATING 只等状态（严格按 DJI 建议） + if (state == MediaFileListState.IDLE) { + LogUtil.log(TAG, "【pullList】状态=IDLE → 调用SDK pullMediaFileList"); + MediaDataCenter.getInstance().getMediaManager().pullMediaFileListFromCamera( + new PullMediaFileListParam.Builder().count(-1).build(), + new CommonCallbacks.CompletionCallback() { + @Override + public void onSuccess() { + if (pullTimeoutRunnable == null) { + LogUtil.log(TAG, "【pullList】SDK pull onSuccess 但数据已处理完毕，忽略"); + return; + } + LogUtil.log(TAG, "【pullList】SDK pull onSuccess → 续时20s，等待UP_TO_DATE"); + mainHandler.removeCallbacks(pullTimeoutRunnable); + mainHandler.postDelayed(pullTimeoutRunnable, PULL_TIMEOUT_S * 1000L); + } + + @Override + public void onFailure(@NonNull IDJIError e) { + LogUtil.log(TAG, "【pullList】SDK pull onFailure: " + e.description() + + " → 继续等状态监听器或超时"); + } + }); + return; + } + + // ★ DJI方式：UPDATING 不主动拉，只等状态监听器通知 UP_TO_DATE 或超时 + LogUtil.log(TAG, "【pullList】状态=" + state + "，等状态监听器通知 UP_TO_DATE 或超时"); + } + + /** 状态监听器回调——SDK 通知 UP_TO_DATE 时处理 */ + private void processFileList() { + LogUtil.log(TAG, "【processFileList】进入 | isPulling=" + isPulling + " | cycle=" + retryCycle); + if (pullTimeoutRunnable != null) { + mainHandler.removeCallbacks(pullTimeoutRunnable); + pullTimeoutRunnable = null; + LogUtil.log(TAG, "【processFileList】超时看门狗已取消"); + } + isPulling = false; + retryCycle = 0; + djiRetries = 0; + ownRetries = 0; + useOwnMethod = false; + + try { + List rawList = MediaDataCenter.getInstance().getMediaManager() + .getMediaFileListData().getData(); + LogUtil.log(TAG, "【processFileList】SDK返回文件数=" + (rawList != null ? rawList.size() : 0)); + if (rawList == null || rawList.isEmpty()) { + LogUtil.log(TAG, "【processFileList】文件列表为空，关机"); + ApronExecutionStatus.getInstance().setAircraftWaitShutDown(true); + sendEvent2Server("无媒体文件", 1); + disablePlayback(); + return; + } + LogUtil.log(TAG, "文件列表数量: " + rawList.size()); + + mediaFiles = new ArrayList<>(); + for (MediaFile mf : rawList) { + if (!uploadedFileNames.contains(mf.getFileName())) { + mediaFiles.add(mf); + } + } + Movement.getInstance().setTask_media_count(mediaFiles.size()); + if (PreferenceUtils.getInstance().getMissionType() == 0) sendFlightTaskProgress2Server(); + + if (mediaFiles.isEmpty()) { + LogUtil.log(TAG, "全部已上传，直接清理"); + ApronExecutionStatus.getInstance().setAircraftWaitShutDown(true); + removeAllFiles(); + return; + } + + if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.O) { + pullOriginalMediaFileFromCamera(); + } + } catch (Exception e) { + LogUtil.log(TAG, "处理文件列表异常: " + e.getMessage()); + ApronExecutionStatus.getInstance().setAircraftWaitShutDown(true); + sendEvent2Server("处理文件列表失败", 2); + disablePlayback(); + } + } + + /* ================================================================= + * 文件下载/上传 — 保持原始逻辑不变 + * ================================================================= */ @RequiresApi(Build.VERSION_CODES.O) public void pullOriginalMediaFileFromCamera() { @@ -379,7 +451,6 @@ public class MediaManager extends BaseManager { downLoadMediaFileIndex++; if (downLoadMediaFileIndex == mediaFiles.size()) { - // This refers to when all files have been downloaded or failed. Clear SD card, cache, exit media mode, and shut down the drone downLoadMediaFileIndex = 0; removeAllFiles(); } else { @@ -445,7 +516,7 @@ public class MediaManager extends BaseManager { } catch (IOException error) { LogUtil.log(TAG, "File " + downLoadMediaFileIndex + " error: " + error.getMessage()); ApronExecutionStatus.getInstance().setAircraftWaitShutDown(true); - sendEvent2Server("文件流关闭失败",2); + sendEvent2Server("文件流关闭失败", 2); disablePlayback(); LogUtil.log(TAG, "发送关闭无人机"); } @@ -474,7 +545,7 @@ public class MediaManager extends BaseManager { }, 2000); } else { ApronExecutionStatus.getInstance().setAircraftWaitShutDown(true); - sendEvent2Server("第" + downLoadMediaFileIndex + "个文件下载失败(已重试" + MAX_DOWNLOAD_RETRY + "次)",2); + sendEvent2Server("第" + downLoadMediaFileIndex + "个文件下载失败(已重试" + MAX_DOWNLOAD_RETRY + "次)", 2); disablePlayback(); LogUtil.log(TAG, "发送关闭无人机"); } @@ -485,7 +556,7 @@ public class MediaManager extends BaseManager { Log.e(TAG, "Error opening file: " + e.getMessage()); // 发生异常时也要确保关机 ApronExecutionStatus.getInstance().setAircraftWaitShutDown(true); - sendEvent2Server("文件打开失败",2); + sendEvent2Server("文件打开失败", 2); disablePlayback(); LogUtil.log(TAG, "发送关闭无人机"); // 关闭文件流 @@ -498,15 +569,19 @@ public class MediaManager extends BaseManager { } } + /* ================================================================= + * MinIO上传 — 原始逻辑不变 + * ================================================================= */ + private AmazonS3 s3 = new AmazonS3Client(new AWSCredentials() { @Override public String getAWSAccessKeyId() { - return PreferenceUtils.getInstance().getAccessKey(); // minio的key + return PreferenceUtils.getInstance().getAccessKey(); } @Override public String getAWSSecretKey() { - return PreferenceUtils.getInstance().getSecretKey(); // minio的密钥 + return PreferenceUtils.getInstance().getSecretKey(); } }, Region.getRegion(Regions.US_EAST_1), new ClientConfiguration() .withConnectionTimeout(30000) @@ -520,7 +595,7 @@ public class MediaManager extends BaseManager { @Override public void subscribe(ObservableEmitter emitter) throws Exception { // 服务器地址 - s3.setEndpoint(PreferenceUtils.getInstance().getUploadUrl()); // http://ip:端口号 + s3.setEndpoint(PreferenceUtils.getInstance().getUploadUrl()); // Bucket只在首次上传时检查创建，后续上传不再重复请求 if (!bucketChecked) { synchronized (this) { @@ -534,17 +609,11 @@ public class MediaManager extends BaseManager { } } - // 上传文件到网关MINIO存储服务 s3.putObject( new PutObjectRequest( PreferenceUtils.getInstance().getBucketName(), "/" + PreferenceUtils.getInstance().getObjectKey() + "/" + mediaFile.getFileName(), file -// new PutObjectRequest( -// PreferenceUtils.getInstance().getBucketName(), -// "/" + PreferenceUtils.getInstance().getObjectKey() + "/" + -// PreferenceUtils.getInstance().getFlightId() + "/" + mediaFile.getFileName(), -// file ).withProgressListener(new ProgressListener() { @Override public void progressChanged(ProgressEvent progressEvent) { @@ -572,7 +641,6 @@ public class MediaManager extends BaseManager { }) ); - // 获取文件上传后访问地址url GeneratePresignedUrlRequest urlRequest = new GeneratePresignedUrlRequest( PreferenceUtils.getInstance().getBucketName(), "/" + PreferenceUtils.getInstance().getObjectKey() + "/" @@ -580,7 +648,6 @@ public class MediaManager extends BaseManager { ); String url = s3.generatePresignedUrl(urlRequest).toString(); - // 文件上传后访问地址url emitter.onNext(url); emitter.onComplete(); } @@ -590,28 +657,23 @@ public class MediaManager extends BaseManager { .subscribe(new Observer() { @Override public void onSubscribe(Disposable d) { - // Handle on subscribe (optional) } @Override public void onNext(String url) { - // 上传成功，重置下载重试计数 downloadFailTimes = 0; - //上传完成发送事件 sendMediaUpload2Server(mediaFile.getFileName(), downLoadMediaFileIndex + 1, mediaFiles.size()); } @RequiresApi(Build.VERSION_CODES.O) @Override public void onError(Throwable e) { - // 每上传失败一张就清除缓存 uploadedFileNames.add(mediaFile.getFileName()); FileUtil.deleteFile(file); LogUtil.log(TAG, "Error uploading file " + downLoadMediaFileIndex + ": " + e.getMessage()); downLoadMediaFileIndex++; if (downLoadMediaFileIndex == mediaFiles.size()) { - // 所有文件已经下载完成或失败，清空SD卡，缓存，退出媒体模式，发送无人机关机 downLoadMediaFileIndex = 0; removeAllFiles(); } else { @@ -622,16 +684,14 @@ public class MediaManager extends BaseManager { @RequiresApi(Build.VERSION_CODES.O) @Override public void onComplete() { - // 每上传一张就清除缓存，并记录已上传文件名 uploadedFileNames.add(mediaFile.getFileName()); FileUtil.deleteFile(file); LogUtil.log(TAG, "File " + downLoadMediaFileIndex + " uploaded successfully."); - sendEvent2Server( "第" + downLoadMediaFileIndex + "个文件已上传",1); + sendEvent2Server("第" + downLoadMediaFileIndex + "个文件已上传", 1); downLoadMediaFileIndex++; if (downLoadMediaFileIndex == mediaFiles.size()) { - // 所有文件已上传完成，清空SD卡，缓存，退出媒体模式，发送无人机关机 - sendEvent2Server( "媒体文件已上传完毕",1); + sendEvent2Server("媒体文件已上传完毕", 1); removeAllFiles(); downLoadMediaFileIndex = 0; } else { @@ -641,12 +701,15 @@ public class MediaManager extends BaseManager { }); } + /* ================================================================= + * 删除文件 / 退出媒体模式 + * ================================================================= */ + public void removeAllFiles() { - // 确保即使没有文件也能正常关机 if (mediaFiles == null || mediaFiles.isEmpty()) { LogUtil.log(TAG, "没有文件需要清除，直接关机"); ApronExecutionStatus.getInstance().setAircraftWaitShutDown(true); - sendEvent2Server("没有媒体文件需要清除",1); + sendEvent2Server("没有媒体文件需要清除", 1); disablePlayback(); LogUtil.log(TAG, "发送关闭无人机"); return; @@ -657,7 +720,7 @@ public class MediaManager extends BaseManager { public void onSuccess() { ApronExecutionStatus.getInstance().setAircraftWaitShutDown(true); LogUtil.log(TAG, "清除文件成功 "); - sendEvent2Server("媒体文件已清除",1); + sendEvent2Server("媒体文件已清除", 1); disablePlayback(); LogUtil.log(TAG, "发送关闭无人机"); } @@ -666,16 +729,13 @@ public class MediaManager extends BaseManager { public void onFailure(@NonNull IDJIError idjiError) { ApronExecutionStatus.getInstance().setAircraftWaitShutDown(true); LogUtil.log(TAG, "清除文件失败: " + new Gson().toJson(idjiError)); - sendEvent2Server("媒体文件清除失败",2); + sendEvent2Server("媒体文件清除失败", 2); LogUtil.log(TAG, "发送关闭无人机"); } }); } - //退出媒体模式 - public void disablePlayback() { - // 任务结束，停止视频流刷新定时器 - // StreamManager.getInstance().stopStreamRefreshTimer(); + public void disablePlayback() { MediaDataCenter.getInstance().getMediaManager().disable(new CommonCallbacks.CompletionCallback() { @Override public void onSuccess() { @@ -684,27 +744,65 @@ public class MediaManager extends BaseManager { @Override public void onFailure(@NonNull IDJIError idjiError) { - LogUtil.log(TAG, "退出媒体模式失败:"+new Gson().toJson(idjiError)); + LogUtil.log(TAG, "退出媒体模式失败:" + new Gson().toJson(idjiError)); } }); } - private int downLoadMediaFileIndex = 0; + private static final long DISABLE_TIMEOUT_MS = 15_000; - private String getSDCardPath(){ - if (checkSDCard()) { - return Environment.getExternalStorageDirectory() - .getPath(); - } else { - return Environment.getExternalStorageDirectory() - .getParentFile().getPath(); - } + /** + * 退出媒体模式 + 超时保护 + 完成后执行后续。 + * disable 正常回调回来就执行；如果SDK不回调用，15s后强制执行，防卡死。 + */ + private void disablePlaybackAndThen(Runnable then) { + LogUtil.log(TAG, "【disablePlayback】开始退出媒体模式（15s超时兜底）"); + final AtomicBoolean done = new AtomicBoolean(false); + + Runnable disableTimeout = () -> { + if (done.compareAndSet(false, true)) { + LogUtil.log(TAG, "【disablePlayback】⏰ 15s超时！SDK disable无响应，强制执行后续"); + then.run(); + } + }; + mainHandler.postDelayed(disableTimeout, DISABLE_TIMEOUT_MS); + + MediaDataCenter.getInstance().getMediaManager().disable(new CommonCallbacks.CompletionCallback() { + @Override + public void onSuccess() { + if (done.compareAndSet(false, true)) { + mainHandler.removeCallbacks(disableTimeout); + LogUtil.log(TAG, "【disablePlayback】SDK disable成功 → 执行后续"); + then.run(); + } + } + + @Override + public void onFailure(@NonNull IDJIError idjiError) { + if (done.compareAndSet(false, true)) { + mainHandler.removeCallbacks(disableTimeout); + LogUtil.log(TAG, "【disablePlayback】SDK disable失败: " + idjiError.description() + " → 仍执行后续"); + then.run(); + } + } + }); } + /* ================================================================= + * 工具方法 + * ================================================================= */ + + private int downLoadMediaFileIndex = 0; + + private String getSDCardPath() { + if (checkSDCard()) { + return Environment.getExternalStorageDirectory().getPath(); + } else { + return Environment.getExternalStorageDirectory().getParentFile().getPath(); + } + } private boolean checkSDCard() { return TextUtils.equals(Environment.MEDIA_MOUNTED, Environment.getExternalStorageState()); } - - } 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 f453c70e..4237aec4 100644 --- a/app/src/main/java/com/aros/apron/manager/MissionV3Manager.java +++ b/app/src/main/java/com/aros/apron/manager/MissionV3Manager.java @@ -18,11 +18,13 @@ import com.aros.apron.constant.ErrorCode; import com.aros.apron.entity.CurrentWayline; import com.aros.apron.entity.MessageDown; import com.aros.apron.entity.Movement; +import com.aros.apron.entity.ParsedWaypoint; import com.aros.apron.entity.Synchronizedstatus; import com.aros.apron.tools.LogUtil; import com.aros.apron.tools.PreferenceUtils; import com.aros.apron.tools.RestartAPPTool; import com.aros.apron.tools.TakeoffProgressScheduler; +import com.aros.apron.tools.Utils; import com.dji.wpmzsdk.common.data.KMZInfo; import com.dji.wpmzsdk.manager.WPMZManager; import com.google.gson.Gson; @@ -33,6 +35,7 @@ import java.io.File; import java.io.FileOutputStream; import java.io.IOException; import java.io.InputStream; +import java.util.ArrayList; import java.util.List; import dji.sdk.keyvalue.key.CameraKey; @@ -821,6 +824,13 @@ public class MissionV3Manager extends BaseManager { if (kmzInfo != null) { // Utils.printJson(TAG,"航点详情:"+new Gson().toJson(kmzInfo)); WaylineWaylinesParseInfo waylineWaylinesParseInfo = kmzInfo.getWaylineWaylinesParseInfo(); + + + + //LogUtil.log(TAG,"航点详情:"+new Gson().toJson(kmzInfo)); + //LogUtil.log(TAG,"航点详情:"+waylineWaylinesParseInfo.getWaylines()); + + //解析航点 if (waylineWaylinesParseInfo != null) { List waylines = waylineWaylinesParseInfo.getWaylines(); if (waylines != null && waylines.size() > 0) { @@ -828,6 +838,95 @@ public class MissionV3Manager extends BaseManager { if (waypoints != null && waypoints.size() > 0) { CurrentWayline.getInstance().setWaypoints(waypoints); LogUtil.log(TAG, "该航线有" + waypoints.size() + "个航点"); + + + // 打印每个航点的详细信息 + // 构建解析后的航点列表 + // 先从航线级 actionGroups 提取每个航点的 hoverTime（startIndex→endIndex） + java.util.Map hoverTimeMap = new java.util.HashMap<>(); + try { + String waylineJson = new Gson().toJson(waylines.get(0)); + com.google.gson.JsonObject waylineObj = new Gson().fromJson(waylineJson, com.google.gson.JsonObject.class); + com.google.gson.JsonArray actionGroups = waylineObj.getAsJsonArray("actionGroups"); + if (actionGroups != null) { + for (int g = 0; g < actionGroups.size(); g++) { + com.google.gson.JsonObject group = actionGroups.get(g).getAsJsonObject(); + int startIdx = group.has("startIndex") ? group.get("startIndex").getAsInt() : 0; + int endIdx = group.has("endIndex") ? group.get("endIndex").getAsInt() : startIdx; + com.google.gson.JsonArray actions = group.getAsJsonArray("actions"); + if (actions != null) { + int totalHover = 0; + for (int a = 0; a < actions.size(); a++) { + com.google.gson.JsonObject action = actions.get(a).getAsJsonObject(); + if (action.has("aircraftHoverParam")) { + com.google.gson.JsonObject hoverParam = action.getAsJsonObject("aircraftHoverParam"); + if (hoverParam != null && hoverParam.has("hoverTime")) { + totalHover += (int) Math.round(hoverParam.get("hoverTime").getAsDouble()); + } + } + } + for (int idx = startIdx; idx <= endIdx && idx < waypoints.size(); idx++) { + hoverTimeMap.put(idx, totalHover); + } + } + } + } + } catch (Exception e) { + LogUtil.log(TAG, "航线级actionGroups解析跳过: " + e.getMessage()); + } + + List parsedList = new ArrayList<>(); + for (int i = 0; i < waypoints.size(); i++) { + WaylineExecuteWaypoint wp = waypoints.get(i); + // 先打印完整 JSON，方便查看所有字段 + String wpJson = new Gson().toJson(wp); + // LogUtil.log(TAG, "航点[" + i + "]完整JSON: " + wpJson); + + // 从 JSON 提取各字段（WaylineExecuteWaypoint 无直接 getter） + double lat = 0, lng = 0, alt = 0, speed = 0; + int hoverTime = hoverTimeMap.getOrDefault(i, 0); + try { + com.google.gson.JsonObject jsonObj = new Gson().fromJson(wpJson, com.google.gson.JsonObject.class); + // 经纬度：优先顶层latitude → waylineWaypoint.latitude → location.latitude + if (jsonObj.has("latitude") && !jsonObj.get("latitude").isJsonNull()) { + lat = jsonObj.get("latitude").getAsDouble(); + } else if (jsonObj.has("waylineWaypoint") && jsonObj.getAsJsonObject("waylineWaypoint").has("latitude")) { + lat = jsonObj.getAsJsonObject("waylineWaypoint").get("latitude").getAsDouble(); + } else if (jsonObj.has("location") && jsonObj.getAsJsonObject("location").has("latitude")) { + lat = jsonObj.getAsJsonObject("location").get("latitude").getAsDouble(); + } + if (jsonObj.has("longitude") && !jsonObj.get("longitude").isJsonNull()) { + lng = jsonObj.get("longitude").getAsDouble(); + } else if (jsonObj.has("waylineWaypoint") && jsonObj.getAsJsonObject("waylineWaypoint").has("longitude")) { + lng = jsonObj.getAsJsonObject("waylineWaypoint").get("longitude").getAsDouble(); + } else if (jsonObj.has("location") && jsonObj.getAsJsonObject("location").has("longitude")) { + lng = jsonObj.getAsJsonObject("location").get("longitude").getAsDouble(); + } + // 高度：优先altitude → waylineWaypoint.altitude → executeHeight + if (jsonObj.has("altitude") && !jsonObj.get("altitude").isJsonNull()) { + alt = jsonObj.get("altitude").getAsDouble(); + } else if (jsonObj.has("waylineWaypoint") && jsonObj.getAsJsonObject("waylineWaypoint").has("altitude")) { + alt = jsonObj.getAsJsonObject("waylineWaypoint").get("altitude").getAsDouble(); + } else if (jsonObj.has("executeHeight") && !jsonObj.get("executeHeight").isJsonNull()) { + alt = jsonObj.get("executeHeight").getAsDouble(); + } + if (jsonObj.has("speed") && !jsonObj.get("speed").isJsonNull()) { + speed = jsonObj.get("speed").getAsDouble(); + } + } catch (Exception e) { + LogUtil.log(TAG, "航点[" + i + "]部分字段解析跳过: " + e.getMessage()); + } + + ParsedWaypoint pw = new ParsedWaypoint(i, lat, lng, alt, hoverTime, speed); + parsedList.add(pw); + + // LogUtil.log(TAG, "航点[" + i + "] " + pw.toString()); + } + // 保存解析后的航点列表 + CurrentWayline.getInstance().setParsedWaypoints(parsedList); + + + } else { LogUtil.log(TAG, "WPMZManager getWaypointInfo有误"); } diff --git a/app/src/main/java/com/aros/apron/manager/StreamManager.java b/app/src/main/java/com/aros/apron/manager/StreamManager.java index 0bb59912..a5d9bbfb 100644 --- a/app/src/main/java/com/aros/apron/manager/StreamManager.java +++ b/app/src/main/java/com/aros/apron/manager/StreamManager.java @@ -1,9 +1,7 @@ package com.aros.apron.manager; -import android.content.Context; import android.os.Handler; import android.os.Looper; -import android.util.Log; import androidx.annotation.NonNull; @@ -16,8 +14,7 @@ import com.aros.apron.tools.PreferenceUtils; import com.aros.apron.tools.SimplePortScanner; import com.google.gson.Gson; -import java.util.concurrent.ExecutorService; -import java.util.concurrent.Executors; +import java.util.concurrent.atomic.AtomicBoolean; import dji.sdk.keyvalue.key.CameraKey; import dji.sdk.keyvalue.key.DJIKey; @@ -44,8 +41,6 @@ import dji.v5.manager.interfaces.ILiveStreamManager; public class StreamManager extends BaseManager { private static final String TAG = "StreamManager"; - // ========== 【新增】线程池和主线程 Handler，防止 ANR ========== - private final ExecutorService streamExecutor = Executors.newSingleThreadExecutor(); private final Handler mainHandler = new Handler(Looper.getMainLooper()); // ========== 5秒定时刷新视频流，防止起飞卡死 ========== @@ -110,48 +105,66 @@ public class StreamManager extends BaseManager { return StreamHolder.INSTANCE; } + + public void restart(MessageDown message) { + sendMsg2Server(message); + LogUtil.log(TAG, "重启推流：先停后启"); + ILiveStreamManager mgr = MediaDataCenter.getInstance().getLiveStreamManager(); + if (mgr == null) return; + mgr.stopStream(new CommonCallbacks.CompletionCallback() { + @Override public void onSuccess() { + LogUtil.log(TAG, "旧流已停，重新启动"); + resetStreamState(); + startLiveWithRTSP(); + } + @Override public void onFailure(@NonNull IDJIError e) { + LogUtil.log(TAG, "停流失败，直接重启"); + resetStreamState(); + startLiveWithRTSP(); + } + }); + } + + + + + + + + // ========== 【新增】重置推流状态，用于端口关闭后重启 ========== public void resetStreamState() { //stopStreamRefreshTimer(); // SimplePortScanner.getInstance().stopScan(); // 同时停止端口扫描 mainHandler.removeCallbacksAndMessages(null); // 清理所有待执行的回调 startLiveFailTimes = 0; - isLiveStreamAlreadyStart = false; - isStartingRTSP = false; + isStartingRTSP.set(false); LogUtil.log(TAG, "推流状态已重置"); } public void stopstream() { - streamExecutor.execute(() -> { - ILiveStreamManager liveStreamManager = MediaDataCenter.getInstance().getLiveStreamManager(); - liveStreamManager.stopStream(new CommonCallbacks.CompletionCallback() { - @Override - public void onSuccess() { - LogUtil.log(TAG, "直播关闭成功"); - } - - @Override - public void onFailure(@NonNull IDJIError idjiError) { - LogUtil.log(TAG, "直播关闭失败"); - } - }); + ILiveStreamManager mgr = MediaDataCenter.getInstance().getLiveStreamManager(); + mgr.stopStream(new CommonCallbacks.CompletionCallback() { + @Override + public void onSuccess() { + LogUtil.log(TAG, "直播关闭成功，重置状态"); + isStartingRTSP.set(false); + } + @Override + public void onFailure(@NonNull IDJIError e) { + LogUtil.log(TAG, "直播关闭失败:" + e.description()); + isStartingRTSP.set(false); + } }); } public void startstream() { - streamExecutor.execute(() -> { - ILiveStreamManager liveStreamManager = MediaDataCenter.getInstance().getLiveStreamManager(); - liveStreamManager.startStream(new CommonCallbacks.CompletionCallback() { - @Override - public void onSuccess() { - LogUtil.log(TAG, "直播开启成功"); - } - - @Override - public void onFailure(@NonNull IDJIError idjiError) { - LogUtil.log(TAG, "直播开启成功失败"); - } - }); + ILiveStreamManager mgr = MediaDataCenter.getInstance().getLiveStreamManager(); + mgr.startStream(new CommonCallbacks.CompletionCallback() { + @Override + public void onSuccess() { LogUtil.log(TAG, "直播开启成功"); } + @Override + public void onFailure(@NonNull IDJIError e) { LogUtil.log(TAG, "直播开启失败"); } }); } @@ -164,8 +177,8 @@ public class StreamManager extends BaseManager { public void onLiveStreamStatusUpdate(LiveStreamStatus status) { if (status != null) { Movement.getInstance().setLiveStatus(status.isStreaming() ? 1 : 0); - Log.d(TAG, "推流状态" + status.isStreaming() + "帧率:" + status.getFps() + "--" + "码率:" + status.getVbps() + "---" + "延迟:" + status.getRtt()); - + LogUtil.log(TAG, "推流状态" + status.isStreaming() + "帧率:" + status.getFps() + "--" + "码率:" + status.getVbps() + "---" + "延迟:" + status.getRtt()); + sendEvent2Server("推流状态" + status.isStreaming() + "帧率:" + status.getFps() + "--" + "码率:" + status.getVbps() + "---" + "延迟:" + status.getRtt(),1); } } @@ -204,8 +217,9 @@ public class StreamManager extends BaseManager { private volatile int startLiveFailTimes; - private volatile boolean isLiveStreamAlreadyStart; - private volatile boolean isStartingRTSP = false; // 防止并发调用 + // ★ 用 AtomicBoolean 替代 volatile boolean，CAS 防并发，不会卡死 + private final AtomicBoolean isStartingRTSP = new AtomicBoolean(false); + private final AtomicBoolean isStartingRTMP = new AtomicBoolean(false); // 无限重试：指数退避控制 private static final long RETRY_BASE_MS = 3000; // 初始 3 秒 @@ -214,13 +228,12 @@ public class StreamManager extends BaseManager { // 知眸测试 public void startLiveWithCustom() { - streamExecutor.execute(() -> { - Boolean isAircraftConnected = KeyManager.getInstance().getValue(DJIKey.create(ProductKey.KeyConnection)); - if (isAircraftConnected == null || !isAircraftConnected) { - LogUtil.log(TAG, "飞行器未连接"); - return; - } - ILiveStreamManager liveStreamManager = MediaDataCenter.getInstance().getLiveStreamManager(); + Boolean isAircraftConnected = KeyManager.getInstance().getValue(DJIKey.create(ProductKey.KeyConnection)); + if (isAircraftConnected == null || !isAircraftConnected) { + LogUtil.log(TAG, "飞行器未连接"); + return; + } + ILiveStreamManager liveStreamManager = MediaDataCenter.getInstance().getLiveStreamManager(); mainHandler.post(() -> { LogUtil.log(TAG, "自定义推流地址:" + PreferenceUtils.getInstance().getCustomStreamUrl()); LiveStreamSettings.Builder streamSettingBuilder = new LiveStreamSettings.Builder(); @@ -236,9 +249,8 @@ public class StreamManager extends BaseManager { }); if (!liveStreamManager.isStreaming()) { - doStartLiveCustom(liveStreamManager); + mainHandler.postDelayed(() -> doStartLiveCustom(liveStreamManager), 3000); } - }); } private void doStartLiveCustom(ILiveStreamManager liveStreamManager) { @@ -247,7 +259,6 @@ public class StreamManager extends BaseManager { public void onSuccess() { mainHandler.post(() -> { LogUtil.log(TAG, "自定义推流启动成功"); - isLiveStreamAlreadyStart = true; }); } @@ -255,16 +266,16 @@ public class StreamManager extends BaseManager { public void onFailure(@NonNull IDJIError error) { mainHandler.post(() -> { LogUtil.log(TAG, "第" + startLiveFailTimes + "次自定义推流失败:" + new Gson().toJson(error)); - if (!isLiveStreamAlreadyStart) { + // ★ 用 SDK 真实状态判断，不用手动标志位 + if (!liveStreamManager.isStreaming()) { long retryDelay = Math.min(RETRY_BASE_MS * (1L << Math.min(startLiveFailTimes, 4)), RETRY_MAX_MS); startLiveFailTimes++; if (startLiveFailTimes <= 3 || startLiveFailTimes % LOG_INTERVAL == 0) { LogUtil.log(TAG, "自定义推流准备第" + startLiveFailTimes + "次重试，间隔 " + retryDelay + "ms"); } - isStartingRTSP = false; - isLiveStreamAlreadyStart = false; + isStartingRTSP.set(false); mainHandler.postDelayed(() -> { - streamExecutor.execute(() -> startLiveWithCustom()); + startLiveWithCustom(); }, retryDelay); } }); @@ -273,240 +284,370 @@ public class StreamManager extends BaseManager { } - private int isliveindex = 1; //1 代表 port 2 代表 fpv + // ========== RTMP 推流（MQTT rtmp_push 协议） ========== - public void switchptspfpv(ComponentIndexType ComponentIndex, MessageDown message) { - if(isliveindex==2){ - sendMsg2Server(message); + /** + * MQTT 收到 rtmp_push 协议后调用：先停止当前推流，再启动 RTMP 推流 + * @param message MQTT 下发的消息，data.rtmp_url 为 RTMP 推流地址 + */ + public void startLiveWithRTMP(MessageDown message) { + + // CAS 防并发 + if (!isStartingRTMP.compareAndSet(false, true)) { + LogUtil.log(TAG, "startLiveWithRTMP 正在执行中，忽略本次调用"); return; } - isliveindex = 2; + + String rtmpUrl = message.getData() != null ? message.getData().getRtmp_url() : null; + if (rtmpUrl == null || rtmpUrl.isEmpty()) { + LogUtil.log(TAG, "RTMP URL 为空"); + isStartingRTMP.set(false); + sendFailMsg2Server(message, "RTMP URL 为空"); + return; + } + + LogUtil.log(TAG, "========== 开始 RTMP 推流流程 =========="); + LogUtil.log(TAG, "RTMP URL: " + rtmpUrl); + + Boolean isAircraftConnected = KeyManager.getInstance().getValue(DJIKey.create(FlightControllerKey.KeyConnection)); + if (isAircraftConnected == null || !isAircraftConnected) { + LogUtil.log(TAG, "飞行器未连接"); + isStartingRTMP.set(false); + sendFailMsg2Server(message, "飞行器未连接"); + return; + } + + ILiveStreamManager mgr = MediaDataCenter.getInstance().getLiveStreamManager(); + if (mgr == null) { + LogUtil.log(TAG, "LiveStreamManager 为 null"); + sendFailMsg2Server(message, "LiveStreamManager 为 null"); + isStartingRTMP.set(false); + return; + } + + // ★ 杀掉所有 RTSP 排队中的回调，重置 RTSP 状态，防止 RTSP 延迟任务覆盖 RTMP 配置 + resetStreamState(); + + // 先发送回执，表示收到指令 sendMsg2Server(message); - ILiveStreamManager liveStreamManager = MediaDataCenter.getInstance().getLiveStreamManager(); - LogUtil.log(TAG, "切换 RTSP 推流 fpv:" + PreferenceUtils.getInstance().getRtspUserName() - + "--" + PreferenceUtils.getInstance().getRtspPort() + "--" + PreferenceUtils.getInstance().getRtspPassWord()); - LiveStreamSettings.Builder streamSettingBuilder = new LiveStreamSettings.Builder(); - - LiveStreamSettings streamSettings = streamSettingBuilder.setLiveStreamType(LiveStreamType.RTSP) - .setRtspSettings(new RtspSettings.Builder().setPassWord(PreferenceUtils.getInstance().getRtspPassWord()). - setPort(Integer.parseInt(PreferenceUtils.getInstance().getRtspPort())). - setUserName(PreferenceUtils.getInstance().getRtspUserName()).build()).build(); - - liveStreamManager.setLiveStreamSettings(streamSettings); - - liveStreamManager.setCameraIndex(ComponentIndex); - - liveStreamManager.setLiveStreamQuality(StreamQuality.FULL_HD); - liveStreamManager.setLiveVideoBitrateMode(LiveVideoBitrateMode.AUTO); - } - - public void switchptspport(ComponentIndexType ComponentIndex, MessageDown message) { - if(isliveindex==1){ - sendMsg2Server(message); - return; - } - isliveindex = 1; - sendMsg2Server(message); - ILiveStreamManager liveStreamManager = MediaDataCenter.getInstance().getLiveStreamManager(); - LogUtil.log(TAG, "切换 RTSP 推流 port:" + PreferenceUtils.getInstance().getRtspUserName() - + "--" + PreferenceUtils.getInstance().getRtspPort() + "--" + PreferenceUtils.getInstance().getRtspPassWord()); - - LiveStreamSettings.Builder streamSettingBuilder = new LiveStreamSettings.Builder(); - - LiveStreamSettings streamSettings = streamSettingBuilder.setLiveStreamType(LiveStreamType.RTSP) - .setRtspSettings(new RtspSettings.Builder().setPassWord(PreferenceUtils.getInstance().getRtspPassWord()). - setPort(Integer.parseInt(PreferenceUtils.getInstance().getRtspPort())). - setUserName(PreferenceUtils.getInstance().getRtspUserName()).build()).build(); - - liveStreamManager.setLiveStreamSettings(streamSettings); - - liveStreamManager.setCameraIndex(ComponentIndex); - - liveStreamManager.setLiveStreamQuality(StreamQuality.FULL_HD); - liveStreamManager.setLiveVideoBitrateMode(LiveVideoBitrateMode.AUTO); - } - - // ========== 【核心修复】RTSP 推流入口，全部在子线程执行 ========== - public void startLiveWithRTSP() { - // 防止并发调用 - if (isStartingRTSP) { - LogUtil.log(TAG, "startLiveWithRTSP 正在执行中，忽略本次调用"); - return; - } - - streamExecutor.execute(() -> { - isStartingRTSP = true; - - if (startLiveFailTimes == 0) { - isLiveStreamAlreadyStart = false; - LogUtil.log(TAG, "========== 开始 RTSP 推流流程 =========="); + // 先停止当前推流，成功或失败都继续启动 RTMP + mgr.stopStream(new CommonCallbacks.CompletionCallback() { + @Override + public void onSuccess() { + LogUtil.log(TAG, "旧流已停止，开始配置 RTMP"); + configureAndStartRTMP(mgr, rtmpUrl, message); } - Boolean isAircraftConnected = KeyManager.getInstance().getValue(DJIKey.create(FlightControllerKey.KeyConnection)); - if (isAircraftConnected == null || !isAircraftConnected) { - LogUtil.log(TAG, "飞行器未连接"); - isStartingRTSP = false; - return; + @Override + public void onFailure(@NonNull IDJIError e) { + LogUtil.log(TAG, "停流失败，仍尝试启动 RTMP: " + e.description()); + configureAndStartRTMP(mgr, rtmpUrl, message); } - - ILiveStreamManager liveStreamManager = MediaDataCenter.getInstance().getLiveStreamManager(); - if (liveStreamManager == null) { - LogUtil.log(TAG, "LiveStreamManager 为 null"); - isStartingRTSP = false; - return; - } - - if (liveStreamManager.isStreaming()) { - LogUtil.log(TAG, "RTSP 推流已在运行，无需重复启动"); - isLiveStreamAlreadyStart = true; - isStartingRTSP = false; - return; - } - - if (!MainActivity.Companion.getStreamReceive()) { - LogUtil.log(TAG, "相机流未准备好，尝试模拟点击 FPV Widget 恢复"); - mainHandler.post(() -> { - MainActivity mainActivity = MainActivity.Companion.getInstance(); - if (mainActivity != null) { - mainActivity.smartRefreshVideoStream(); - } - }); - - mainHandler.postDelayed(() -> { - streamExecutor.execute(() -> { - startLiveFailTimes++; - if (startLiveFailTimes <= 3 || startLiveFailTimes % LOG_INTERVAL == 0) { - LogUtil.log(TAG, "相机流未准备好，第" + startLiveFailTimes + "次重试"); - } - startLiveWithRTSP(); - }); - }, 2000); - isStartingRTSP = false; // 释放锁，让重试能正常进入 - return; - } - - - String rtspUser = PreferenceUtils.getInstance().getRtspUserName(); - String rtspPort = PreferenceUtils.getInstance().getRtspPort(); - String rtspPass = PreferenceUtils.getInstance().getRtspPassWord(); - - if (rtspUser == null || rtspPort == null || rtspPass == null || - rtspUser.isEmpty() || rtspPort.isEmpty() || rtspPass.isEmpty()) { - LogUtil.log(TAG, "RTSP 配置参数有误：user=" + rtspUser + ", port=" + rtspPort); - isStartingRTSP = false; - return; - } - - LogUtil.log(TAG, "RTSP 配置检查通过：user=" + rtspUser + ", port=" + rtspPort + ", camera=" + (isliveindex == 1 ? "PORT_1" : "FPV")); - - //等待相机模式稳定（避免刚切换模式就推流） - try { - Thread.sleep(500); - } catch (InterruptedException e) { - Thread.currentThread().interrupt(); - } - - //设置 RTSP 参数 - final ILiveStreamManager finalLiveStreamManager = liveStreamManager; - mainHandler.post(() -> { - LiveStreamSettings.Builder streamSettingBuilder = new LiveStreamSettings.Builder(); - LiveStreamSettings streamSettings = streamSettingBuilder.setLiveStreamType(LiveStreamType.RTSP) - .setRtspSettings(new RtspSettings.Builder() - .setPassWord(rtspPass) - .setPort(Integer.parseInt(rtspPort)) - .setUserName(rtspUser) - .build()) - .build(); - - finalLiveStreamManager.setLiveStreamSettings(streamSettings); - - // 设置相机源 - ComponentIndexType cameraIndex = (isliveindex == 1) ? ComponentIndexType.PORT_1 : ComponentIndexType.FPV; - finalLiveStreamManager.setCameraIndex(cameraIndex); - - finalLiveStreamManager.setLiveStreamQuality(StreamQuality.FULL_HD); - finalLiveStreamManager.setLiveVideoBitrateMode(LiveVideoBitrateMode.AUTO); - - LogUtil.log(TAG, "RTSP 参数设置完成，等待 500ms 后启动推流"); - - mainHandler.postDelayed(() -> { - streamExecutor.execute(() -> { - // 9. 启动推流前再次检查 - if (finalLiveStreamManager.isStreaming()) { - LogUtil.log(TAG, "推流已在运行，跳过启动"); - isLiveStreamAlreadyStart = true; - isStartingRTSP = false; - return; - } - - LogUtil.log(TAG, "开始调用 startStream..."); - doStartLiveWithRTSP(finalLiveStreamManager, false); - }); - }, 500); - }); }); } + private void configureAndStartRTMP(ILiveStreamManager mgr, String rtmpUrl, MessageDown message) { + mainHandler.post(() -> { + LiveStreamSettings streamSettings = new LiveStreamSettings.Builder() + .setLiveStreamType(LiveStreamType.RTMP) + .setRtmpSettings(new RtmpSettings.Builder() + .setUrl(rtmpUrl) + .build()) + .build(); + mgr.setLiveStreamSettings(streamSettings); + mgr.setCameraIndex((isliveindex == 1) ? ComponentIndexType.PORT_1 : ComponentIndexType.FPV); + mgr.setLiveStreamQuality(StreamQuality.FULL_HD); + mgr.setLiveVideoBitrateMode(LiveVideoBitrateMode.AUTO); - // ========== 【新增】实际启动流的私有方法，确保在子线程执行 ========== - private void doStartLiveWithRTSP(ILiveStreamManager liveStreamManager, boolean isRestart) { - if (liveStreamManager.isStreaming()) { - LogUtil.log(TAG, "推流已在运行，跳过启动 (isRestart=" + isRestart + ")"); - isLiveStreamAlreadyStart = true; - isStartingRTSP = false; // 重置并发标志 + LogUtil.log(TAG, "RTMP 配置完成，3s 后启动推流"); + + mainHandler.postDelayed(() -> { + if (mgr.isStreaming()) { + // ★ 旧流未停干净：主动再停一次，然后重试启动 + LogUtil.log(TAG, "SDK显示仍在推流，再次停止后重试"); + mgr.stopStream(new CommonCallbacks.CompletionCallback() { + @Override + public void onSuccess() { + LogUtil.log(TAG, "二次停流成功，重试 RTMP 启动"); + mainHandler.postDelayed(() -> doStartRTMPStream(mgr, message), 2000); + } + @Override + public void onFailure(@NonNull IDJIError e) { + LogUtil.log(TAG, "二次停流失败，仍尝试 RTMP 启动: " + e.description()); + mainHandler.postDelayed(() -> doStartRTMPStream(mgr, message), 2000); + } + }); + return; + } + doStartRTMPStream(mgr, message); + }, 3000); + }); + } + + private void doStartRTMPStream(ILiveStreamManager mgr, MessageDown message) { + if (mgr.isStreaming()) { + // ★ 二次停流后仍显示推流中：再做最后一次 stop + retry，不轻易放弃 + LogUtil.log(TAG, "RTMP 启动前检查：SDK仍在推流，最后一次强制停流"); + mgr.stopStream(new CommonCallbacks.CompletionCallback() { + @Override + public void onSuccess() { + LogUtil.log(TAG, "三次停流成功，最终尝试 RTMP 启动"); + mainHandler.postDelayed(() -> { + mgr.startStream(new CommonCallbacks.CompletionCallback() { + @Override + public void onSuccess() { + LogUtil.log(TAG, "RTMP 推流启动成功（终极重试）"); + isStartingRTMP.set(false); + LogUtil.log(TAG, "========== RTMP 推流启动成功 =========="); + } + @Override + public void onFailure(@NonNull IDJIError error) { + LogUtil.log(TAG, "RTMP 推流最终失败: " + error.description()); + isStartingRTMP.set(false); + sendFailMsg2Server(message, "RTMP 推流最终失败: " + error.description()); + } + }); + }, 2000); + } + @Override + public void onFailure(@NonNull IDJIError e) { + LogUtil.log(TAG, "三次停流失败，彻底放弃"); + isStartingRTMP.set(false); + sendFailMsg2Server(message, "RTMP 推流失败：多次停流无效"); + } + }); return; } - - LogUtil.log(TAG, "开始调用 startStream... (isRestart=" + isRestart + ")"); - - liveStreamManager.startStream(new CommonCallbacks.CompletionCallback() { + LogUtil.log(TAG, "开始调用 RTMP startStream..."); + mgr.startStream(new CommonCallbacks.CompletionCallback() { @Override public void onSuccess() { - mainHandler.post(() -> { - LogUtil.log(TAG, "自定义 RTSP 推流启动成功" + (isRestart ? "(重启)" : "")); - isliveindex = 1; - isLiveStreamAlreadyStart = true; - startLiveFailTimes = 0; // 重置失败计数 - isStartingRTSP = false; // 重置并发标志 - LogUtil.log(TAG, "========== RTSP 推流启动成功 =========="); - // 启动5秒定时刷新视频流，防止起飞卡死 - // startStreamRefreshTimer(); - // 开始端口扫描 - // SimplePortScanner.getInstance().startScan(); - }); + LogUtil.log(TAG, "RTMP 推流启动成功"); + isStartingRTMP.set(false); + LogUtil.log(TAG, "========== RTMP 推流启动成功 =========="); } @Override public void onFailure(@NonNull IDJIError error) { - mainHandler.post(() -> { - String detailedError = "第" + startLiveFailTimes + "次开始 RTSP 推流失败：type=" + error.errorType()+ ", code=" + error.errorCode() + ", hint=" + error.hint(); - LogUtil.log(TAG, detailedError); - LogUtil.log(TAG, "完整错误:" + new Gson().toJson(error)); - - if (!isLiveStreamAlreadyStart) { - // 计算指数退避间隔：3s → 6s → 12s → 24s → 30s(封顶) - long retryDelay = Math.min(RETRY_BASE_MS * (1L << Math.min(startLiveFailTimes, 4)), RETRY_MAX_MS); - startLiveFailTimes++; - if (startLiveFailTimes <= 3 || startLiveFailTimes % LOG_INTERVAL == 0) { - LogUtil.log(TAG, "准备第" + startLiveFailTimes + "次重试，间隔 " + retryDelay + "ms"); - } - - // 重置所有状态，让下次重试是干净的 - isStartingRTSP = false; - isLiveStreamAlreadyStart = false; - stopstream(); // 先关再开，避免端口占用 - - // 指数退避重试 - mainHandler.postDelayed(() -> { - startLiveWithRTSP(); - }, retryDelay); - } else { - isStartingRTSP = false; - } - }); + LogUtil.log(TAG, "RTMP 推流启动失败: " + error.description()); + isStartingRTMP.set(false); + sendFailMsg2Server(message, "RTMP 推流失败: " + error.description()); } }); } + + // ========== 停止 RTMP 推流（MQTT stop_rtmp 协议） ========== + + /** + * MQTT 收到 stop_rtmp 协议后调用：停止 RTMP 推流，切回默认 RTSP + * @param message MQTT 下发的消息 + */ + public void stopRTMP(MessageDown message) { + sendMsg2Server(message); + LogUtil.log(TAG, "========== 停止 RTMP 推流，切回 RTSP =========="); + + // ★ 立即释放 RTMP 锁 + 清场，不阻塞快速连发的 push_rtmp + isStartingRTMP.set(false); + resetStreamState(); + + ILiveStreamManager mgr = MediaDataCenter.getInstance().getLiveStreamManager(); + if (mgr == null) { + LogUtil.log(TAG, "LiveStreamManager 为 null"); + return; + } + + mgr.stopStream(new CommonCallbacks.CompletionCallback() { + @Override + public void onSuccess() { + LogUtil.log(TAG, "RTMP 已停，启动 RTSP"); + startLiveWithRTSP(); + } + @Override + public void onFailure(@NonNull IDJIError e) { + LogUtil.log(TAG, "停流失败，仍启动 RTSP: " + e.description()); + startLiveWithRTSP(); + } + }); + } + + private volatile int isliveindex = 1; //1 代表 port 2 代表 fpv + + /** + * 切换 RTSP 推流相机源（和 DJI 示例一样：直接设 cameraIndex，SDK 内部切换） + */ + public void switchptspfpv(ComponentIndexType ComponentIndex, MessageDown message) { + if (isliveindex == 2) { sendMsg2Server(message); return; } + isliveindex = 2; + LogUtil.log(TAG, "切换推流到 FPV"); + MediaDataCenter.getInstance().getLiveStreamManager().setCameraIndex(ComponentIndexType.FPV); + sendMsg2Server(message); + } + + public void switchptspport(ComponentIndexType ComponentIndex, MessageDown message) { + if (isliveindex == 1) { sendMsg2Server(message); return; } + isliveindex = 1; + LogUtil.log(TAG, "切换推流到 PORT_1"); + MediaDataCenter.getInstance().getLiveStreamManager().setCameraIndex(ComponentIndexType.PORT_1); + sendMsg2Server(message); + } + + + + + + public void startLiveWithRTSP() { + // ★ CAS 防并发：只有一个线程能拿到 true，其他被拦截 + if (!isStartingRTSP.compareAndSet(false, true)) { + LogUtil.log(TAG, "startLiveWithRTSP 正在执行中，忽略本次调用"); + return; + } + + // ★ RTMP 推流正在进行中，不再启动 RTSP，防止覆盖 + if (isStartingRTMP.get()) { + LogUtil.log(TAG, "RTMP 推流正在进行中，取消 RTSP 启动"); + isStartingRTSP.set(false); + return; + } + + if (startLiveFailTimes == 0) { + LogUtil.log(TAG, "========== 开始 RTSP 推流流程 =========="); + } + + Boolean isAircraftConnected = KeyManager.getInstance().getValue(DJIKey.create(FlightControllerKey.KeyConnection)); + if (isAircraftConnected == null || !isAircraftConnected) { + LogUtil.log(TAG, "飞行器未连接"); + isStartingRTSP.set(false); + return; + } + + ILiveStreamManager mgr = MediaDataCenter.getInstance().getLiveStreamManager(); + if (mgr == null) { + LogUtil.log(TAG, "LiveStreamManager 为 null"); + isStartingRTSP.set(false); + return; + } + + // ★ 用 SDK 真实状态判断是否已在推流 + if (mgr.isStreaming()) { + LogUtil.log(TAG, "RTSP 推流已在运行（SDK状态），无需重复启动"); + isStartingRTSP.set(false); + return; + } + + if (!MainActivity.Companion.getStreamReceive()) { + LogUtil.log(TAG, "相机流未准备好，3s 后重试"); + startLiveFailTimes++; + isStartingRTSP.set(false); + // ★ 重试前检查 RTMP 是否已接管 + mainHandler.postDelayed(() -> { + if (isStartingRTMP.get()) { + LogUtil.log(TAG, "RTMP 已接管，取消 RTSP 重试（相机流就绪等待）"); + return; + } + startLiveWithRTSP(); + }, 3000); + return; + } + + String rtspUser = PreferenceUtils.getInstance().getRtspUserName(); + String rtspPort = PreferenceUtils.getInstance().getRtspPort(); + String rtspPass = PreferenceUtils.getInstance().getRtspPassWord(); + if (rtspUser == null || rtspPort == null || rtspPass == null || + rtspUser.isEmpty() || rtspPort.isEmpty() || rtspPass.isEmpty()) { + LogUtil.log(TAG, "RTSP 配置参数有误"); + isStartingRTSP.set(false); + return; + } + + LogUtil.log(TAG, "RTSP 配置检查通过：user=" + rtspUser + ", port=" + rtspPort); + + LiveStreamSettings streamSettings = new LiveStreamSettings.Builder() + .setLiveStreamType(LiveStreamType.RTSP) + .setRtspSettings(new RtspSettings.Builder() + .setPassWord(rtspPass) + .setPort(Integer.parseInt(rtspPort)) + .setUserName(rtspUser) + .build()) + .build(); + mgr.setLiveStreamSettings(streamSettings); + mgr.setCameraIndex((isliveindex == 1) ? ComponentIndexType.PORT_1 : ComponentIndexType.FPV); + mgr.setLiveStreamQuality(StreamQuality.FULL_HD); + mgr.setLiveVideoBitrateMode(LiveVideoBitrateMode.AUTO); + + LogUtil.log(TAG, "参数设置完成，3s 后启动推流"); + mainHandler.postDelayed(() -> { + // ★ RTMP 已接管，取消 RTSP 启动 + if (isStartingRTMP.get()) { + LogUtil.log(TAG, "RTMP 已接管，取消 RTSP 启动（3s 等待期间 RTMP 介入）"); + isStartingRTSP.set(false); + return; + } + // ★ 启动前再次用 SDK 状态确认 + if (mgr.isStreaming()) { + LogUtil.log(TAG, "SDK显示已在推流，跳过"); + isStartingRTSP.set(false); + return; + } + LogUtil.log(TAG, "开始调用 startStream..."); + mgr.startStream(new CommonCallbacks.CompletionCallback() { + @Override + public void onSuccess() { + LogUtil.log(TAG, "RTSP 推流启动成功"); + startLiveFailTimes = 0; + isStartingRTSP.set(false); + LogUtil.log(TAG, "========== RTSP 推流启动成功 =========="); + } + + @Override + public void onFailure(@NonNull IDJIError error) { + LogUtil.log(TAG, "第" + startLiveFailTimes + "次 RTSP 推流失败:" + error.description()); + + // ★ 用 SDK 真实状态判断是否要重试 + if (!mgr.isStreaming()) { + // ★ RTMP 已接管，不再重试 RTSP + if (isStartingRTMP.get()) { + LogUtil.log(TAG, "RTMP 已接管，取消 RTSP 失败重试"); + isStartingRTSP.set(false); + return; + } + long retryDelay = Math.min(RETRY_BASE_MS * (1L << Math.min(startLiveFailTimes, 4)), RETRY_MAX_MS); + startLiveFailTimes++; + isStartingRTSP.set(false); + + // 停流 → 等回调 → 重试 + mgr.stopStream(new CommonCallbacks.CompletionCallback() { + @Override + public void onSuccess() { + LogUtil.log(TAG, "重试前停流成功，" + retryDelay + "ms 后重试"); + // ★ 重试前再次检查 RTMP 状态 + mainHandler.postDelayed(() -> { + if (isStartingRTMP.get()) { + LogUtil.log(TAG, "RTMP 已接管，取消 RTSP 重试"); + return; + } + startLiveWithRTSP(); + }, retryDelay); + } + @Override + public void onFailure(@NonNull IDJIError e) { + LogUtil.log(TAG, "重试前停流失败，仍重试"); + // ★ 重试前再次检查 RTMP 状态 + mainHandler.postDelayed(() -> { + if (isStartingRTMP.get()) { + LogUtil.log(TAG, "RTMP 已接管，取消 RTSP 重试"); + return; + } + startLiveWithRTSP(); + }, retryDelay); + } + }); + } else { + isStartingRTSP.set(false); + } + } + }); + }, 3000); + } } diff --git a/app/src/main/java/com/aros/apron/mix/Aprondown.java b/app/src/main/java/com/aros/apron/mix/Aprondown.java index 0dbe6cc7..00179e33 100644 --- a/app/src/main/java/com/aros/apron/mix/Aprondown.java +++ b/app/src/main/java/com/aros/apron/mix/Aprondown.java @@ -414,7 +414,7 @@ public class Aprondown { } } else { LogUtil.log(TAG_LOG, "执行位移"); - DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.3f); + DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.5f); } } else if (lostDuration > 8000) { LogUtil.log(TAG_LOG, "判定未识别到二维码,飞往备降点"); @@ -570,6 +570,30 @@ public class Aprondown { Apronmixvalue.getInstance().setIsaglinetrue(false); // 下次降落重新对准 } + /** ★ 急停清理 */ + public void stopAndReset() { + isStartAruco = false; + startFastStick = false; + frameCounter = 0; + if (lastFuture != null && !lastFuture.isDone()) { + lastFuture.cancel(true); + lastFuture = null; + } + handler.removeCallbacks(runnable); + handlerCallbackCount = 0; + isHeightStableMonitoring = false; + lastHeightCheckTime = 0; + lastUltrasonicHeight = 0; + arucoNotFoundTag = false; + startTime = 0; + endTime = 0; + dropTimesTag = false; + dropTimes = 0; + if (pidControlX != null) pidControlX.reset(); + if (pidControlY != null) pidControlY.reset(); + LogUtil.log(TAG_LOG, "【急停清理】已重置"); + } + private double calculateYawErrorFromCorners(Point[] pts) { double dxTop = pts[1].x - pts[0].x; @@ -702,7 +726,7 @@ public class Aprondown { @Override public void run() { performOperation(); - if (handlerCallbackCount < 15) { + if (handlerCallbackCount < 18) { handler.postDelayed(this, 50); } else { performNextStep(); @@ -722,4 +746,4 @@ public class Aprondown { dropTimes = 0; handler.removeCallbacks(runnable); } -} \ No newline at end of file +} diff --git a/app/src/main/java/com/aros/apron/mix/Aprongim.java b/app/src/main/java/com/aros/apron/mix/Aprongim.java index 39073310..50919f1f 100644 --- a/app/src/main/java/com/aros/apron/mix/Aprongim.java +++ b/app/src/main/java/com/aros/apron/mix/Aprongim.java @@ -1035,7 +1035,7 @@ public class Aprongim { } else { // 高空丢失：下降寻找 LogUtil.log(TAG_LOG, "【执行移动】丢失下降 vz=-0.3"); - DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.3f); + DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.5f); } } else if (lostDuration > 8000) { LogUtil.log(TAG_LOG, "判定未识别到二维码,飞往备降点"); @@ -1079,13 +1079,43 @@ public class Aprongim { } } + /** ★ 急停清理 */ + public void stopAndReset() { + isStartAruco = false; + startFastStick = false; + frameCounter = 0; + if (lastFuture != null && !lastFuture.isDone()) { + lastFuture.cancel(true); + lastFuture = null; + } + handler.removeCallbacks(runnable); + handlerCallbackCount = 0; + isYawAligned = false; + currentLandingMode = 0; + counterRound = 0; + for (int i = 0; i < landingCounters.length; i++) { + landingCounters[i] = 0; + } + isHeightStableMonitoring = false; + lastHeightCheckTime = 0; + lastUltrasonicHeight = 0; + arucoNotFoundTag = false; + startTime = 0; + endTime = 0; + dropTimesTag = false; + dropTimes = 0; + if (pidControlX != null) pidControlX.reset(); + if (pidControlY != null) pidControlY.reset(); + LogUtil.log(TAG_LOG, "【急停清理】已重置"); + } + private int handlerCallbackCount = 0; private Handler handler = new Handler(Looper.getMainLooper()); private Runnable runnable = new Runnable() { @Override public void run() { performOperation(); - if (handlerCallbackCount < 15) { + if (handlerCallbackCount < 18) { handler.postDelayed(this, 50); } else { performNextStep(); @@ -1105,4 +1135,4 @@ public class Aprongim { dropTimes = 0; handler.removeCallbacks(runnable); } -} \ No newline at end of file +} diff --git a/app/src/main/java/com/aros/apron/mix/Apronmixvalue.java b/app/src/main/java/com/aros/apron/mix/Apronmixvalue.java index 74332a79..b8b89490 100644 --- a/app/src/main/java/com/aros/apron/mix/Apronmixvalue.java +++ b/app/src/main/java/com/aros/apron/mix/Apronmixvalue.java @@ -61,7 +61,7 @@ public class Apronmixvalue { Aprondown.getInstance().setDropTimes(0); // 如果高度已经大于 40 分米，直接切换，不用上升 - if (Movement.getInstance().getUltrasonicHeight() >= 40) { + if (Movement.getInstance().getUltrasonicHeight() >= 40 || Movement.getInstance().getElevation()>4) { Synchronizedstatus.setSwitchtime(true); return; } @@ -69,7 +69,7 @@ public class Apronmixvalue { Runnable runnable = new Runnable() { @Override public void run() { - if (Movement.getInstance().getUltrasonicHeight() < 50) { + if (Movement.getInstance().getUltrasonicHeight() < 50 && Movement.getInstance().getElevation() <5) { DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 3f); handler.postDelayed(this, 200); } 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 index 72b28d90..6866ee0c 100644 --- a/app/src/main/java/com/aros/apron/tools/AprilTagPort.java +++ b/app/src/main/java/com/aros/apron/tools/AprilTagPort.java @@ -3,7 +3,9 @@ package com.aros.apron.tools; import android.os.Handler; import android.os.Looper; -import com.aros.apron.constant.AMSConfig; +import android.os.Handler; +import android.os.Looper; + import com.aros.apron.entity.Movement; import com.aros.apron.manager.AlternateLandingManager; import com.example.nativec.ApriltagDetector; @@ -11,6 +13,16 @@ import com.example.nativec.ApriltagDetection; import com.example.nativec.ApriltagPose; import com.example.nativec.DetectionResult; +import org.opencv.calib3d.Calib3d; +import org.opencv.core.Core; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.MatOfDouble; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.MatOfPoint3f; +import org.opencv.core.Point; +import org.opencv.core.Point3; + import java.util.List; import java.util.concurrent.Executors; import java.util.concurrent.Future; @@ -18,36 +30,64 @@ import java.util.concurrent.ScheduledExecutorService; import java.util.concurrent.TimeUnit; /** - * AprilTag 视觉降落桥接类（v1.0）。 + * AprilTag 视觉降落（参照 C++ Landing + FlightStep + VisualProcessor 完整移植）。 * - *

与 Aruco 方案的区别

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

使用

- * 
{@code
- *   // 在相机帧回调里调用
- *   AprilTagPort.getInstance().processFrame(yuvData, width, height);
- * }
+ *

5 套 PID 控制器

+ * + * + * + * + * + * + *
PID-X (+0x70)X 轴正常模式Kp=0.8 Ki=0.008 Kd=0.03 iMax=0.05
PID-Y (+0x9c)Y 轴正常模式Kp=0.8 Ki=0.008 Kd=0.03 iMax=0.05
PID-Yaw1 (+0xc8)偏航正常Kp=1.6 Ki=0.008 Kd=0.06 iMax=0.05
PID-Yaw2 (+0xf4)偏航低空Kp=1.6 Ki=0.008 Kd=0.06 iMax=0.05
PID-Z (+0x120)垂直下降Kp=0.4 Ki=0.008 Kd=0.036 iMax=0.05
*/ 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 - // 位姿误差阈值 + // ========== PID 参数（来自 C++ VisualProcessor 反编译） ========== + private static final float KP = 0.8f; + private static final float KI = 0.008f; + private static final float KD = 0.03f; + private static final float IMAX = 0.05f; + private static final float PID_DT = 0.1f; // PID dt（100ms 帧间隔） + private static final float PID_FILT_HZ = 2.5f; // PID 滤波器截止频率 + + // ========== 速度限制（来自 C++ setSpeed 反编译） ========== + private static final float SPEED_ABOVE_15M = 3.0f; + private static final float SPEED_10_15M = 1.0f; // 带条件 + private static final float SPEED_3_10M = 1.0f; + private static final float SPEED_0_6_3M = 0.3f; + private static final float SPEED_BELOW_0_6M = 0.5f; + private static final float EMERGENCY_DESCENT = -3.0f; + + // ========== 位姿阈值 ========== private static final double POSE_ERROR_THRESHOLD = 0.05; + private static final float PID_RESET_THRESHOLD = 0.05f; // abs(error) < 此值 → reset PID + + // ========== 高度分段阈值 ========== + private static final float ALT_LOW_MODE = 3.0f; // 低于此高度进 doPIDLow + private static final float ALT_FORCE_LAND = 0.6f; // 低于此高度 forceLand + + // ========== 偏航 ========== + private static final double YAW_ALIGNED_DEG = 5.0; + private static final float YAW_P_GAIN = 0.6f; + private static final float YAW_MAX_RATE = 20.0f; + + // ========== 下降参数 ========== + private static final float MAX_DESCENT = -2.0f; + private static final float MIN_DESCENT = -0.05f; + + // ========== 平滑 ========== + private static final double EMA_ALPHA = 0.3; + + // ========== 丢失参数（对应 C++ checkGoAround + failsafeCheck） ========== + private static final int LOST_GOAROUND_FAST = 3; // >3帧无目标 → 拉起 + private static final int LOST_GOAROUND_SLOW = 50; // >50帧(≈5s) 高度<10m → 拉起 + private static final int LOST_FAILSAFE_MAX = 200; // >200帧(≈20s) → 备降兜底 + private static final int MAX_DROP_TIMES = 5; // 最多复降次数 + private static final float GOAROUND_CLIMB_VZ = 3.0f; // 拉起速度 m/s + private static final float GOAROUND_ALT_OK = 15.0f; // 拉起成功高度 // ========== 状态 ========== private volatile boolean isProcessing; @@ -55,25 +95,52 @@ public class AprilTagPort { private boolean startFastStick; private boolean canLanding; private boolean aprilTagNotFound; - private long lostStartTime; - private int dropTimes; - private boolean dropTimesTag; + private int lostFrameCount; // C++ field_348：丢失帧计数 + private int dropTimes; // C++ field_368：复降次数 + private boolean goAroundActive;// C++ field_373：正在拉起 + private boolean justDropped; // C++ field_372：刚完成一次拉起 + private int goAroundFrames; // 拉起持续帧数（兜底：50帧≈5s≈15m） 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=不过滤) + // ========== 5 套 PID 控制器 ========== + private PIDControl pidX; // +0x70 X 轴正常 + private PIDControl pidY; // +0x9c Y 轴正常 + private PIDControl pidYaw1; // +0xc8 偏航正常（2x Kp/Kd） + private PIDControl pidYaw2; // +0xf4 偏航低空（2x Kp/Kd） + private PIDControl pidZ; // +0x120 垂直下降（0.5x Kp, 1.2x Kd） + + // ========== 平滑位姿 ========== + private double smoothX, smoothY, smoothZ, smoothYaw; + private boolean poseInitialized; + + // ========== 线程 ========== ScheduledExecutorService executor = Executors.newScheduledThreadPool(1); Future lastFuture; - // ========== 降落触发后速降 ========== + // ========== 速降 ========== private int fastDescendCount; private Handler handler = new Handler(Looper.getMainLooper()); private Runnable fastDescendRunnable; + // ========== 记录 AprilTag 偏航 ========== + + private boolean isTriggerSuccess; + + public boolean isTriggerSuccess() { + return isTriggerSuccess; + } + + public void setTriggerSuccess(boolean triggerSuccess) { + isTriggerSuccess = triggerSuccess; + } + + private double apriltagYaw; + private boolean apriltagYawValid; + + // ========== 降落模式 ========== + private boolean emergencyLandMode; // C++ isEGMode + private float speedLimit; // 当前速度上限（from setSpeed） + // ========== 单例 ========== private AprilTagPort() {} @@ -86,12 +153,28 @@ public class AprilTagPort { return Holder.INSTANCE; } - // ========== 初始化 ========== + // ========== 初始化（对应 C++ Landing::init + resetPID） ========== 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); + // PID-X: Kp=0.8 Ki=0.008 Kd=0.03 iMax=0.05 + pidX = new PIDControl(KP, KI, KD, IMAX, PID_FILT_HZ, PID_DT); + // PID-Y: Kp=0.8 Ki=0.008 Kd=0.03 iMax=0.05 + pidY = new PIDControl(KP, KI, KD, IMAX, PID_FILT_HZ, PID_DT); + // PID-Yaw1: Kp=1.6 Ki=0.008 Kd=0.06 iMax=0.05（2x Kp/Kd） + pidYaw1 = new PIDControl(KP * 2.0f, KI, KD * 2.0f, IMAX, PID_FILT_HZ, PID_DT); + // PID-Yaw2: Kp=1.6 Ki=0.008 Kd=0.06 iMax=0.05（2x Kp/Kd） + pidYaw2 = new PIDControl(KP * 2.0f, KI, KD * 2.0f, IMAX, PID_FILT_HZ, PID_DT); + // PID-Z: Kp=0.4 Ki=0.008 Kd=0.036 iMax=0.05（0.5x Kp, 1.2x Kd） + pidZ = new PIDControl(KP * 0.5f, KI, KD * 1.2f, IMAX, PID_FILT_HZ, PID_DT); + resetAllPID(); + } + + /** 重置全部 PID（对应 C++ Landing::resetPID） */ + private void resetAllPID() { pidX.reset(); pidY.reset(); + pidYaw1.reset(); + pidYaw2.reset(); + pidZ.reset(); } // ========== 公开方法 ========== @@ -101,52 +184,109 @@ public class AprilTagPort { public void setStartAprilTag(boolean v) { isStartAprilTag = v; } public boolean isCanLanding() { return canLanding; } public void setCanLanding(boolean v) { + LogUtil.log(TAG, "[状态] setCanLanding=" + v + " (was=" + canLanding + ")"); canLanding = v; - lostStartTime = 0; + lostFrameCount = 0; } public boolean isStartFastStick() { return startFastStick; } - public void setStartFastStick(boolean v) { startFastStick = v; } + public void setStartFastStick(boolean v) { + LogUtil.log(TAG, "[状态] setStartFastStick=" + v + " (was=" + startFastStick + ")"); + startFastStick = v; + } + public void setEGLandMode(boolean v) { + LogUtil.log(TAG, "[状态] setEGLandMode=" + v); + emergencyLandMode = v; + } + public boolean isEGMode() { return emergencyLandMode; } /** - * 重置状态，准备下一次降落。 + * 重置状态，准备下一次降落（对应 C++ Landing::reset）。 */ public void reset() { + LogUtil.log(TAG, "[重置] 开始重置所有状态, dropTimes=" + dropTimes + + " frameCounter=" + frameCounter); startFastStick = false; isStartAprilTag = false; canLanding = false; aprilTagNotFound = false; - lostStartTime = 0; + lostFrameCount = 0; + goAroundActive = false; + justDropped = false; + goAroundFrames = 0; dropTimes = 0; - dropTimesTag = false; frameCounter = 0; fastDescendCount = 0; - pidX.reset(); - pidY.reset(); + emergencyLandMode = false; + apriltagYawValid = false; + apriltagYaw = 0; + resetAllPID(); poseInitialized = false; smoothX = 0; smoothY = 0; smoothZ = 0; smoothYaw = 0; + speedLimit = SPEED_3_10M; if (handler != null && fastDescendRunnable != null) { handler.removeCallbacks(fastDescendRunnable); } + LogUtil.log(TAG, "[重置] 完成"); } // ================================================================ - // 核心：处理一帧 YUV 图像 + // setSpeed（对应 C++ Landing::setSpeed） // ================================================================ /** - * 处理一帧 YUV 图像，检测 AprilTag 并控制无人机。 - * - * @param yuvData NV21 / YUV420_888 帧数据 - * @param width 图像宽度（像素） - * @param height 图像高度（像素） + * 根据当前高度和状态，设定水平速度上限（对应 C++ Landing::setSpeed）。 */ + private float setSpeed(double altM) { + // 紧急降落模式 → 3.0 m/s 下降 + if (emergencyLandMode) { + LogUtil.log(TAG, "[setSpeed] 紧急降落模式 vz=" + EMERGENCY_DESCENT); + return EMERGENCY_DESCENT; + } + + // >15m → 3.0 + if (altM > 15.0) { + LogUtil.log(TAG, "[setSpeed] alt>15m → 3.0"); + return SPEED_ABOVE_15M; + } + + // 10~15m: 如果偏差<某个值 → 1.0，否则 fallthrough + if (altM > 10.0) { + double absX = Math.abs(smoothX); + // C++: if abs(error) < 70 → 1.0 + if (absX < 0.7) { + LogUtil.log(TAG, "[setSpeed] 10 ALT_LOW_MODE) { + LogUtil.log(TAG, String.format("[setSpeed] %.1f ALT_FORCE_LAND) { + LogUtil.log(TAG, String.format("[setSpeed] %.1f allPoses = result.getLandingPoses(); - if (allPoses.isEmpty()) { - allPoses = result.getSinglePoses(); + // ── 先用 OpenCV 联合 PnP（和 C++ cv::solvePnP 一致）── + ApriltagPose opencvPose = computeJointPoseOpenCV(detector, result); + List allPoses; + if (opencvPose != null) { + allPoses = java.util.Collections.singletonList(opencvPose); + LogUtil.log(TAG, "[位姿] 使用OpenCV联合PnP, err=" + + String.format("%.4f", opencvPose.error)); + } else { + // 回退：JNI joint pose 或 landingPoses + ApriltagPose jniJoint = result.hasJointPose() ? result.getJointPose() : null; + if (jniJoint != null) { + allPoses = java.util.Collections.singletonList(jniJoint); + LogUtil.log(TAG, "[位姿] OpenCV失败, 回退JNI联合PnP"); + } else { + allPoses = result.getLandingPoses(); + if (allPoses.isEmpty()) { + allPoses = result.getSinglePoses(); + } + LogUtil.log(TAG, "[位姿数] landingPoses=" + result.getLandingPoses().size() + + " singlePoses=" + result.getSinglePoses().size()); + } } if (allPoses.isEmpty()) { + LogUtil.log(TAG, "[无位姿] allPoses为空, 进入丢失处理"); handleLostTag(ultrasonicHeight); isProcessing = false; isStartAprilTag = false; return; } - // 按 1/error 加权平均所有位姿 + // ── 加权平均位姿（等同 joint PnP 的融合效果） ── double totalWeight = 0; double fusedX = 0, fusedY = 0, fusedZ = 0, fusedYaw = 0; for (ApriltagPose p : allPoses) { @@ -205,17 +369,22 @@ public class AprilTagPort { fusedY += p.y() * w; fusedZ += p.z() * w; fusedYaw += p.yawDegrees() * w; + LogUtil.log(TAG, String.format("[单tag] x=%.3f y=%.3f z=%.3f yaw=%.1f° err=%.4f w=%.2f", + p.x(), p.y(), p.z(), p.yawDegrees(), p.error, w)); } fusedX /= totalWeight; fusedY /= totalWeight; fusedZ /= totalWeight; fusedYaw /= totalWeight; - // ── EMA 平滑滤波，防速度乱跳 ── + // ── EMA 平滑（同 C++ EMA_ALPHA=0.3） ── if (!poseInitialized) { smoothX = fusedX; smoothY = fusedY; smoothZ = fusedZ; smoothYaw = fusedYaw; poseInitialized = true; + LogUtil.log(TAG, "[EMA初始化] smooth=(" + + String.format("%.3f,%.3f,%.2f,%.1f°", + smoothX, smoothY, smoothZ, smoothYaw) + ")"); } else { smoothX = EMA_ALPHA * fusedX + (1 - EMA_ALPHA) * smoothX; smoothY = EMA_ALPHA * fusedY + (1 - EMA_ALPHA) * smoothY; @@ -223,21 +392,32 @@ public class AprilTagPort { smoothYaw = EMA_ALPHA * fusedYaw + (1 - EMA_ALPHA) * smoothYaw; } - // 平均误差 + // ── 记录偏航（对应 C++ recordAprilTagYaw） ── + apriltagYaw = (float) fusedYaw; + apriltagYawValid = true; + + // ── 平均误差 ── 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)); + double absX = Math.abs(smoothX); + double absY = Math.abs(smoothY); + double absYaw = Math.abs(smoothYaw); + double altM = smoothZ; - // 位姿误差过大 → 不可信 + 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) + ")，悬停"); + LogUtil.log(TAG, "[误差过大] avgErr=" + String.format("%.4f", avgErr) + + " > threshold=" + POSE_ERROR_THRESHOLD + "，悬停"); DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 0f); isProcessing = false; isStartAprilTag = false; @@ -245,84 +425,400 @@ public class AprilTagPort { } // ── 高度来源 ── - // （备用：超声波兜底。需要时取消注释） - // double ultraM = ultrasonicHeight / 10.0; - // double altM = (ultrasonicHeight > 0 && ultrasonicHeight <= 50) - // ? ultraM : smoothZ; - double altM = smoothZ; // 单位：米，AprilTag位姿Z分量（相机离地高度） + LogUtil.log(TAG, "[高度] 视觉高度=" + String.format("%.2fm", altM) + + " 超声波=" + ultrasonicHeight + "(0.1cm)"); - // ── 偏差绝对值 ── - double absX = Math.abs(smoothX); // 米，降落点左右偏差 - double absY = Math.abs(smoothY); // 米，降落点前后偏差 - double absYaw = Math.abs(smoothYaw); // 度，偏航角偏差 + // ── setSpeed：根据高度设定速度上限 ── + speedLimit = setSpeed(altM); - // ── 阶段判断 ── - boolean aboveTwo = altM > 2.0; // 2米以上 - boolean yawAligned = absYaw <= 5.0; // 5度以内算对准 + // ── 判断模式：doPID 还是 doPIDLow ── + boolean lowMode = (altM < ALT_LOW_MODE); + LogUtil.log(TAG, "[模式选择] alt=" + String.format("%.2f", altM) + + " lowMode=" + lowMode); - // ── X/Y 水平 PID：动态缩放（类似 errPx/scaleFactor）── - double scaleXY = Math.max(altM * 0.6, 0.3); // 缩放因子，最小0.3 - double normX = smoothX / scaleXY; // 归一化输入（无单位） - double normY = smoothY / scaleXY; // 归一化输入（无单位） + float vx, vy, yawRate, vz; - // ── 水平速度上限：越低越慢，防过冲 ── - double speedLimit = MAX_HORIZ_SPEED; // >3m: 1.2 m/s - if (altM < 1.0) speedLimit = 0.2; // <1m: 0.2 m/s - else if (altM < 2.0) speedLimit = 0.5; // 1~2m: 0.5 m/s - else if (altM < 3.0) speedLimit = 0.8; // 2~3m: 0.8 m/s - float maxV = (float) speedLimit; // m/s - - pidX.setInputFilterAll((float) normX); - pidY.setInputFilterAll((float) normY); - float vx = clamp(pidX.get_pid(), maxV); // m/s，前后 - float vy = clamp(pidY.get_pid(), maxV); // m/s，左右 - if (absX < 0.02) vx = 0f; // <2cm死区 - if (absY < 0.02) vy = 0f; // <2cm死区 - - // ── Yaw旋转：2m以上边转边降，2m以下不转 ── - float yawRate = 0f; // °/s - if (aboveTwo && !yawAligned) { - boolean nearTwo = altM <= 3.0; // 2~3m：紧急修正区 - if (nearTwo) { - // 全力转，确保到2m时对准 - yawRate = (absYaw > 10.0) ? 40.0f : 8.0f; - } else { - // 3m以上：从容修正 - if (absYaw > 60.0) yawRate = 35.0f; // >60°偏差 - else if (absYaw > 30.0) yawRate = 25.0f; // 30~60° - else if (absYaw > 15.0) yawRate = 15.0f; // 15~30° - else if (absYaw > 8.0) yawRate = 8.0f; // 8~15° - else yawRate = 5.0f; // 5~8° - } - yawRate = (smoothYaw > 0) ? -yawRate : yawRate; // 反向修正 + if (lowMode) { + // doPIDLow：使用 2x Kp/Kd 的偏航 PID + float[] xy = doPIDCore(pidX, pidY, smoothX, smoothY, speedLimit, absX, absY); + vx = xy[0]; vy = xy[1]; + yawRate = doYawCore(pidYaw2, smoothYaw, absYaw, altM); + vz = doDescentCore(pidZ, altM, absX, absY, absYaw); + } else { + // doPID：正常 PID + float[] xy = doPIDCore(pidX, pidY, smoothX, smoothY, speedLimit, absX, absY); + vx = xy[0]; vy = xy[1]; + yawRate = doYawCore(pidYaw1, smoothYaw, absYaw, altM); + vz = doDescentCore(pidZ, altM, absX, absY, absYaw); } - // ── 垂直速度：一路降到底 ── - float baseDescent = mapHeightToDescent((float) altM); // m/s，基础下降率 - // 对齐系数：偏差大→降慢，对准→全速。[0.15, 1.0] - float alignFactor = 1.0f - clamp01( - (float) Math.max(absX / 0.3, absY / 0.3)); - // absX=0.3m→0.0, absX=0m→1.0, absX>=0.255m→0.15 - alignFactor = Math.max(0.15f, alignFactor); // 最低15% - float vz = baseDescent * alignFactor; // m/s，最终下降速度（负=下降） - 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())); + "【控制】vx=%.3f vy=%.3f yaw=%.1f vz=%.2f | " + + "err=(%.3f,%.3f,%.1f°) alt=%.2fm tag=%d limit=%.2f", + vx, vy, yawRate, vz, + smoothX, smoothY, smoothYaw, altM, + allPoses.size(), speedLimit)); DroneHelper.getInstance().moveVxVyYawrateHeight(vx, vy, yawRate, vz); + LogUtil.log(TAG, "[控制下发] vx=" + vx + " vy=" + vy + + " yaw=" + yawRate + " vz=" + vz + " 已发送"); isProcessing = false; isStartAprilTag = false; } catch (Exception e) { - LogUtil.log(TAG, "Exception: " + e.getMessage()); + LogUtil.log(TAG, "[异常] processFrame: " + e.getMessage()); + e.printStackTrace(); isProcessing = false; isStartAprilTag = false; } }, 0, TimeUnit.MILLISECONDS); } + // ================================================================ + // OpenCV 联合 PnP（对应 C++ bundle joint PnP: cv::solvePnP） + // ================================================================ + + /** + * 用 OpenCV Calib3d.solvePnP() 替代 native 的加权平均， + * 所有角点一次求解 + LM 细化 + 协方差 = 和 C++ 完全一致。 + */ + private ApriltagPose computeJointPoseOpenCV(ApriltagDetector detector, DetectionResult result) { + List dets = result.getDetections(); + if (dets == null || dets.size() < 2) return null; + + int total = dets.size() * 4; + double[] cam = detector.getCameraParams(); + double fx = cam[0], fy = cam[1], cx = cam[2], cy = cam[3]; + + // 收集角点 + List objList = new java.util.ArrayList<>(total); + List imgList = new java.util.ArrayList<>(total); + + for (int i = 0; i < dets.size(); i++) { + ApriltagDetection det = dets.get(i); + double s = detector.getTagSize(det.id); + double[] off = detector.getTagOffset(det.id); + double dx = off[0], dy = off[1]; + double hs = s / 2.0; + + for (int j = 0; j < 4; j++) { + double u = det.p[j * 2]; + double v = det.p[j * 2 + 1]; + imgList.add(new Point(u, v)); + + double ox, oy; + switch (j) { + case 0: ox = -hs; oy = -hs; break; + case 1: ox = hs; oy = -hs; break; + case 2: ox = hs; oy = hs; break; + default:ox = -hs; oy = hs; break; + } + objList.add(new Point3(ox + dx, oy + dy, 0.0)); + } + } + + MatOfPoint3f objPoints = new MatOfPoint3f(); + objPoints.fromList(objList); + MatOfPoint2f imgPoints = new MatOfPoint2f(); + imgPoints.fromList(imgList); + + // 相机矩阵 + Mat camMatrix = new Mat(3, 3, CvType.CV_64FC1); + camMatrix.put(0, 0, fx, 0, cx, 0, fy, cy, 0, 0, 1); + MatOfDouble dist = new MatOfDouble(); // 无畸变（已校正） + + // solvePnP (ITERATIVE = 和 C++ 的 SOLVEPNP_ITERATIVE 一致) + Mat rvec = new Mat(3, 1, CvType.CV_64FC1); + Mat tvec = new Mat(3, 1, CvType.CV_64FC1); + + boolean ok = Calib3d.solvePnP(objPoints, imgPoints, camMatrix, dist, rvec, tvec, false, + Calib3d.SOLVEPNP_ITERATIVE); + if (!ok) { + releaseAll(objPoints, imgPoints, camMatrix, dist, rvec, tvec); + return null; + } + + // LM 细化 + Calib3d.solvePnPRefineLM(objPoints, imgPoints, camMatrix, dist, rvec, tvec); + + // rvec → R + Mat Rmat = new Mat(3, 3, CvType.CV_64FC1); + Calib3d.Rodrigues(rvec, Rmat); + + // 重投影误差 + MatOfPoint2f proj = new MatOfPoint2f(); + Calib3d.projectPoints(objPoints, rvec, tvec, camMatrix, dist, proj); + double error = 0; + float[] projBuf = new float[2]; + float[] imgBuf = new float[2]; + for (int i = 0; i < total; i++) { + proj.get(i, 0, projBuf); + imgPoints.get(i, 0, imgBuf); + double du = imgBuf[0] - projBuf[0]; + double dv = imgBuf[1] - projBuf[1]; + error += du * du + dv * dv; + } + error = Math.sqrt(error / total); + + // 协方差: (J^T*J)^{-1} * sigma² + Mat covMat = new Mat(6, 6, CvType.CV_64FC1); + if (total > 6) { + Mat jac = new Mat(); // 让 OpenCV 自动分配 + Calib3d.projectPoints(objPoints, rvec, tvec, camMatrix, dist, proj, jac); + Core.gemm(jac.t(), jac, 1.0, new Mat(), 0.0, covMat); + double sigma2 = error * error / (2.0 * total - 6); + if (sigma2 < 1e-12) sigma2 = 1e-12; + Core.invert(covMat, covMat, Core.DECOMP_SVD); + Core.multiply(covMat, new org.opencv.core.Scalar(sigma2), covMat); + jac.release(); + } + + releaseAll(objPoints, imgPoints, camMatrix, dist, proj); + + // 构造结果 + ApriltagPose pose = new ApriltagPose(); + pose.error = error; + Rmat.get(0, 0, pose.R); + tvec.get(0, 0, pose.t); + covMat.get(0, 0, pose.covariance); + releaseAll(Rmat, tvec, rvec, covMat); + + LogUtil.log(TAG, String.format("[OpenCV-PnP] %d tags(%d pts) err=%.4f", + dets.size(), total, error)); + return pose; + } + + /** 批量释放 OpenCV Mat */ + private static void releaseAll(Mat... mats) { + for (Mat m : mats) { + if (m != null) m.release(); + } + } + + // ================================================================ + // doPID（对应 C++ Landing::doPID） + // ================================================================ + + /** + * 水平 X/Y PID 控制（对应 C++ Landing::doPID / doPIDLow）。 + * @return float[2] {vx, vy} + */ + private float[] doPIDCore(PIDControl pidX, PIDControl pidY, + double errX, double errY, + float limit, double absX, double absY) { + if (absX < PID_RESET_THRESHOLD) { + pidX.reset(); + LogUtil.log(TAG, "[PID-X] absErr<" + PID_RESET_THRESHOLD + " → reset"); + } + if (absY < PID_RESET_THRESHOLD) { + pidY.reset(); + LogUtil.log(TAG, "[PID-Y] absErr<" + PID_RESET_THRESHOLD + " → reset"); + } + + double scaleXY = Math.max(smoothZ * 0.6, 0.3); + float normX = (float) (errX / scaleXY); + float normY = (float) (errY / scaleXY); + + pidX.setInputFilterAll(normX); + pidY.setInputFilterAll(normY); + + float vx = clamp(pidX.get_pid(), limit); + float vy = clamp(pidY.get_pid(), limit); + + if (absX < 0.02) vx = 0f; + if (absY < 0.02) vy = 0f; + + // PID 分量日志 + float px = pidX.getmKp() * pidX.getmInput(); + float ix = pidX.getmIntegrator(); + float dx = pidX.getmKd() * pidX.getmDerivative(); + float py = pidY.getmKp() * pidY.getmInput(); + float iy = pidY.getmIntegrator(); + float dy = pidY.getmKd() * pidY.getmDerivative(); + + LogUtil.log(TAG, String.format( + "【PID-X】P=%.4f I=%.4f D=%.4f | input=%.3f total=%.3f", + px, ix, dx, pidX.getmInput(), (px + ix + dx))); + LogUtil.log(TAG, String.format( + "【PID-Y】P=%.4f I=%.4f D=%.4f | input=%.3f total=%.3f", + py, iy, dy, pidY.getmInput(), (py + iy + dy))); + + return new float[]{vx, vy}; + } + + // ================================================================ + // Yaw 控制（对应 C++ getYawCmd + landControlYawRate） + // ================================================================ + + /** + * 偏航 PID 控制（对应 C++ Landing::getYawCmd + landControlYawRate）。 + * C++: setInputFilterAll(yawErr) → get_pid() → clamp(±MAX_YAW_RATE) + */ + private float doYawCore(PIDControl yawPID, double yawErr, double absYaw, double altM) { + boolean yawAligned = absYaw <= YAW_ALIGNED_DEG; + + if (yawAligned) { + yawPID.reset(); // 对准时清零积分（对应 C++ abs(err) MIN_DESCENT) baseDescent = MIN_DESCENT; + if (baseDescent < MAX_DESCENT) baseDescent = MAX_DESCENT; + + // 对齐系数：偏差大→降速，对准→全速 + float xyFactor = clamp01((float) Math.max(absX / 1.5, absY / 1.5)); + float yawFactor = yawAligned ? 0f : clamp01((float) (absYaw / 30.0)); + float alignFactor = 1.0f - Math.max(xyFactor, yawFactor); + alignFactor = Math.max(0.25f, alignFactor); + + float vz = baseDescent * alignFactor; + + LogUtil.log(TAG, String.format("[下降] base=%.2f align=%.2f vz=%.2f", + baseDescent, alignFactor, vz)); + return vz; + } + + // ================================================================ + // 丢失处理（对应 C++ checkGoAround + failsafeCheck + lostGPSDescend） + // ================================================================ + + /** + * C++ 丢失逻辑： + * lostFrame > 3 + low alt → go-around 拉起(vz=+3.0) + * lostFrame > 50 + alt<10m → go-around 拉起 + * go-around 中 alt>15m → 拉起成功, dropTimes++, 降 + * lostFrame > 200 → Failsafe 备降 + * dropTimes > MAX_DROP_TIMES → Failsafe 备降 + * 高空丢失 → 慢降寻找(vz=-0.3) + */ + private void handleLostTag(int ultrasonicHeight) { + if (!aprilTagNotFound) { + aprilTagNotFound = true; + LogUtil.log(TAG, "[丢失-开始] 首次丢失 ult=" + ultrasonicHeight + " dropTimes=" + dropTimes); + } + + lostFrameCount++; + double altM = smoothZ; + boolean lowUlt = ultrasonicHeight <= 20; + + LogUtil.log(TAG, "[丢失] frame=" + lostFrameCount + " ult=" + ultrasonicHeight + + " alt=" + String.format("%.1f", altM) + "m goAround=" + goAroundActive + + " dropTimes=" + dropTimes + "/" + MAX_DROP_TIMES); + + // ── Failsafe 检查 ── + if (dropTimes > MAX_DROP_TIMES) { + LogUtil.log(TAG, "[Failsafe] dropTimes=" + dropTimes + " > " + MAX_DROP_TIMES + ", 备降"); + AlternateLandingManager.getInstance().startTaskProcess(null); + Movement.getInstance().setAlternate(true); + return; + } + if (lostFrameCount > LOST_FAILSAFE_MAX) { + LogUtil.log(TAG, "[Failsafe] lostFrame=" + lostFrameCount + " > " + LOST_FAILSAFE_MAX + ", 备降"); + AlternateLandingManager.getInstance().startTaskProcess(null); + Movement.getInstance().setAlternate(true); + return; + } + + // ── 正在拉起 → 持续上升直到 >15m（飞控高度为主，帧数兜底）── + if (goAroundActive) { + goAroundFrames++; + double realAlt = Movement.getInstance().getElevation(); // 飞控实际高度（同 C++） + boolean reached = realAlt > GOAROUND_ALT_OK + || (goAroundFrames > 60); // 兜底：60帧≈6s≈18m + if (reached) { + goAroundActive = false; + justDropped = true; + dropTimes++; + lostFrameCount = 0; + LogUtil.log(TAG, "[拉起完成] realAlt=" + String.format("%.1f", realAlt) + + "m frames=" + goAroundFrames + " dropTimes=" + dropTimes); + DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.3f); + return; + } + LogUtil.log(TAG, "[拉起中] vz=" + GOAROUND_CLIMB_VZ + + " realAlt=" + String.format("%.1f", realAlt) + + "m frames=" + goAroundFrames); + DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, GOAROUND_CLIMB_VZ); + return; + } + + // ── 判断是否触发拉起 ── + boolean c1 = lostFrameCount > LOST_GOAROUND_FAST && lowUlt; // >3帧 + 低空 + boolean c2 = lostFrameCount > LOST_GOAROUND_SLOW && altM < 10.0; // >50帧 + <10m + + if (c1 || c2) { + goAroundActive = true; + justDropped = false; + LogUtil.log(TAG, "[Go-Around!] c1=" + c1 + " c2=" + c2 + " frame=" + lostFrameCount + + " alt=" + String.format("%.1f", altM)); + DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, GOAROUND_CLIMB_VZ); + return; + } + + // ── 未触发：慢降或悬停 ── + if (lowUlt) { + LogUtil.log(TAG, "[丢失-低空] 悬停等待 frame=" + lostFrameCount); + DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 0f); + } else { + LogUtil.log(TAG, "[丢失-高空] 慢降寻找 vz=-0.3 frame=" + lostFrameCount); + DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.3f); + } + } + + // ================================================================ + // 速降（对应 C++ Landing::forceLand / fastDescendRunnable） + // ================================================================ + + 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); + } + } + }; + } + // ================================================================ // 工具方法 // ================================================================ @@ -341,89 +837,10 @@ public class AprilTagPort { 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()) { @@ -435,8 +852,8 @@ public class AprilTagPort { 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])); + " 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(); diff --git a/app/src/main/java/com/aros/apron/tools/ApronArucoDetect.java b/app/src/main/java/com/aros/apron/tools/ApronArucoDetect.java index 0166a2e8..ec033f97 100644 --- a/app/src/main/java/com/aros/apron/tools/ApronArucoDetect.java +++ b/app/src/main/java/com/aros/apron/tools/ApronArucoDetect.java @@ -311,7 +311,7 @@ public class ApronArucoDetect { } } else { LogUtil.log(TAG_LOG, "执行位移"); - DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.3f); + DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.5f); } isHeightStableMonitoring = false; @@ -390,23 +390,21 @@ public class ApronArucoDetect { dropTimesTag = true; } else { - // 原有丢失处理 - LogUtil.log(TAG_LOG, "找不到了二维码"); + // 丢失处理 + int lostUltHeight = Movement.getInstance().getUltrasonicHeight(); - // 【新增】识别失败截图保存 - if (saveFailScreenshot) { - saveFailScreenshot(grayImgMat, width, height); - - } if (!arucoNotFoundTag) { startTime = System.currentTimeMillis(); arucoNotFoundTag = true; + LogUtil.log(TAG_LOG, String.format("【丢失开始】ult=%d dropTimes=%d", lostUltHeight, dropTimes)); } endTime = System.currentTimeMillis(); long lostDuration = endTime - startTime; if (lostDuration > 1000 && lostDuration <= 12000) { - if (Movement.getInstance().getUltrasonicHeight() <= 20) { + if (lostUltHeight <= 20) { + LogUtil.log(TAG_LOG, String.format("【丢失拉高】vz=3.0 ult=%d 丢失=%dms dropTimes=%d", + lostUltHeight, lostDuration, dropTimes)); DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, 3f); if (dropTimes > Integer.parseInt(AMSConfig.getInstance().getAlternateLandingTimes())) { LogUtil.log(TAG_LOG, "超过复降限制,去备降点"); @@ -420,11 +418,11 @@ public class ApronArucoDetect { LogUtil.log(TAG_LOG, "复降第:" + dropTimes + "次"); } } else { - LogUtil.log(TAG_LOG, "执行位移"); + LogUtil.log(TAG_LOG, String.format("【丢失下降】vz=-0.3 ult=%d 丢失=%dms", lostUltHeight, lostDuration)); DroneHelper.getInstance().moveVxVyYawrateHeight(0f, 0f, 0f, -0.3f); } } else if (lostDuration > 8000) { - LogUtil.log(TAG_LOG, "判定未识别到二维码,飞往备降点"); + LogUtil.log(TAG_LOG, String.format("【丢失超时】触发备降 丢失=%dms", lostDuration)); AlternateLandingManager.getInstance().startTaskProcess(null); Movement.getInstance().setAlternate(true); } @@ -697,6 +695,30 @@ public class ApronArucoDetect { failScreenshotIndex = 0; } + /** ★ 急停清理 */ + public void stopAndReset() { + isStartAruco = false; + startFastStick = false; + frameCounter = 0; + if (lastFuture != null && !lastFuture.isDone()) { + lastFuture.cancel(true); + lastFuture = null; + } + handler.removeCallbacks(runnable); + handlerCallbackCount = 0; + isHeightStableMonitoring = false; + lastHeightCheckTime = 0; + lastUltrasonicHeight = 0; + arucoNotFoundTag = false; + startTime = 0; + endTime = 0; + dropTimesTag = false; + dropTimes = 0; + if (pidControlX != null) pidControlX.reset(); + if (pidControlY != null) pidControlY.reset(); + LogUtil.log(TAG_LOG, "【急停清理】已重置"); + } + private double calculateYawErrorFromCorners(Point[] pts) { double dxTop = pts[1].x - pts[0].x; @@ -750,6 +772,13 @@ public class ApronArucoDetect { float vx = (float) Math.max(-0.2, Math.min(0.2, rawVx)); float vy = (float) Math.max(-0.2, Math.min(0.2, rawVy)); + LogUtil.log(TAG_LOG, String.format( + "【PID链路】errX=%.0f errY=%.0f scale=%.0f | 输入: pidX_in=%.4f pidY_in=%.4f | PID输出: rawVx=%.4f rawVy=%.4f | 钳位后: vx=%.4f vy=%.4f", + errX, errY, scaleFactor, + errX / scaleFactor, -errY / scaleFactor, + rawVx, rawVy, + vx, vy)); + double pixelWidth = Math.sqrt(Math.pow(pts[1].x - pts[0].x, 2) + Math.pow(pts[1].y - pts[0].y, 2)); @@ -780,31 +809,12 @@ public class ApronArucoDetect { float vz; if (currentHeight <= 4) { vz = 0.0f; - if (Math.abs(errX) > 120) { - vx = rawVx > 0 ? 0.135f : -0.135f; - } else if (Math.abs(errX) > 80) { - vx = rawVx > 0 ? 0.09f : -0.09f; - } else if (Math.abs(errX) > 60) { - vx = rawVx > 0 ? 0.07f : -0.07f; - } else if (Math.abs(errX) > 30) { - vx = rawVx > 0 ? 0.05f : -0.05f; - } else { - vx = 0f; - } - - - if (Math.abs(errY) > 120) { - vy = rawVy > 0 ? 0.135f : -0.135f; - } else if (Math.abs(errY) > 80) { - vy = rawVy > 0 ? 0.09f : -0.09f; - } else if (Math.abs(errY) > 60) { - vy = rawVy > 0 ? 0.07f : -0.07f; - } else if (Math.abs(errY) > 30) { - vy = rawVy > 0 ? 0.05f : -0.05f; - } else { - vy = 0f; // 【修正】 - } - + // 【修复】低空改用 PID 连续输出 + 低速度上限，去掉阶梯式开关控制 + // 之前 bang-bang 固定档位导致来回震荡（0.05→0.09→0.135→-0.09...） + // 现在用 PID 平滑输出，上限压到 0.06m/s，避免低空猛冲晃出二维码范围 + float lowMaxSpeed = 0.06f; + vx = (float) Math.max(-lowMaxSpeed, Math.min(lowMaxSpeed, rawVx)); + vy = (float) Math.max(-lowMaxSpeed, Math.min(lowMaxSpeed, rawVy)); } else if (currentHeight <= 8) { vz = SLOW_SUPER_SPEED; } else { @@ -813,8 +823,11 @@ public class ApronArucoDetect { DroneHelper.getInstance().moveVxVyYawrateHeight(vx, vy, yawRate, vz); - LogUtil.log(TAG_LOG, "vx" + vx + "vy" + vy + " errX=" + (int) errX + " errY=" + (int) errY + - " pixelW=" + (int) pixelWidth + " vz=" + vz + " ult=" + currentHeight + " yaw=" + yawRate); + LogUtil.log(TAG_LOG, String.format( + "【摇杆下发】vx=%.3f vy=%.3f vz=%.2f yaw=%.1f | errX=%d errY=%d pixelW=%d ult=%d | LENS=(%d,%d)", + vx, vy, vz, yawRate, + (int)errX, (int)errY, (int)pixelWidth, currentHeight, + (int)LENS_OFFSET_X, (int)LENS_OFFSET_Y)); } private int handlerCallbackCount = 0; @@ -823,7 +836,7 @@ public class ApronArucoDetect { @Override public void run() { performOperation(); - if (handlerCallbackCount < 15) { + if (handlerCallbackCount < 18) { handler.postDelayed(this, 50); } else { performNextStep(); @@ -843,4 +856,4 @@ public class ApronArucoDetect { dropTimes = 0; handler.removeCallbacks(runnable); } -} \ No newline at end of file +} diff --git a/app/src/main/java/com/aros/apron/tools/ApronArucoDetectPort.java b/app/src/main/java/com/aros/apron/tools/ApronArucoDetectPort.java index e7dbd038..a69bc8aa 100644 --- a/app/src/main/java/com/aros/apron/tools/ApronArucoDetectPort.java +++ b/app/src/main/java/com/aros/apron/tools/ApronArucoDetectPort.java @@ -1013,13 +1013,61 @@ public class ApronArucoDetectPort { } } + /** ★ 急停清理：重置所有状态，取消所有定时器，确保下次降落可以重新触发 */ + public void stopAndReset() { + // 1. 取消入口 guard，否则下次 detectArucoTags 直接 return + isStartAruco = false; + startFastStick = false; + frameCounter = 0; + + // 2. 取消 ScheduledExecutorService 里的 pending 检测任务 + if (lastFuture != null && !lastFuture.isDone()) { + lastFuture.cancel(true); + lastFuture = null; + } + + // 3. 停止快速下降 handler + handler.removeCallbacks(runnable); + handlerCallbackCount = 0; + + // 4. 重置旋转阶段 + isYawAligned = false; + currentLandingMode = 0; + + // 5. 重置分支计数器 + counterRound = 0; + for (int i = 0; i < landingCounters.length; i++) { + landingCounters[i] = 0; + } + + // 6. 重置高度稳定性监测 + isHeightStableMonitoring = false; + lastHeightCheckTime = 0; + lastUltrasonicHeight = 0; + + // 7. 重置丢失计时器 + arucoNotFoundTag = false; + startTime = 0; + endTime = 0; + + // 8. 重置复降计数 + dropTimesTag = false; + dropTimes = 0; + + // 9. PID 复位 + if (pidControlX != null) pidControlX.reset(); + if (pidControlY != null) pidControlY.reset(); + + LogUtil.log(TAG_LOG, "【急停清理】所有状态和定时器已重置"); + } + private int handlerCallbackCount = 0; private Handler handler = new Handler(Looper.getMainLooper()); private Runnable runnable = new Runnable() { @Override public void run() { performOperation(); - if (handlerCallbackCount < 15) { + if (handlerCallbackCount < 18) { handler.postDelayed(this, 50); } else { performNextStep(); 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 e43d1488..46cf0026 100644 --- a/app/src/main/java/com/aros/apron/tools/ApronArucodownmany.java +++ b/app/src/main/java/com/aros/apron/tools/ApronArucodownmany.java @@ -899,6 +899,34 @@ public class ApronArucodownmany { } } + /** ★ 急停清理 */ + public void stopAndReset() { + isStartAruco = false; + startFastStick = false; + frameCounter = 0; + if (lastFuture != null && !lastFuture.isDone()) { + lastFuture.cancel(true); + lastFuture = null; + } + handler.removeCallbacks(runnable); + handlerCallbackCount = 0; + counterRound = 0; + for (int i = 0; i < landingCounters.length; i++) { + landingCounters[i] = 0; + } + isHeightStableMonitoring = false; + lastHeightCheckTime = 0; + lastUltrasonicHeight = 0; + arucoNotFoundTag = false; + startTime = 0; + endTime = 0; + dropTimesTag = false; + dropTimes = 0; + if (pidControlX != null) pidControlX.reset(); + if (pidControlY != null) pidControlY.reset(); + LogUtil.log(TAG_LOG, "【急停清理】已重置"); + } + private double calculateYawErrorFromCorners(Point[] pts) { double dxTop = pts[1].x - pts[0].x; @@ -1045,4 +1073,4 @@ public class ApronArucodownmany { dropTimes = 0; handler.removeCallbacks(runnable); } -} \ No newline at end of file +} diff --git a/app/src/main/java/com/aros/apron/tools/MqttManager.java b/app/src/main/java/com/aros/apron/tools/MqttManager.java index d927574c..9c51f18b 100644 --- a/app/src/main/java/com/aros/apron/tools/MqttManager.java +++ b/app/src/main/java/com/aros/apron/tools/MqttManager.java @@ -40,10 +40,26 @@ public class MqttManager { } public void needConnect() { + // 已经连接就不要再建新的，避免生成新handle导致旧handle失效 + if (mqttAndroidClient != null && mqttAndroidClient.isConnected()) { + LogUtil.log(TAG, "MQTT已连接，跳过重复needConnect"); + return; + } initMqttClientParams(); } + private void initMqttClientParams() { - mqttAndroidClient = new MqttAndroidClient(ApronApp.Companion.getApplication(), AMSConfig.getInstance().getMqttServerUri(), generateRandomString(10)); + // 先关闭旧的client，释放MqttService里的旧handle，防止"Invalid ClientHandle" + if (mqttAndroidClient != null) { + try { + mqttAndroidClient.close(); + } catch (Exception e) { + LogUtil.log(TAG, "关闭旧MQTT客户端异常:" + e); + } + } + // 使用固定的clientId前缀 + 时间戳，避免每次随机导致Service端残留多个无效handle + String clientId = "ams_" + System.currentTimeMillis(); + mqttAndroidClient = new MqttAndroidClient(ApronApp.Companion.getApplication(), AMSConfig.getInstance().getMqttServerUri(), clientId); mMqttConnectOptions = new MqttConnectOptions(); mMqttConnectOptions.setAutomaticReconnect(true); mMqttConnectOptions.setMaxInflight(1000);// 避免消息积压导致连接拥塞 diff --git a/app/src/main/java/com/aros/apron/tools/PsdkWidgetScheduler.java b/app/src/main/java/com/aros/apron/tools/PsdkWidgetScheduler.java index af421353..c1809226 100644 --- a/app/src/main/java/com/aros/apron/tools/PsdkWidgetScheduler.java +++ b/app/src/main/java/com/aros/apron/tools/PsdkWidgetScheduler.java @@ -156,7 +156,7 @@ public class PsdkWidgetScheduler { report.setData(data); String jsonPayload = new Gson().toJson(report); - LogUtil.log(TAG, "PSDK widget JSON payload: " + jsonPayload); + // LogUtil.log(TAG, "PSDK widget JSON payload: " + jsonPayload); MqttMessage mqttMessage = new MqttMessage(jsonPayload.getBytes("UTF-8")); mqttMessage.setQos(1); client.publish(AMSConfig.UP_UAV_EVENT, mqttMessage); diff --git a/app/src/main/java/com/example/nativec/ApriltagDetector.java b/app/src/main/java/com/example/nativec/ApriltagDetector.java index b7608f4a..4ab456bb 100644 --- a/app/src/main/java/com/example/nativec/ApriltagDetector.java +++ b/app/src/main/java/com/example/nativec/ApriltagDetector.java @@ -268,6 +268,34 @@ public class ApriltagDetector { } } + // ── 联合 PnP（多 tag 时使用所有角点一次求解）── + if (detections.size() >= 2) { + try { + int n = detections.size(); + ApriltagDetection[] arr = new ApriltagDetection[n]; + double[] sizes = new double[n]; + double[] offsX = new double[n]; + double[] offsY = new double[n]; + for (int i = 0; i < n; i++) { + arr[i] = detections.get(i); + sizes[i] = getTagSize(arr[i].id); + double[] off = getTagOffset(arr[i].id); + offsX[i] = off[0]; + offsY[i] = off[1]; + } + ApriltagPose joint = ApriltagNative.estimate_joint_pose( + arr, sizes, offsX, offsY, fx, fy, cx, cy); + if (joint != null) { + result.setJointPose(joint); + } + } catch (Exception e) { + Log.w(TAG, "joint PnP failed: " + e.getMessage()); + } + } else if (detections.size() == 1) { + ApriltagPose lp = result.getBestLandingPose(); + if (lp != null) result.setJointPose(lp); + } + // ── 统计 ── long elapsed = System.currentTimeMillis() - t0; synchronized (lock) { diff --git a/app/src/main/jniLibs/arm64-v8a/libnativec.so b/app/src/main/jniLibs/arm64-v8a/libnativec.so new file mode 100644 index 00000000..5430e168 Binary files /dev/null and b/app/src/main/jniLibs/arm64-v8a/libnativec.so differ diff --git a/app/src/main/jniLibs/armeabi-v7a/libnativec.so b/app/src/main/jniLibs/armeabi-v7a/libnativec.so new file mode 100644 index 00000000..01ec52e2 Binary files /dev/null and b/app/src/main/jniLibs/armeabi-v7a/libnativec.so differ diff --git a/app/src/main/jniLibs/x86/libnativec.so b/app/src/main/jniLibs/x86/libnativec.so new file mode 100644 index 00000000..a01b62f0 Binary files /dev/null and b/app/src/main/jniLibs/x86/libnativec.so differ diff --git a/app/src/main/jniLibs/x86_64/libnativec.so b/app/src/main/jniLibs/x86_64/libnativec.so new file mode 100644 index 00000000..9c26d3c1 Binary files /dev/null and b/app/src/main/jniLibs/x86_64/libnativec.so differ diff --git a/app/src/main/res/layout-land/activity_main.xml b/app/src/main/res/layout-land/activity_main.xml index b3aa0aad..566f8987 100644 --- a/app/src/main/res/layout-land/activity_main.xml +++ b/app/src/main/res/layout-land/activity_main.xml @@ -318,6 +318,12 @@ android:layout_width="100dp" android:layout_height="100dp" /> +