From d6a6c505a1e36d4cfd38f95d6269451eb6cff8fa Mon Sep 17 00:00:00 2001 From: cxf <2417125293@qq.com> Date: Thu, 18 Jun 2026 15:04:36 +0800 Subject: [PATCH] =?UTF-8?q?=E9=98=B2=E6=AD=A2apc=E7=BA=BF=E7=A8=8B?= =?UTF-8?q?=E6=B1=A0=E7=88=86=E4=BA=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../aros/apron/activity/ConnectionActivity.kt | 10 +- .../com/aros/apron/activity/MainActivity.kt | 6 +- .../java/com/aros/apron/base/BaseManager.java | 87 ++------ .../com/aros/apron/callback/MqttCallBack.java | 8 +- .../main/java/com/aros/apron/entity/Osd.java | 18 ++ .../com/aros/apron/manager/FlightManager.java | 1 + .../com/aros/apron/manager/LTEManager.java | 27 ++- .../com/aros/apron/manager/MediaManager.java | 6 +- .../aros/apron/manager/MissionV3Manager.java | 14 +- .../com/aros/apron/manager/OSDManager.java | 12 + .../aros/apron/manager/RtspStreamManager.java | 6 +- .../com/aros/apron/manager/StreamManager.java | 10 +- .../apron/manager/TakeOffToPointManager.java | 5 +- .../com/aros/apron/tools/AprilTagPort.java | 72 +++--- .../aros/apron/tools/KmzParseController.java | 211 ++++++++++++++++++ .../com/aros/apron/tools/MqttManager.java | 5 +- 16 files changed, 374 insertions(+), 124 deletions(-) create mode 100644 app/src/main/java/com/aros/apron/tools/KmzParseController.java diff --git a/app/src/main/java/com/aros/apron/activity/ConnectionActivity.kt b/app/src/main/java/com/aros/apron/activity/ConnectionActivity.kt index 00a9f4fb..afa84ab0 100644 --- a/app/src/main/java/com/aros/apron/activity/ConnectionActivity.kt +++ b/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 { 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 c3eb15df..b135f06c 100644 --- a/app/src/main/java/com/aros/apron/activity/MainActivity.kt +++ b/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) 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 8ea298bf..255f59d2 100644 --- a/app/src/main/java/com/aros/apron/base/BaseManager.java +++ b/app/src/main/java/com/aros/apron/base/BaseManager.java @@ -43,78 +43,35 @@ 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()); - messageReply.setTid(entity.getTid()); - messageReply.setTimestamp(entity.getTimestamp()); - messageReply.setMethod(entity.getMethod()); - 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(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()); - } else { - LogUtil.log(TAG, "回复失败：mqtt 未连接, tid=" + entity.getTid()); - } - } catch (Exception e) { - e.printStackTrace(); - LogUtil.log(TAG, "回复异常：" + e.toString() + ", tid=" + entity.getTid()); + try { + if (MqttManager.getInstance().mqttAndroidClient.isConnected()) { + MessageReply messageReply = new MessageReply(); + messageReply.setBid(entity.getBid()); + messageReply.setTid(entity.getTid()); + messageReply.setTimestamp(entity.getTimestamp()); + messageReply.setMethod(entity.getMethod()); + 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(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()); + } else { + LogUtil.log(TAG, "回复失败：mqtt 未连接, tid=" + entity.getTid()); } - }); + } catch (Exception e) { + e.printStackTrace(); + LogUtil.log(TAG, "回复异常：" + e.toString() + ", tid=" + entity.getTid()); + } } - final Handler mainHandler = new Handler(Looper.getMainLooper()); + /** * 响应replay+event 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 0f8801a2..8109d648 100644 --- a/app/src/main/java/com/aros/apron/callback/MqttCallBack.java +++ b/app/src/main/java/com/aros/apron/callback/MqttCallBack.java @@ -139,7 +139,7 @@ public class MqttCallBack extends BaseManager implements MqttCallbackExtended { } else { switch (message.getMethod()) { case Constant.PILOT_ON: - // LogUtil.log(TAG, "收到：遥控器是否开机" + jsonString); + //LogUtil.log(TAG, "收到：遥控器是否开机" + jsonString); SystemManager.getInstance().checkRemoteControlPowerStatus(message); break; case Constant.AIRCRAFT_ON: @@ -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(); diff --git a/app/src/main/java/com/aros/apron/entity/Osd.java b/app/src/main/java/com/aros/apron/entity/Osd.java index 7ef310ac..d30a8174 100644 --- a/app/src/main/java/com/aros/apron/entity/Osd.java +++ b/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; } 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 04a3913b..1f7713dd 100644 --- a/app/src/main/java/com/aros/apron/manager/FlightManager.java +++ b/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() { @Override public void onSuccess(EmptyMsg emptyMsg) { + LogUtil.log(TAG,"返航执行成功"); sendMsg2Server(message); } 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 ebeeb0b0..619f4c12 100644 --- a/app/src/main/java/com/aros/apron/manager/LTEManager.java +++ b/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() { +// KeyManager.getInstance().listen(KeyTools.createKey(AirLinkKey.KeyWlmLinkQualityLevel), this, new CommonCallbacks.KeyListener() { +// @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() + ""); - Movement.getInstance().setGnd_quality_4g(t1.getLteLinkQualityLevel() + ""); - Movement.getInstance().setUav_quality_4g(t1.getLteLinkQualityLevel() + ""); + 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()+ ""); + Movement.getInstance().setUav_quality_4g(t1.getLinkQualityLevel().getLteUavSingalBar().value()+ ""); + } } } }); + + KeyManager.getInstance().listen(KeyTools.createKey(AirLinkKey.KeyDownLinkQualityRaw), this, new CommonCallbacks.KeyListener() { @Override public void onValueChange(@Nullable Integer integer, @Nullable Integer 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 635ee012..3f6e4a86 100644 --- a/app/src/main/java/com/aros/apron/manager/MediaManager.java +++ b/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(); - com.aros.apron.tools.SimplePortScanner.getInstance().stopScan(); + // StreamManager.getInstance().stopStreamRefreshTimer(); + //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() { 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 49fe4078..f453c70e 100644 --- a/app/src/main/java/com/aros/apron/manager/MissionV3Manager.java +++ b/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; diff --git a/app/src/main/java/com/aros/apron/manager/OSDManager.java b/app/src/main/java/com/aros/apron/manager/OSDManager.java index e5a6c00b..61fcc52f 100644 --- a/app/src/main/java/com/aros/apron/manager/OSDManager.java +++ b/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:有保养 diff --git a/app/src/main/java/com/aros/apron/manager/RtspStreamManager.java b/app/src/main/java/com/aros/apron/manager/RtspStreamManager.java index 99f1a856..cb3240fb 100644 --- a/app/src/main/java/com/aros/apron/manager/RtspStreamManager.java +++ b/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(); }); } 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 fb1ddc9e..0bb59912 100644 --- a/app/src/main/java/com/aros/apron/manager/StreamManager.java +++ b/app/src/main/java/com/aros/apron/manager/StreamManager.java @@ -112,8 +112,8 @@ public class StreamManager extends BaseManager { // ========== 【新增】重置推流状态，用于端口关闭后重启 ========== public void resetStreamState() { - stopStreamRefreshTimer(); - SimplePortScanner.getInstance().stopScan(); // 同时停止端口扫描 + //stopStreamRefreshTimer(); + // 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(); }); } diff --git a/app/src/main/java/com/aros/apron/manager/TakeOffToPointManager.java b/app/src/main/java/com/aros/apron/manager/TakeOffToPointManager.java index 9da9be58..1b005a6a 100644 --- a/app/src/main/java/com/aros/apron/manager/TakeOffToPointManager.java +++ b/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.上传指点航线 * 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 bf43969f..72b28d90 100644 --- a/app/src/main/java/com/aros/apron/tools/AprilTagPort.java +++ b/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 absY = Math.abs(smoothY); - double absYaw = Math.abs(smoothYaw); + // ── 偏差绝对值 ── + double absX = Math.abs(smoothX); // 米，降落点左右偏差 + double absY = Math.abs(smoothY); // 米，降落点前后偏差 + double absYaw = Math.abs(smoothYaw); // 度，偏航角偏差 - boolean aboveTwo = altM > 2.0; - boolean yawAligned = absYaw <= 5.0; + // ── 阶段判断 ── + boolean aboveTwo = altM > 2.0; // 2米以上 + boolean yawAligned = absYaw <= 5.0; // 5度以内算对准 - // ── X/Y 水平 PID：动态缩放 ── - double scaleXY = Math.max(altM * 0.6, 0.3); - double normX = smoothX / scaleXY; - double normY = smoothY / scaleXY; + // ── X/Y 水平 PID：动态缩放（类似 errPx/scaleFactor）── + double scaleXY = Math.max(altM * 0.6, 0.3); // 缩放因子，最小0.3 + double normX = smoothX / scaleXY; // 归一化输入（无单位） + double normY = smoothY / scaleXY; // 归一化输入（无单位） - double speedLimit = MAX_HORIZ_SPEED; - if (altM < 1.0) speedLimit = 0.2; - else if (altM < 2.0) speedLimit = 0.5; - else if (altM < 3.0) speedLimit = 0.8; - float maxV = (float) speedLimit; + // ── 水平速度上限：越低越慢，防过冲 ── + 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); - float vy = clamp(pidY.get_pid(), maxV); - if (absX < 0.02) vx = 0f; - if (absY < 0.02) vy = 0f; + 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时已转好，2m以下不转 ── - float yawRate = 0f; + // ── Yaw旋转：2m以上边转边降，2m以下不转 ── + 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; - else if (absYaw > 30.0) yawRate = 25.0f; - else if (absYaw > 15.0) yawRate = 15.0f; - else if (absYaw > 8.0) yawRate = 8.0f; - else yawRate = 5.0f; + 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; + 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); - float vz = baseDescent * alignFactor; + // 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", diff --git a/app/src/main/java/com/aros/apron/tools/KmzParseController.java b/app/src/main/java/com/aros/apron/tools/KmzParseController.java new file mode 100644 index 00000000..5424ba1e --- /dev/null +++ b/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航点解析（经纬度、悬停时间） +// * +// *

单文件自包含：解压 kmz -> 读取 wpmz/template.kml -> 解析航点。

+// * +// * @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> 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>().ok(parse(templateKml)); +// } catch (RenException e) { +// throw e; +// } catch (Exception e) { +// throw new RenException("kmz文件解析失败：" + e.getMessage()); +// } +// } +// +// @GetMapping("waypointByPath") +// @Operation(summary = "解析本地路径的KMZ航点（经纬度、悬停时间）") +// public Result> 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>().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 parse(byte[] kml) throws Exception { +// List 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 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 actionGroups = placemark.elements("actionGroup"); +// for (Element actionGroup : actionGroups) { +// List 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; +// } +// } +//} 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 1af7bf30..d927574c 100644 --- a/app/src/main/java/com/aros/apron/tools/MqttManager.java +++ b/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.setMaxInflight(1000);// 增加最大并发未确认消息数量 + mMqttConnectOptions.setAutomaticReconnect(true); + mMqttConnectOptions.setMaxInflight(1000);// 避免消息积压导致连接拥塞 mMqttConnectOptions.setCleanSession(true); //设置是否清除缓存 mMqttConnectOptions.setConnectionTimeout(30); //设置超时时间，单位：秒 ltz denote mMqttConnectOptions.setKeepAliveInterval(20); //设置心跳包发送间隔，单位：秒 ltz denote