防止apc线程池爆了
This commit is contained in:
parent
e3c49008ef
commit
d6a6c505a1
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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() + "");
|
||||
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<Integer>() {
|
||||
@Override
|
||||
public void onValueChange(@Nullable Integer integer, @Nullable Integer value) {
|
||||
|
|
|
|||
|
|
@ -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() {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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:有保养
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
});
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
});
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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.上传指点航线
|
||||
*
|
||||
|
|
|
|||
|
|
@ -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",
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue