防止apc线程池爆了

app/src/main/java/com/aros/apron/activity/ConnectionActivity.kt

@@ -28,7 +28,6 @@ import dji.sdk.keyvalue.key.KeyTools
 import dji.sdk.keyvalue.value.common.ComponentIndexType
 import dji.v5.manager.KeyManager
 import dji.v5.utils.common.StringUtils
-import org.opencv.android.OpenCVLoader
 
 
 open class ConnectionActivity : BaseActivity() {
@@ -338,6 +337,15 @@ open class ConnectionActivity : BaseActivity() {
     override fun onResume() {
         super.onResume()
         LogUtil.log(TAG,"进入首页连接")
+        // ===== AMS配置参数写入OSD和日志 =====
+        val closeObstacle = if (PreferenceUtils.getInstance().getCloseObsEnable()) 1 else 0
+        val interruptAction = PreferenceUtils.getInstance().getMissionInterruptAction()
+        val landType = PreferenceUtils.getInstance().getLandType()
+        val cameraLoc = PreferenceUtils.getInstance().getCameraLocationType()
+
+
+        LogUtil.log(TAG, String.format("AMS配置: 关闭避障=%d 中断动作=%d 降落方式=%d 相机位置=%d",
+            closeObstacle, interruptAction, landType, cameraLoc));
     }
 
     override fun useEventBus(): Boolean {

app/src/main/java/com/aros/apron/activity/MainActivity.kt

@@ -889,7 +889,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 +944,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 +1002,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)

app/src/main/java/com/aros/apron/base/BaseManager.java

@@ -43,54 +43,12 @@ public abstract class BaseManager {
 
     public String TAG = getClass().getSimpleName();
 
-
-    /**
-     * 推送psdkmovement
-     * @param
-     */
-    public void sendpsdkinfo2Server(){
-        try {
-            if (MqttManager.getInstance().mqttAndroidClient.isConnected()) {
-                MessageReply messageReply = new MessageReply();
-                MessageReply.Data data=new MessageReply.Data();
-                data.setResult(0);
-                messageReply.setData(data);
-                MqttMessage mqttMessage = new MqttMessage(new Gson().toJson(messageReply).getBytes("UTF-8"));
-                mqttMessage.setQos(0);
-                MqttManager.getInstance().mqttAndroidClient.publish(AMSConfig.UP_UAV_SERVICES_REPLY, mqttMessage);
-            } else {
-                LogUtil.log(TAG, "回复失败：mqtt 未连接");
-            }
-        } catch (Exception e) {
-            e.printStackTrace();
-            LogUtil.log(TAG, "回复异常：" + e.toString());
-        }
-
-
-
-    }
-    /**
-     * 推送psdkreply
-     * @param
-     */
-    public void sendpsdkreply2server(){
-
-
-
-
-
-    }
-
     /**
      * 响应reply
      * @param entity
      */
     public void sendMsg2Server(MessageDown entity) {
-        LogUtil.log(TAG, "sendMsg2Server 入口, method=" + entity.getMethod() + ", tid=" + entity.getTid());
-        // 直接在主线程发送（Paho 内部有独立发送线程，不会阻塞主线程）
-        mainHandler.post(() -> {
         try {
-                LogUtil.log(TAG, "sendMsg2Server 执行, connected=" + MqttManager.getInstance().mqttAndroidClient.isConnected() + ", tid=" + entity.getTid());
             if (MqttManager.getInstance().mqttAndroidClient.isConnected()) {
                 MessageReply messageReply = new MessageReply();
                 messageReply.setBid(entity.getBid());
@@ -112,9 +70,8 @@ public abstract class BaseManager {
             e.printStackTrace();
             LogUtil.log(TAG, "回复异常：" + e.toString() + ", tid=" + entity.getTid());
         }
-        });
     }
-    final Handler mainHandler = new Handler(Looper.getMainLooper());
+
 
     /**
      * 响应replay+event

app/src/main/java/com/aros/apron/callback/MqttCallBack.java

@@ -1082,6 +1082,12 @@ public class MqttCallBack extends BaseManager implements MqttCallbackExtended {
         LogUtil.log(TAG, "未检测到图传，5 秒后再次确认...");
 
         handler.postDelayed(() -> {
+            // 返航/降落状态下视频流停止是正常现象，不应重启App导致MQTT断连
+            int goHomeState = Movement.getInstance().getGoHomeState();
+            if (goHomeState == 1 || goHomeState == 2) {
+                LogUtil.log(TAG, "返航/降落中(goHomeState=" + goHomeState + ")，跳过VTX检测不重启");
+                return;
+            }
             if (!Movement.getInstance().isVtx()) {
                 // 图传仍未恢复 → 执行重启逻辑
                 int times = PreferenceUtils.getInstance().getRestartAMSTimes();

app/src/main/java/com/aros/apron/entity/Osd.java

@@ -118,6 +118,11 @@ public class Osd {
         private int wind_speed;
         private boolean ocu_sync_lte;
         private int ocu_sync_lte_time;
+        // ===== AMS配置参数 =====
+        private int close_obstacle_enable;      // 关闭全向避障 0=开启避障 1=关闭避障
+        private int mission_interrupt_action;   // 航线意外终止动作 1=悬停 2=继续 3=拉高返航
+        private int land_type;                  // 精准降落方式 1=RTK 2=视觉
+        private int camera_location_type;       // 主相机位置 1=中 2=右 3=下 4=融右 5=融中
 
 
         public double getRtk_takeoff_altitude() {
@@ -168,6 +173,19 @@ public class Osd {
             this.ocu_sync_lte_time = ocu_sync_lte_time;
         }
 
+        // ===== AMS配置参数 getters/setters =====
+        public int getClose_obstacle_enable() { return close_obstacle_enable; }
+        public void setClose_obstacle_enable(int close_obstacle_enable) { this.close_obstacle_enable = close_obstacle_enable; }
+
+        public int getMission_interrupt_action() { return mission_interrupt_action; }
+        public void setMission_interrupt_action(int mission_interrupt_action) { this.mission_interrupt_action = mission_interrupt_action; }
+
+        public int getLand_type() { return land_type; }
+        public void setLand_type(int land_type) { this.land_type = land_type; }
+
+        public int getCamera_location_type() { return camera_location_type; }
+        public void setCamera_location_type(int camera_location_type) { this.camera_location_type = camera_location_type; }
+
         public int getActivation_time() {
             return activation_time;
         }

app/src/main/java/com/aros/apron/manager/FlightManager.java

@@ -1541,6 +1541,7 @@ public class FlightManager extends BaseManager {
                 KeyManager.getInstance().performAction(createKey(FlightControllerKey.KeyStartGoHome), new CommonCallbacks.CompletionCallbackWithParam<EmptyMsg>() {
                     @Override
                     public void onSuccess(EmptyMsg emptyMsg) {
+                        LogUtil.log(TAG,"返航执行成功");
                         sendMsg2Server(message);
                     }
 

app/src/main/java/com/aros/apron/manager/LTEManager.java

@@ -56,16 +56,33 @@ public class LTEManager extends BaseManager {
                 }
             });
 
-            KeyManager.getInstance().listen(KeyTools.createKey(AirLinkKey.KeyWlmLinkQualityLevel), this, new CommonCallbacks.KeyListener<WlmLinkQualityLevelInfo>() {
+//            KeyManager.getInstance().listen(KeyTools.createKey(AirLinkKey.KeyWlmLinkQualityLevel), this, new CommonCallbacks.KeyListener<WlmLinkQualityLevelInfo>() {
+//                @Override
+//                public void onValueChange(@Nullable WlmLinkQualityLevelInfo wlmLinkQualityLevelInfo, @Nullable WlmLinkQualityLevelInfo t1) {
+//                    if (t1 != null) {
+//                        Movement.getInstance().setQuality_4g(t1.getLteLinkQualityLevel() + "");
+//                        Movement.getInstance().setGnd_quality_4g(t1.getLteLinkQualityLevel() + "");
+//                        Movement.getInstance().setUav_quality_4g(t1.getLteLinkQualityLevel() + "");
+//                    }
+//                }
+//            });
+
+
+            dji.v5.manager.aircraft.lte.LTEManager.getInstance().addLTELinkInfoListener(new LTELinkInfoListener() {
                 @Override
-                public void onValueChange(@Nullable WlmLinkQualityLevelInfo wlmLinkQualityLevelInfo, @Nullable WlmLinkQualityLevelInfo t1) {
+                public void onLTELinkInfoUpdate(LTELinkInfo t1) {
                     if (t1 != null) {
-                        Movement.getInstance().setQuality_4g(t1.getLteLinkQualityLevel() + "");
+                        LogUtil.log(TAG, "信号质量" + t1.getLinkQualityLevel());
-                        Movement.getInstance().setGnd_quality_4g(t1.getLteLinkQualityLevel() + "");
+                        if(t1.getLinkQualityLevel()!=null){
-                        Movement.getInstance().setUav_quality_4g(t1.getLteLinkQualityLevel() + "");
+                            Movement.getInstance().setQuality_4g(t1.getLinkQualityLevel().getLteGndSingalBar().value()+ "");
+                            Movement.getInstance().setGnd_quality_4g(t1.getLinkQualityLevel().getOcuSyncSignalQualityLevel().value()+ "");
+                            Movement.getInstance().setUav_quality_4g(t1.getLinkQualityLevel().getLteUavSingalBar().value()+ "");
+                        }
                     }
                 }
             });
+
+
             KeyManager.getInstance().listen(KeyTools.createKey(AirLinkKey.KeyDownLinkQualityRaw), this, new CommonCallbacks.KeyListener<Integer>() {
                 @Override
                 public void onValueChange(@Nullable Integer integer, @Nullable Integer value) {

app/src/main/java/com/aros/apron/manager/MediaManager.java

@@ -116,8 +116,8 @@ public class MediaManager extends BaseManager {
         isPlaybackEnabling = true;
 
         // 停止端口扫描和 RTSP 刷新，关闭 RTSP 推流，确保媒体上传稳定
-        StreamManager.getInstance().stopStreamRefreshTimer();
+       // StreamManager.getInstance().stopStreamRefreshTimer();
-        com.aros.apron.tools.SimplePortScanner.getInstance().stopScan();
+        //com.aros.apron.tools.SimplePortScanner.getInstance().stopScan();
         StreamManager.getInstance().stopstream();
 
         // 仅首次调用时清理状态，重试时保留计数器
@@ -675,7 +675,7 @@ public class MediaManager extends BaseManager {
     //退出媒体模式
     public void  disablePlayback() {
         // 任务结束，停止视频流刷新定时器
-        StreamManager.getInstance().stopStreamRefreshTimer();
+      //  StreamManager.getInstance().stopStreamRefreshTimer();
         MediaDataCenter.getInstance().getMediaManager().disable(new CommonCallbacks.CompletionCallback() {
             @Override
             public void onSuccess() {

app/src/main/java/com/aros/apron/manager/MissionV3Manager.java

@@ -236,7 +236,7 @@ public class MissionV3Manager extends BaseManager {
 
                                 //航线飞行
                                 if (Movement.getInstance().getFlightmode() == 1) {
-                                    Movement.getInstance().setMode_code(5);
+
                                 } else if (Movement.getInstance().getFlightmode() == 2) {
                                     //指令飞行
                                     Movement.getInstance().setMode_code(17);
@@ -255,6 +255,13 @@ public class MissionV3Manager extends BaseManager {
 //                                    Movement.getInstance().setTask_status("in_progress");
 //                                }
 
+                                //航线飞行
+                                if (Movement.getInstance().getFlightmode() == 1) {
+                                    Movement.getInstance().setMode_code(5);
+                                } else if (Movement.getInstance().getFlightmode() == 2) {
+
+                                }
+
                                 Movement.getInstance().setVirtualStickQuitMission(false);
 
 
@@ -617,17 +624,20 @@ public class MissionV3Manager extends BaseManager {
             //4.返航或降落状态无法执行航线
             if (Movement.getInstance().getGoHomeState() == 1 || Movement.getInstance().getGoHomeState() == 2) {
                 sendFailMsg2Server(message, "返航中,无法执行航线任务");
+                LogUtil.log(TAG,"返航中,无法执行航线任务");
                 return false;
             }
             //5.检查航线备降点参数
             if (message.getData().getAlternate_land_point() == null) {
                 sendEvent2Server("备降点参数异常", 2);
+                LogUtil.log(TAG,"备降点参数异常");
                 TaskFailManager.getInstance().sendTaskFailMsg2Server(-1);
                 return false;
             }
             //6.检查航线参数
             if (message.getData().getFile() == null) {
                 sendEvent2Server("航线参数异常", 2);
+                LogUtil.log(TAG,"航线参数异常");
                 TaskFailManager.getInstance().sendTaskFailMsg2Server(-1);
                 return false;
             }
@@ -636,6 +646,7 @@ public class MissionV3Manager extends BaseManager {
                     KeyBatteryPowerPercent, 0));
             if (value != null && value < Integer.parseInt(PreferenceUtils.getInstance().getMinumumBattery())) {
                 sendEvent2Server("任务执行失败,电量过低", 2);
+                LogUtil.log(TAG,"任务执行失败,电量过低");
                 TaskFailManager.getInstance().sendTaskFailMsg2Server(ErrorCode.DEVICE_BATTERY_LEVEL_TOO_LOW);
                 return false;
             }
@@ -643,6 +654,7 @@ public class MissionV3Manager extends BaseManager {
                     KeyManager.getInstance().getValue(KeyTools.createKey(FlightControllerKey.KeyRemoteControllerFlightMode));
             if (remoteControllerFlightMode != null && remoteControllerFlightMode != RemoteControllerFlightMode.P) {
                 sendEvent2Server("任务执行失败,请将遥控器切换为P/N挡", 2);
+                LogUtil.log(TAG,"任务执行失败,请将遥控器切换为P/N挡");
                 TaskFailManager.getInstance().sendTaskFailMsg2Server(-1);
                 return false;
 

app/src/main/java/com/aros/apron/manager/OSDManager.java

@@ -16,6 +16,7 @@ import com.aros.apron.entity.Movement;
 import com.aros.apron.entity.Osd;
 import com.aros.apron.tools.LogUtil;
 import com.aros.apron.tools.MqttManager;
+import com.aros.apron.tools.PreferenceUtils;
 import com.aros.apron.tools.TakeoffProgressScheduler;
 import com.google.gson.Gson;
 import com.google.gson.GsonBuilder;
@@ -284,6 +285,17 @@ public class OSDManager extends BaseManager {
             data.setOcu_sync_lte(Movement.getInstance().isOcuSyncLte());
             data.setOcu_sync_lte_time(Movement.getInstance().getOcuSyncLteTime());
 
+            // ===== AMS配置参数写入OSD和日志 =====
+            int closeObstacle = PreferenceUtils.getInstance().getCloseObsEnable() ? 1 : 0;
+            data.setClose_obstacle_enable(closeObstacle);
+            int interruptAction = PreferenceUtils.getInstance().getMissionInterruptAction();
+            data.setMission_interrupt_action(interruptAction);
+            int landType = PreferenceUtils.getInstance().getLandType();
+            data.setLand_type(landType);
+            int cameraLoc = PreferenceUtils.getInstance().getCameraLocationType();
+            data.setCamera_location_type(cameraLoc);
+
+
             Osd.Data.MaintainStatus maintainStatus = new Osd.Data.MaintainStatus();
             Osd.Data.MaintainStatus.MaintainStatusArray maintainItem = new Osd.Data.MaintainStatus.MaintainStatusArray();
             // state: 保养状态, enum_int, 0:无保养, 1:有保养

app/src/main/java/com/aros/apron/manager/RtspStreamManager.java

@@ -247,7 +247,7 @@ public class RtspStreamManager {
                         mainHandler.post(() -> {
                             LogUtil.log(TAG, "强制启动RTSP推流成功");
                             isStreaming.set(true);
-                            startPortScanner();
+                            //startPortScanner();
                         });
                     }
 
@@ -303,7 +303,7 @@ public class RtspStreamManager {
                 mainHandler.post(() -> {
                     LogUtil.log(TAG, "RTSP推流启动成功" + (isRestart ? "(重启)" : ""));
                     isStreaming.set(true);
-                    startPortScanner();
+                    //startPortScanner();
                 });
             }
 
@@ -343,7 +343,7 @@ public class RtspStreamManager {
                         mainHandler.post(() -> {
                             LogUtil.log(TAG, "推流停止成功");
                             isStreaming.set(false);
-                            SimplePortScanner.getInstance().stopScan();
+                           // SimplePortScanner.getInstance().stopScan();
                         });
                     }
 

app/src/main/java/com/aros/apron/manager/StreamManager.java

@@ -112,8 +112,8 @@ public class StreamManager extends BaseManager {
 
     // ========== 【新增】重置推流状态，用于端口关闭后重启 ==========
     public void resetStreamState() {
-        stopStreamRefreshTimer();
+        //stopStreamRefreshTimer();
-        SimplePortScanner.getInstance().stopScan(); // 同时停止端口扫描
+       // SimplePortScanner.getInstance().stopScan(); // 同时停止端口扫描
         mainHandler.removeCallbacksAndMessages(null); // 清理所有待执行的回调
         startLiveFailTimes = 0;
         isLiveStreamAlreadyStart = false;
@@ -277,6 +277,7 @@ public class StreamManager extends BaseManager {
 
     public void switchptspfpv(ComponentIndexType ComponentIndex, MessageDown message) {
         if(isliveindex==2){
+            sendMsg2Server(message);
             return;
         }
         isliveindex = 2;
@@ -302,6 +303,7 @@ public class StreamManager extends BaseManager {
 
     public void switchptspport(ComponentIndexType ComponentIndex, MessageDown message) {
         if(isliveindex==1){
+            sendMsg2Server(message);
             return;
         }
         isliveindex = 1;
@@ -470,9 +472,9 @@ public class StreamManager extends BaseManager {
                     isStartingRTSP = false; // 重置并发标志
                     LogUtil.log(TAG, "========== RTSP 推流启动成功 ==========");
                     // 启动5秒定时刷新视频流，防止起飞卡死
-                    startStreamRefreshTimer();
+                   // startStreamRefreshTimer();
                     // 开始端口扫描
-                    SimplePortScanner.getInstance().startScan();
+                   // SimplePortScanner.getInstance().startScan();
                 });
             }
 

app/src/main/java/com/aros/apron/manager/TakeOffToPointManager.java

@@ -129,11 +129,13 @@ public class TakeOffToPointManager extends BaseManager {
             if (Movement.getInstance().getGoHomeState() == 1
                     || Movement.getInstance().getGoHomeState() == 2) {
                 sendFailMsg2Server(message, "返航中,无法执行航线任务");
+                LogUtil.log(TAG,"返航中,无法执行航线任务");
                 return false;
             }
             //5.检查航线备降点参数
             if (message.getData().getAlternate_land_point() == null) {
                 sendEvent2Server("备降点参数异常", 2);
+                LogUtil.log(TAG,"备降点参数异常");
                 TaskFailManager.getInstance().sendTaskFailMsg2Server(-1);
                 return false;
             }
@@ -148,6 +150,7 @@ public class TakeOffToPointManager extends BaseManager {
                     KeyBatteryPowerPercent, 0));
             if (value != null && value < Integer.parseInt(PreferenceUtils.getInstance().getMinumumBattery())) {
                 sendEvent2Server("任务执行失败,电量过低", 2);
+                LogUtil.log(TAG,"任务执行失败,电量过低");
                 TaskFailManager.getInstance().sendTaskFailMsg2Server(ErrorCode.DEVICE_BATTERY_LEVEL_TOO_LOW);
                 return false;
             }
@@ -155,6 +158,7 @@ public class TakeOffToPointManager extends BaseManager {
                     KeyManager.getInstance().getValue(KeyTools.createKey(FlightControllerKey.KeyRemoteControllerFlightMode));
             if (remoteControllerFlightMode != null && remoteControllerFlightMode != RemoteControllerFlightMode.P) {
                 sendEvent2Server("任务执行失败,请将遥控器切换为P/N挡", 2);
+                LogUtil.log(TAG,"任务执行失败,请将遥控器切换为P/N挡");
                 TaskFailManager.getInstance().sendTaskFailMsg2Server(-1);
                 return false;
             }
@@ -277,7 +281,6 @@ public class TakeOffToPointManager extends BaseManager {
         }
         return false;
     }
-
     /**
      * 6.上传指点航线
      *

app/src/main/java/com/aros/apron/tools/AprilTagPort.java

@@ -244,63 +244,67 @@ public class AprilTagPort {
                     return;
                 }
 
-                // ── 高度：纯AprilTag ──
+                // ── 高度来源 ──
                 // （备用：超声波兜底。需要时取消注释）
                 // double ultraM = ultrasonicHeight / 10.0;
                 // double altM = (ultrasonicHeight > 0 && ultrasonicHeight <= 50)
                 //     ? ultraM : smoothZ;
-                double altM = smoothZ;
+                double altM = smoothZ; // 单位：米，AprilTag位姿Z分量（相机离地高度）
 
-                // ── 统一 PID：2m以上边转边降，2m以下只平移 ──
+                // ── 偏差绝对值 ──
-                double absX = Math.abs(smoothX);
+                double absX = Math.abs(smoothX);   // 米，降落点左右偏差
-                double absY = Math.abs(smoothY);
+                double absY = Math.abs(smoothY);   // 米，降落点前后偏差
-                double absYaw = Math.abs(smoothYaw);
+                double absYaw = Math.abs(smoothYaw); // 度，偏航角偏差
 
-                boolean aboveTwo = altM > 2.0;
+                // ── 阶段判断 ──
-                boolean yawAligned = absYaw <= 5.0;
+                boolean aboveTwo = altM > 2.0;     // 2米以上
+                boolean yawAligned = absYaw <= 5.0; // 5度以内算对准
 
-                // ── X/Y 水平 PID：动态缩放 ──
+                // ── X/Y 水平 PID：动态缩放（类似 errPx/scaleFactor）──
-                double scaleXY = Math.max(altM * 0.6, 0.3);
+                double scaleXY = Math.max(altM * 0.6, 0.3); // 缩放因子，最小0.3
-                double normX = smoothX / scaleXY;
+                double normX = smoothX / scaleXY;   // 归一化输入（无单位）
-                double normY = smoothY / scaleXY;
+                double normY = smoothY / scaleXY;   // 归一化输入（无单位）
 
-                double speedLimit = MAX_HORIZ_SPEED;
+                // ── 水平速度上限：越低越慢，防过冲 ──
-                if (altM < 1.0)       speedLimit = 0.2;
+                double speedLimit = MAX_HORIZ_SPEED;   // >3m: 1.2 m/s
-                else if (altM < 2.0)  speedLimit = 0.5;
+                if (altM < 1.0)       speedLimit = 0.2;  // <1m:   0.2 m/s
-                else if (altM < 3.0)  speedLimit = 0.8;
+                else if (altM < 2.0)  speedLimit = 0.5;  // 1~2m:  0.5 m/s
-                float maxV = (float) speedLimit;
+                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);
+                float vx = clamp(pidX.get_pid(), maxV);  // m/s，前后
-                float vy = clamp(pidY.get_pid(), maxV);
+                float vy = clamp(pidY.get_pid(), maxV);  // m/s，左右
-                if (absX < 0.02) vx = 0f;
+                if (absX < 0.02) vx = 0f;  // <2cm死区
-                if (absY < 0.02) vy = 0f;
+                if (absY < 0.02) vy = 0f;  // <2cm死区
 
-                // ── Yaw：2m以上边转边降，确保到2m时已转好，2m以下不转 ──
+                // ── Yaw旋转：2m以上边转边降，2m以下不转 ──
-                float yawRate = 0f;
+                float yawRate = 0f; // °/s
                 if (aboveTwo && !yawAligned) {
-                    boolean nearTwo = altM <= 3.0; // 2-3m紧急修正区
+                    boolean nearTwo = altM <= 3.0; // 2~3m：紧急修正区
                     if (nearTwo) {
-                        // 接近2m：全力转，确保到2m时对准
+                        // 全力转，确保到2m时对准
                         yawRate = (absYaw > 10.0) ? 40.0f : 8.0f;
                     } else {
                         // 3m以上：从容修正
-                        if (absYaw > 60.0)      yawRate = 35.0f;
+                        if (absYaw > 60.0)      yawRate = 35.0f; // >60°偏差
-                        else if (absYaw > 30.0) yawRate = 25.0f;
+                        else if (absYaw > 30.0) yawRate = 25.0f; // 30~60°
-                        else if (absYaw > 15.0) yawRate = 15.0f;
+                        else if (absYaw > 15.0) yawRate = 15.0f; // 15~30°
-                        else if (absYaw > 8.0)  yawRate = 8.0f;
+                        else if (absYaw > 8.0)  yawRate = 8.0f;  // 8~15°
-                        else                     yawRate = 5.0f;
+                        else                     yawRate = 5.0f;  // 5~8°
                     }
-                    yawRate = (smoothYaw > 0) ? -yawRate : yawRate;
+                    yawRate = (smoothYaw > 0) ? -yawRate : yawRate; // 反向修正
                 }
 
                 // ── 垂直速度：一路降到底 ──
-                float baseDescent = mapHeightToDescent((float) altM);
+                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));
-                alignFactor = Math.max(0.15f, alignFactor);
+                // absX=0.3m→0.0, absX=0m→1.0, absX>=0.255m→0.15
-                float vz = baseDescent * alignFactor;
+                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",

app/src/main/java/com/aros/apron/tools/KmzParseController.java

@@ -0,0 +1,211 @@
+//package com.aros.apron.tools;
+//
+//import org.dom4j.Document;
+//import org.dom4j.Element;
+//import org.dom4j.io.SAXReader;
+//
+//import java.io.ByteArrayInputStream;
+//import java.io.ByteArrayOutputStream;
+//import java.io.InputStream;
+//import java.nio.file.Files;
+//import java.nio.file.Path;
+//import java.nio.file.Paths;
+//import java.util.ArrayList;
+//import java.util.List;
+//import java.util.zip.ZipEntry;
+//import java.util.zip.ZipInputStream;
+//
+///**
+// * KMZ航点解析（经纬度、悬停时间）
+// *
+// * <p>单文件自包含：解压 kmz -> 读取 wpmz/template.kml -> 解析航点。</p>
+// *
+// * @author 938693313@qq.com
+// * @since 1.0.0 2026-06-09
+// */
+//public class KmzParseController {
+//
+//    /** kmz 内航线模板文件路径 */
+//    private static final String TEMPLATE_KML = "wpmz/template.kml";
+//
+//    @PostMapping("waypoint")
+//    @Operation(summary = "解析KMZ航点（经纬度、悬停时间）")
+//    public Result<List<Waypoint>> parseWaypoint(
+//            @Parameter(name = "file", description = "kmz文件") @RequestParam("file") MultipartFile file) {
+//        if (file == null || file.isEmpty()) {
+//            throw new RenException("kmz文件不能为空");
+//        }
+//
+//        try (InputStream in = file.getInputStream()) {
+//            byte[] templateKml = readTemplateKml(in);
+//            if (templateKml == null) {
+//                throw new RenException("kmz文件中未找到 " + TEMPLATE_KML);
+//            }
+//            return new Result<List<Waypoint>>().ok(parse(templateKml));
+//        } catch (RenException e) {
+//            throw e;
+//        } catch (Exception e) {
+//            throw new RenException("kmz文件解析失败：" + e.getMessage());
+//        }
+//    }
+//
+//    @GetMapping("waypointByPath")
+//    @Operation(summary = "解析本地路径的KMZ航点（经纬度、悬停时间）")
+//    public Result<List<Waypoint>> parseWaypointByPath(
+//            @Parameter(name = "path", description = "kmz文件本地绝对路径") @RequestParam("path") String path) {
+//        if (StringUtils.isBlank(path)) {
+//            throw new RenException("kmz文件路径不能为空");
+//        }
+//        Path filePath = Paths.get(path.trim());
+//        if (!Files.exists(filePath) || Files.isDirectory(filePath)) {
+//            throw new RenException("kmz文件不存在：" + path);
+//        }
+//
+//        try (InputStream in = Files.newInputStream(filePath)) {
+//            byte[] templateKml = readTemplateKml(in);
+//            if (templateKml == null) {
+//                throw new RenException("kmz文件中未找到 " + TEMPLATE_KML);
+//            }
+//            return new Result<List<Waypoint>>().ok(parse(templateKml));
+//        } catch (RenException e) {
+//            throw e;
+//        } catch (Exception e) {
+//            throw new RenException("kmz文件解析失败：" + e.getMessage());
+//        }
+//    }
+//
+//    /**
+//     * 从 kmz（zip）流中读取 wpmz/template.kml 的字节内容。
+//     */
+//    private byte[] readTemplateKml(InputStream in) throws Exception {
+//        try (ZipInputStream zis = new ZipInputStream(in)) {
+//            ZipEntry entry;
+//            while ((entry = zis.getNextEntry()) != null) {
+//                if (TEMPLATE_KML.equals(entry.getName())) {
+//                    ByteArrayOutputStream bos = new ByteArrayOutputStream();
+//                    byte[] buffer = new byte[8192];
+//                    int len;
+//                    while ((len = zis.read(buffer)) > 0) {
+//                        bos.write(buffer, 0, len);
+//                    }
+//                    return bos.toByteArray();
+//                }
+//                zis.closeEntry();
+//            }
+//        }
+//        return null;
+//    }
+//
+//    /**
+//     * 解析 template.kml，提取每个航点的经纬度与悬停时间。
+//     * 同一航点下存在多个 hover 动作时，悬停时间累加。
+//     */
+//    private List<Waypoint> parse(byte[] kml) throws Exception {
+//        List<Waypoint> result = new ArrayList<>();
+//
+//        SAXReader reader = new SAXReader();
+//        Document document = reader.read(new ByteArrayInputStream(kml));
+//        Element root = document.getRootElement();
+//
+//        Element doc = root.element("Document");
+//        if (doc == null) {
+//            throw new RenException("kml文件有误，无法解析");
+//        }
+//        Element folder = doc.element("Folder");
+//        if (folder == null) {
+//            throw new RenException("模板信息不存在，无法解析");
+//        }
+//
+//        List<Element> placemarks = folder.elements("Placemark");
+//        if (placemarks == null || placemarks.isEmpty()) {
+//            throw new RenException("航点信息不存在，无法解析");
+//        }
+//
+//        for (Element placemark : placemarks) {
+//            Waypoint waypoint = new Waypoint();
+//
+//            // 航点序号
+//            String index = placemark.elementText("index");
+//            waypoint.setIndex(StringUtils.isBlank(index) ? null : Integer.valueOf(index));
+//
+//            // 经纬度：coordinates 格式为 "经度,纬度"
+//            Element point = placemark.element("Point");
+//            if (point != null) {
+//                String coordinates = point.elementText("coordinates");
+//                if (StringUtils.isNotBlank(coordinates)) {
+//                    String[] split = coordinates.replaceAll("\\s+", "").split(",");
+//                    if (split.length >= 2) {
+//                        waypoint.setLongitude(Double.valueOf(split[0]));
+//                        waypoint.setLatitude(Double.valueOf(split[1]));
+//                    }
+//                }
+//            }
+//
+//            // 悬停时间：累加该航点所有 actionGroup 下 hover 动作的 hoverTime（秒）
+//            int hoverTime = 0;
+//            List<Element> actionGroups = placemark.elements("actionGroup");
+//            for (Element actionGroup : actionGroups) {
+//                List<Element> actions = actionGroup.elements("action");
+//                for (Element action : actions) {
+//                    if (!"hover".equals(action.elementText("actionActuatorFunc"))) {
+//                        continue;
+//                    }
+//                    Element funcParam = action.element("actionActuatorFuncParam");
+//                    if (funcParam == null) {
+//                        continue;
+//                    }
+//                    String hover = funcParam.elementText("hoverTime");
+//                    if (StringUtils.isNotBlank(hover)) {
+//                        hoverTime += (int) Math.round(Double.parseDouble(hover));
+//                    }
+//                }
+//            }
+//            waypoint.setHoverTime(hoverTime);
+//
+//            result.add(waypoint);
+//        }
+//        return result;
+//    }
+//
+//    /**
+//     * 航点解析结果。
+//     */
+//    public static class Waypoint {
+//        /** 航点序号 */
+//        private Integer index;
+//        /** 经度 */
+//        private Double longitude;
+//        /** 纬度 */
+//        private Double latitude;
+//        /** 悬停时间（秒，同一航点多个hover动作累加） */
+//        private Integer hoverTime;
+//
+//        public Integer getIndex() {
+//            return index;
+//        }
+//
+//        public void setIndex(Integer index) {
+//            this.index = index;
+//        }
+//
+//        public Double getLongitude() {
+//            return longitude;
+//        }
+//
+//        public void setLongitude(Double longitude) {
+//            this.longitude = longitude;
+//        }
+//
+//        public Double getLatitude() {
+//            return latitude;
+//        }
+//
+//        public void setLatitude(Double latitude) {
+//            this.latitude = latitude;
+//        }
+//
+//        public void setHoverTime(Integer hoverTime) {
+//            this.hoverTime = hoverTime;
+//        }
+//    }
+//}

app/src/main/java/com/aros/apron/tools/MqttManager.java

@@ -42,12 +42,11 @@ public class MqttManager {
     public void needConnect() {
         initMqttClientParams();
     }
-
     private void initMqttClientParams() {
         mqttAndroidClient = new MqttAndroidClient(ApronApp.Companion.getApplication(), AMSConfig.getInstance().getMqttServerUri(), generateRandomString(10));
         mMqttConnectOptions = new MqttConnectOptions();
-        mMqttConnectOptions.setAutomaticReconnect(true); //ltz add
+        mMqttConnectOptions.setAutomaticReconnect(true);
-        mMqttConnectOptions.setMaxInflight(1000);// 增加最大并发未确认消息数量
+        mMqttConnectOptions.setMaxInflight(1000);// 避免消息积压导致连接拥塞
         mMqttConnectOptions.setCleanSession(true); //设置是否清除缓存
         mMqttConnectOptions.setConnectionTimeout(30); //设置超时时间，单位：秒 ltz denote
         mMqttConnectOptions.setKeepAliveInterval(20); //设置心跳包发送间隔，单位：秒 ltz denote