From e284fc10a4dbbed14b4a16eed05fe9bac2f10565 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BC=A0=E4=BA=9A=E9=B9=8F?= <1480083907@qq.com> Date: Sat, 11 Jul 2026 17:52:25 +0800 Subject: [PATCH] update --- sample/build.gradle | 1 - .../core/cloud/CloudCommandExecutor.kt | 753 +++++++++++++++--- .../core/livestream/LiveStreamingService.kt | 35 +- .../core/mission/MissionModels.kt | 16 +- .../core/mission/WaypointMissionService.kt | 49 ++ .../core/mission/WpmlKmzBuilder.kt | 34 +- .../core/telemetry/TelemetryRepository.kt | 9 +- .../ui/DroneControllerScreen.kt | 4 +- 8 files changed, 769 insertions(+), 132 deletions(-) diff --git a/sample/build.gradle b/sample/build.gradle index 6ec5d00..f9732ae 100644 --- a/sample/build.gradle +++ b/sample/build.gradle @@ -130,7 +130,6 @@ dependencies { implementation deps.legacySupport implementation deps.lifecycleViewModel implementation deps.lifecycleLiveData - implementation deps.leakcanary implementation deps.glide implementation deps.dynamicanimation implementation deps.expandedit diff --git a/sample/src/main/java/com/zklh/dronecontroller/core/cloud/CloudCommandExecutor.kt b/sample/src/main/java/com/zklh/dronecontroller/core/cloud/CloudCommandExecutor.kt index 3267005..ebb4a59 100644 --- a/sample/src/main/java/com/zklh/dronecontroller/core/cloud/CloudCommandExecutor.kt +++ b/sample/src/main/java/com/zklh/dronecontroller/core/cloud/CloudCommandExecutor.kt @@ -1,18 +1,23 @@ package com.zklh.dronecontroller.core.cloud import android.util.Log +import com.zklh.dronecontroller.core.diagnostics.DroneWarningItem +import com.zklh.dronecontroller.core.diagnostics.DroneWarningState import com.zklh.dronecontroller.core.flight.FlightControlService import com.zklh.dronecontroller.core.flight.FlyToService import com.zklh.dronecontroller.core.gimbal.GimbalControlService import com.zklh.dronecontroller.core.livestream.LiveStreamingService import com.zklh.dronecontroller.core.media.CameraControlService import com.zklh.dronecontroller.core.media.CameraMediaService +import com.zklh.dronecontroller.core.mission.MissionDroneInfo import com.zklh.dronecontroller.core.mission.MissionFinishAction +import com.zklh.dronecontroller.core.mission.MissionPayloadInfo import com.zklh.dronecontroller.core.mission.MissionPlan import com.zklh.dronecontroller.core.mission.MissionWaypoint import com.zklh.dronecontroller.core.mission.WaypointMissionService import com.zklh.dronecontroller.core.mission.WpmlKmzBuilder import com.zklh.dronecontroller.core.msdk.DjiCommandResult +import com.zklh.dronecontroller.core.telemetry.TelemetryCameraIdentity import com.zklh.dronecontroller.core.telemetry.TelemetrySnapshot import com.zklh.dronecontroller.core.telemetry.hasReliableRtkPosition import com.zklh.dronecontroller.core.telemetry.isRtkBlockingWaypointMission @@ -25,11 +30,9 @@ import java.io.File import java.net.URL import java.util.concurrent.ConcurrentHashMap import java.util.concurrent.atomic.AtomicBoolean -import kotlin.math.asin import kotlin.math.atan2 import kotlin.math.cos import kotlin.math.abs -import kotlin.math.min import kotlin.math.roundToInt import kotlin.math.sin import kotlin.math.sqrt @@ -44,15 +47,11 @@ import org.json.JSONObject private const val CloudCommandLogTag = "ZklhCloudCommand" private const val DefaultFlyToHeightMeters = 20.0 private const val MinimumTakeoffTargetHeightMeters = 2.0 -private const val MinimumWaylineTakeoffHeightMeters = 20.0 -private const val WaylineInitialTakeoffReadyTimeoutMs = 45_000L -private const val WaylineInitialTakeoffSettleDelayMs = 3_000L private const val WaylineStartRetryCount = 6 -private const val WaylineDirectGroundStartRetryCount = 2 private const val WaylineStartRetryDelayMs = 2_000L +private const val WaypointRtkReadyWaitMs = 25_000L +private const val WaypointRtkReadyPollMs = 1_000L private const val TemporaryWaylineExitSettleDelayMs = 1_500L -private const val TemporaryWaylineDepartureOffsetMeters = 5.0 -private const val TemporaryWaylineSamePointThresholdMeters = 2.0 private const val PointFlightProgressIntervalMs = 5_000L private const val PointFlightProgressMonitorTimeoutMs = 30 * 60 * 1_000L private const val PointFlightArriveThresholdMeters = 3.0 @@ -61,6 +60,34 @@ private const val CoordPi = 3.1415926535897932384626 private const val CoordSemiMajorAxis = 6378245.0 private const val CoordEccentricity = 0.00669342162296594323 private const val CoordEarthRadiusMeters = 6_371_000.0 +private const val WpmlUnknownEnumValue = 65_535 +private const val WpmlM350DroneEnumValue = 89 +private const val WpmlH20TPayloadEnumValue = 43 +private const val TemporaryWaylineMinimumTakeoffHeightMeters = 2.0 +private const val TemporaryWaylineDefaultGroundTakeoffHeightMeters = 20.0 +private const val TemporaryWaylineLeadInDistanceMeters = 6.0 +private const val TemporaryWaylineGroundLeadInDistanceMeters = 6.0 +private const val TemporaryWaylineGroundVerticalDistanceThresholdMeters = 5.0 +private const val TemporaryWaylineGroundVerticalSecondPointOffsetMeters = 2.0 +private const val TemporaryWaylineGroundStartMaxSpeedMetersPerSecond = 5.0 +private const val TemporaryWaylineMinimumDistinctWaypointDistanceMeters = 3.0 +private const val M350NativeTakeoffBeforeWaylineReadyTimeoutMs = 30_000L +private const val M350NativeTakeoffBeforeWaylinePollMs = 500L +private const val M350NativeTakeoffBeforeWaylineSettleMs = 4_000L +private const val M350TakeoffToPointManualClimbHeightMeters = 20.0 +private const val M350TakeoffToPointManualClimbToleranceMeters = 0.6 +private const val M350TakeoffToPointManualClimbTimeoutMs = 60_000L +private const val M350TakeoffToPointManualClimbPollMs = 100L +private const val M350TakeoffToPointManualClimbSettleMs = 2_000L +private const val M350TakeoffToPointManualClimbStickRatio = 0.42 +private const val PointFlightTakeoffStallCheckAfterMs = 20_000L +private const val PointFlightTakeoffStallAltitudeMeters = 1.5 +private const val PointFlightTakeoffStallVerticalSpeedMeters = 0.25 +private const val NeutralStickDeadBand = 3 +private const val DjiHmsUnableTakeoffCode = "0x1F100031" +private const val DjiHmsUnableTakeoffCodeAlt = "0x1F100032" +private const val DjiHmsHeightLimitZoneCode = "0x1F100056" +private const val DjiHmsPoorRtkSignalCode = "0x16100066" interface CloudMissionProgressPublisher { fun publishFlightTaskProgress( @@ -96,6 +123,7 @@ class CloudCommandExecutor( private val gimbalControl: GimbalControlService = GimbalControlService(), private val mediaUpload: CloudMediaUploadService? = null, private val telemetryProvider: () -> TelemetrySnapshot = { TelemetrySnapshot() }, + private val warningProvider: () -> DroneWarningState = { DroneWarningState() }, private val progressPublisher: CloudMissionProgressPublisher? = null ) { private val scope = CoroutineScope(SupervisorJob() + Dispatchers.Default) @@ -215,6 +243,22 @@ class CloudCommandExecutor( private suspend fun executeStickControl( data: JSONObject ): DjiCommandResult { + val position = if (data.hasAny("roll", "pitch", "throttle", "yaw")) { + data.toDrcStickPosition() + } else { + data.toProtocolStickPosition() + } + val now = System.currentTimeMillis() + if (position.isNeutral(deadBand = NeutralStickDeadBand) && isPointFlightStartingOrActive()) { + if (now - lastStickLogAt > 1_000L) { + Log.d( + CloudCommandLogTag, + "ignore neutral stick_control during point-flight wayline data=$data" + ) + lastStickLogAt = now + } + return DjiCommandResult.ok("自动指点飞行执行中，已忽略中位杆量心跳") + } stopFlyToForManualControl() val enabled = ensureVirtualStickEnabled() if (!enabled.success) return enabled @@ -224,12 +268,6 @@ class CloudCommandExecutor( } else { virtualStick.setSpeedLevel(15.0) } - val position = if (data.hasAny("roll", "pitch", "throttle", "yaw")) { - data.toDrcStickPosition() - } else { - data.toProtocolStickPosition() - } - val now = System.currentTimeMillis() if (now - lastStickLogAt > 500L) { Log.d( CloudCommandLogTag, @@ -434,8 +472,11 @@ class CloudCommandExecutor( flyToFailureMessage: String, progressKind: PointFlightProgressKind = PointFlightProgressKind.FlyToPoint ): DjiCommandResult { - val telemetry = telemetryProvider() - if (telemetry.isRtkBlockingWaypointMission()) { + var telemetry = telemetryProvider() + if (telemetry.isRtkBlockingWaypointMission() && !telemetry.canAttemptAirborneTemporaryWaylineWithoutRtk()) { + telemetry = waitForTemporaryWaylineRtkReady(command) + } + if (telemetry.isRtkBlockingWaypointMission() && !telemetry.canAttemptAirborneTemporaryWaylineWithoutRtk()) { publishPointFlightFailure(command, progressKind, "RTK 已开启但尚未就绪") return DjiCommandResult.failed( "原生指点飞行不可用；RTK 已开启但尚未就绪（healthy=${telemetry.rtkHealthy}，" + @@ -443,36 +484,116 @@ class CloudCommandExecutor( "M350 航线任务会被 DJI 拒绝，请等待 RTK 固定解，或在 DJI Pilot 中关闭 RTK 后重试。" ) } + if (telemetry.isRtkBlockingWaypointMission()) { + Log.w( + CloudCommandLogTag, + "RTK is not ready, but aircraft is already airborne with valid GPS; " + + "try temporary wayline by FC/GPS position commandId=${command.commandId} " + + "gps=${telemetry.gpsSatelliteCount} solution=${telemetry.rtkPositioningSolution}" + ) + } + val requiresM350PointFlightFallback = telemetry.requiresTemporaryWaylinePointFlight() + val m350GroundStart = requiresM350PointFlightFallback && + !telemetry.isM350NativeTakeoffReadyForAutonomousCommand() + val m350TakeoffToPointGroundStart = + m350GroundStart && progressKind == PointFlightProgressKind.TakeoffToPoint + warningProvider().preStartPointFlightBlocker(telemetry)?.let { blocker -> + if (m350GroundStart) { + Log.w( + CloudCommandLogTag, + "M350 ground point-flight ignores pre-start HMS blocker and lets native takeoff decide " + + "commandId=${command.commandId} blocker=${blocker.toPointFlightBlockerText()}" + ) + } else { + val message = "飞控当前禁止起飞：${blocker.toPointFlightBlockerText()}" + publishPointFlightFailure(command, progressKind, message) + return DjiCommandResult.failed("原生指点飞行不可用；$message") + } + } + val outputFile = File.createTempFile("point-flight-", ".kmz") + val missionId = waypointMission.missionIdFromPath(outputFile.absolutePath) + beginPointFlightStart(command, missionId)?.let { return it } + if (m350GroundStart) { + val takeoffResult = startM350NativeTakeoffBeforeTemporaryWayline(command, progressKind, missionId) + if (!takeoffResult.success) { + clearStartingPointFlight(missionId) + return takeoffResult + } + telemetry = telemetryProvider() + if (m350TakeoffToPointGroundStart) { + val climbResult = climbM350TakeoffToPointManually(command, progressKind, missionId) + if (!climbResult.success) { + clearStartingPointFlight(missionId) + return climbResult + } + telemetry = telemetryProvider() + } + } + if ( + requiresM350PointFlightFallback && + !m350TakeoffToPointGroundStart && + telemetry.isM350NativeTakeoffReadyForAutonomousCommand() + ) { + val nativeFlyTo = tryM350NativeFlyToBeforeTemporaryWayline(command, progressKind, missionId) + if (nativeFlyTo?.success == true) { + return nativeFlyTo + } + } val startLocation = telemetry.pointFlightStartLocation() ?: run { + clearStartingPointFlight(missionId) publishPointFlightFailure(command, progressKind, "当前位置无效，无法生成航线兜底") return DjiCommandResult.failed( "原生指点飞行不可用，且当前位置无效，无法生成航线兜底：$flyToFailureMessage" ) } + val isGroundStart = if (requiresM350PointFlightFallback) { + !telemetry.isM350NativeTakeoffReadyForAutonomousCommand() + } else { + !telemetry.isReadyToStartTemporaryWayline() + } val targetHeight = (command.commanderFlightHeight ?: command.height).coerceAtLeast(1.0) - val safeTakeoffHeight = command.securityTakeoffHeight - .coerceAtLeast(1) - .coerceAtMost(targetHeight.roundToInt().coerceAtLeast(1)) - .toDouble() - val speed = command.maxSpeed.coerceIn(1, 15).toDouble() + val requestedSpeed = command.maxSpeed.coerceIn(1, 15).toDouble() + val speed = if (isGroundStart) { + requestedSpeed.coerceAtMost(TemporaryWaylineGroundStartMaxSpeedMetersPerSecond) + } else { + requestedSpeed + } + val safeTakeoffHeight = telemetry.temporaryWaylineTakeoffSecurityHeight( + targetHeight = targetHeight, + requestedTakeoffHeight = command.securityTakeoffHeight + ) + val distanceToTarget = startLocation.distanceMetersTo( + PointFlightStartLocation(command.latitude, command.longitude) + ) + val routeShape = if ( + isGroundStart && + distanceToTarget <= TemporaryWaylineGroundVerticalDistanceThresholdMeters + ) { + "vertical-to-target" + } else { + "lead-in-to-target" + } val temporaryWaypoints = temporaryPointFlightWaypoints( startLocation = startLocation, command = command, targetHeight = targetHeight, - speed = speed + speed = speed, + groundStart = isGroundStart ) - val outputFile = File.createTempFile("point-flight-", ".kmz") - val missionId = waypointMission.missionIdFromPath(outputFile.absolutePath) - beginPointFlightStart(command, missionId)?.let { return it } + val droneInfo = telemetry.wpmlDroneInfoForTemporaryWayline() + val payloadInfo = telemetry.wpmlPayloadInfoForTemporaryWayline() val plan = MissionPlan( name = missionId, waypoints = temporaryWaypoints, globalSpeed = speed, takeoffHeight = safeTakeoffHeight, globalHeight = targetHeight, - finishAction = MissionFinishAction.NoAction + finishAction = MissionFinishAction.NoAction, + includeGimbalPitch = false, + droneInfo = droneInfo, + payloadInfo = payloadInfo ) return runCatching { @@ -480,11 +601,13 @@ class CloudCommandExecutor( WpmlKmzBuilder().generateKmz(plan, outputFile) Log.d( CloudCommandLogTag, - "FlyTo handler missing, fallback to temporary wayline commandId=${command.commandId} missionId=$missionId " + + "start temporary point-flight wayline commandId=${command.commandId} missionId=$missionId " + "from=${startLocation.latitude},${startLocation.longitude} " + "to=${command.latitude},${command.longitude} height=$targetHeight " + - "commanderFlightHeight=${command.commanderFlightHeight} speed=$speed " + + "commanderFlightHeight=${command.commanderFlightHeight} speed=$speed requestedSpeed=$requestedSpeed " + + "groundStart=$isGroundStart distanceToTarget=${distanceToTarget.formatMeters()} routeShape=$routeShape " + "safeTakeoffHeight=$safeTakeoffHeight waypoints=${temporaryWaypoints.toLogText()} " + + "wpmlDrone=$droneInfo wpmlPayloads=$payloadInfo " + "flyToFailure=$flyToFailureMessage" ) val upload = waypointMission.uploadKmzFile(outputFile.absolutePath) @@ -498,7 +621,7 @@ class CloudCommandExecutor( val start = startTemporaryWaylineMission(missionId) if (start.success) { markPointFlightActive(command, missionId) - startPointFlightProgressMonitor(command, progressKind, missionId) + startPointFlightProgressMonitor(command, progressKind, missionId, isGroundStart) DjiCommandResult.ok("原生指点飞行不可用，已切换为临时航线指点飞行：$missionId") } else { clearStartingPointFlight(missionId) @@ -514,6 +637,184 @@ class CloudCommandExecutor( } } + private suspend fun tryM350NativeFlyToBeforeTemporaryWayline( + command: ParsedFlyToCommand, + progressKind: PointFlightProgressKind, + missionId: String + ): DjiCommandResult? { + Log.d( + CloudCommandLogTag, + "try M350 native FlyTo before temporary wayline commandId=${command.commandId} missionId=$missionId " + + "lat=${command.latitude} lon=${command.longitude} height=${command.height} " + + "maxSpeed=${command.maxSpeed} securityTakeoffHeight=${command.securityTakeoffHeight}" + ) + val nativeResult = flyToService.startFlyTo( + latitude = command.latitude, + longitude = command.longitude, + height = command.height, + maxSpeed = command.maxSpeed, + securityTakeoffHeight = command.securityTakeoffHeight, + flyToMode = command.flyToMode + ) + Log.d( + CloudCommandLogTag, + "M350 native FlyTo before temporary wayline result commandId=${command.commandId} " + + "success=${nativeResult.success} message=${nativeResult.message}" + ) + if (!nativeResult.success) { + return null + } + clearStartingPointFlight(missionId) + clearActivePointFlight() + startPointFlightProgressMonitor(command, progressKind) + return nativeResult + } + + private suspend fun startM350NativeTakeoffBeforeTemporaryWayline( + command: ParsedFlyToCommand, + progressKind: PointFlightProgressKind, + missionId: String + ): DjiCommandResult { + Log.d( + CloudCommandLogTag, + "M350 ground temporary wayline uses native takeoff first commandId=${command.commandId} missionId=$missionId" + ) + publishPointFlightProgress(command, progressKind, "TAKEOFF_STARTED") + val takeoff = flightControl.startTakeoff() + if (!takeoff.success) { + val message = "M350 地面指点飞行先执行原生起飞失败：${takeoff.message}" + publishPointFlightFailure(command, progressKind, message) + return DjiCommandResult.failed(message) + } + + val deadline = System.currentTimeMillis() + M350NativeTakeoffBeforeWaylineReadyTimeoutMs + var latest = telemetryProvider() + while (System.currentTimeMillis() < deadline) { + latest = telemetryProvider() + if (latest.isM350NativeTakeoffReadyForAutonomousCommand()) { + Log.d( + CloudCommandLogTag, + "M350 native takeoff is ready for autonomous command commandId=${command.commandId} " + + "missionId=$missionId isFlying=${latest.isFlying} motorsOn=${latest.motorsOn} " + + "altitude=${latest.altitude.formatMeters()} mode=${latest.flightMode}" + ) + delay(M350NativeTakeoffBeforeWaylineSettleMs) + return DjiCommandResult.ok("M350 原生起飞完成，准备启动自主飞行指令") + } + delay(M350NativeTakeoffBeforeWaylinePollMs) + } + + val message = "M350 原生起飞后未进入可启动航线状态：altitude=${latest.altitude.formatMeters()}，mode=${latest.flightMode}" + publishPointFlightFailure(command, progressKind, message) + return DjiCommandResult.failed(message) + } + + private suspend fun climbM350TakeoffToPointManually( + command: ParsedFlyToCommand, + progressKind: PointFlightProgressKind, + missionId: String + ): DjiCommandResult { + val requestedTargetHeight = (command.commanderFlightHeight ?: command.height).coerceAtLeast(1.0) + val climbTargetHeight = requestedTargetHeight.coerceAtMost(M350TakeoffToPointManualClimbHeightMeters) + var latest = telemetryProvider() + if (latest.altitude >= climbTargetHeight - M350TakeoffToPointManualClimbToleranceMeters) { + return DjiCommandResult.ok("M350 已达到航线接管高度") + } + + Log.d( + CloudCommandLogTag, + "M350 takeoff_to_point starts manual climb before wayline " + + "commandId=${command.commandId} missionId=$missionId " + + "from=${latest.altitude.formatMeters()} target=${climbTargetHeight.formatMeters()}" + ) + publishPointFlightProgress(command, progressKind, "MANUAL_CLIMB_STARTED") + val enable = ensureVirtualStickEnabled() + if (!enable.success) { + val message = "M350 一键起飞获取手动飞行控制权失败：${enable.message}" + publishPointFlightFailure(command, progressKind, message) + return DjiCommandResult.failed(message) + } + + val climbStick = (Stick.MAX_STICK_POSITION_ABS * M350TakeoffToPointManualClimbStickRatio) + .roundToInt() + .coerceIn(1, Stick.MAX_STICK_POSITION_ABS) + val deadline = System.currentTimeMillis() + M350TakeoffToPointManualClimbTimeoutMs + var reached = false + try { + while (System.currentTimeMillis() < deadline) { + latest = telemetryProvider() + if (!latest.motorsOn) break + if (latest.altitude >= climbTargetHeight - M350TakeoffToPointManualClimbToleranceMeters) { + reached = true + break + } + virtualStick.sendStickPosition(StickPosition(leftVertical = climbStick)) + delay(M350TakeoffToPointManualClimbPollMs) + } + } finally { + repeat(3) { + sendNeutralStick() + delay(M350TakeoffToPointManualClimbPollMs) + } + } + + val release = releaseVirtualStickForAutonomousFlight() + if (!release.success) { + val message = "M350 一键起飞释放手动飞行控制权失败：${release.message}" + publishPointFlightFailure(command, progressKind, message) + return DjiCommandResult.failed(message) + } + if (!reached) { + latest = telemetryProvider() + val message = "M350 一键起飞手动爬升未达到航线接管高度：" + + "altitude=${latest.altitude.formatMeters()}m，target=${climbTargetHeight.formatMeters()}m" + publishPointFlightFailure(command, progressKind, message) + return DjiCommandResult.failed(message) + } + + delay(M350TakeoffToPointManualClimbSettleMs) + latest = telemetryProvider() + Log.d( + CloudCommandLogTag, + "M350 takeoff_to_point manual climb completed and released control " + + "commandId=${command.commandId} missionId=$missionId " + + "altitude=${latest.altitude.formatMeters()} mode=${latest.flightMode}" + ) + publishPointFlightProgress(command, progressKind, "MANUAL_CLIMB_COMPLETED") + return DjiCommandResult.ok("M350 已手动爬升到航线接管高度，准备启动临时航线") + } + + private suspend fun waitForTemporaryWaylineRtkReady(command: ParsedFlyToCommand): TelemetrySnapshot { + val deadline = System.currentTimeMillis() + WaypointRtkReadyWaitMs + var latest = telemetryProvider() + Log.w( + CloudCommandLogTag, + "temporary wayline waits for RTK ready commandId=${command.commandId} " + + "healthy=${latest.rtkHealthy} solution=${latest.rtkPositioningSolution} " + + "gps=${latest.gpsSatelliteCount}" + ) + while (System.currentTimeMillis() < deadline) { + delay(WaypointRtkReadyPollMs) + latest = telemetryProvider() + if (!latest.isRtkBlockingWaypointMission() || latest.canAttemptAirborneTemporaryWaylineWithoutRtk()) { + Log.d( + CloudCommandLogTag, + "temporary wayline RTK wait passed commandId=${command.commandId} " + + "healthy=${latest.rtkHealthy} solution=${latest.rtkPositioningSolution} " + + "gps=${latest.gpsSatelliteCount}" + ) + return latest + } + } + Log.w( + CloudCommandLogTag, + "temporary wayline RTK wait timed out commandId=${command.commandId} " + + "healthy=${latest.rtkHealthy} solution=${latest.rtkPositioningSolution} " + + "gps=${latest.gpsSatelliteCount}" + ) + return latest + } + private fun existingPointFlightResult(command: ParsedFlyToCommand): DjiCommandResult? { val commandId = command.commandId.takeIf { it.isNotBlank() } ?: return null return synchronized(pointFlightStateLock) { @@ -611,6 +912,11 @@ class CloudCommandExecutor( } } + private fun isPointFlightStartingOrActive(): Boolean = + synchronized(pointFlightStateLock) { + activePointFlightWaylineMissionId != null || startingPointFlightMissionId != null + } + private fun clearStartingPointFlight(missionId: String) { synchronized(pointFlightStateLock) { if (startingPointFlightMissionId == missionId) { @@ -623,7 +929,8 @@ class CloudCommandExecutor( private fun startPointFlightProgressMonitor( command: ParsedFlyToCommand, progressKind: PointFlightProgressKind, - temporaryMissionId: String? = null + temporaryMissionId: String? = null, + groundStart: Boolean = false ) { val commandId = command.monitorKey(progressKind) pointFlightProgressJobs.remove(commandId)?.cancel() @@ -631,6 +938,7 @@ class CloudCommandExecutor( pointFlightProgressJobs[commandId] = scope.launch { val targetLocation = PointFlightStartLocation(command.latitude, command.longitude) val deadline = System.currentTimeMillis() + PointFlightProgressMonitorTimeoutMs + val monitorStartAt = System.currentTimeMillis() var lastProgressAt = 0L var lastRemainingDistance = Double.MAX_VALUE while (System.currentTimeMillis() < deadline) { @@ -641,6 +949,34 @@ class CloudCommandExecutor( val altitudeError = abs(command.height - telemetry.altitude) val remainingDistance = sqrt(horizontalDistance * horizontalDistance + altitudeError * altitudeError) val remainingSeconds = (remainingDistance / command.maxSpeed.coerceIn(1, 15)).toFloat() + val stalledTakeoff = groundStart && + !temporaryMissionId.isNullOrBlank() && + System.currentTimeMillis() - monitorStartAt >= PointFlightTakeoffStallCheckAfterMs && + telemetry.altitude <= PointFlightTakeoffStallAltitudeMeters && + abs(telemetry.speedZ) <= PointFlightTakeoffStallVerticalSpeedMeters + if (stalledTakeoff) { + val warning = warningProvider().pointFlightStallBlocker() + val message = buildString { + append("M350 临时航线起飞后卡在 ") + append(telemetry.altitude.formatMeters()) + append("m，未继续爬升") + append("，flightMode=${telemetry.flightMode}") + if (warning != null) { + append("；飞控告警：") + append(warning.toPointFlightBlockerText()) + } + } + Log.w( + CloudCommandLogTag, + "point flight takeoff stalled commandId=$commandId missionId=$temporaryMissionId " + + "altitude=${telemetry.altitude.formatMeters()} speedZ=${telemetry.speedZ.formatMeters()} " + + "mode=${telemetry.flightMode} warning=${warning?.toPointFlightBlockerText().orEmpty()}" + ) + stopTemporaryPointFlightWaylineAfterArrival(commandId, temporaryMissionId) + publishPointFlightFailure(command, progressKind, message) + pointFlightProgressJobs.remove(commandId) + return@launch + } val arrived = horizontalDistance <= PointFlightArriveThresholdMeters && altitudeError <= PointFlightArriveAltitudeThresholdMeters && (telemetry.isFlying || telemetry.altitude >= 0.8) @@ -783,56 +1119,21 @@ class CloudCommandExecutor( } /** - * M350 cannot reliably enter a dynamically generated waypoint mission - * directly from the ground. It first needs to finish the normal MSDK - * takeoff phase, then the already-uploaded temporary KMZ can be started. + * Temporary point flight is executed as a normal WPML mission. + * DJI wayline missions support both ground start (take off, climb, then fly) + * and airborne start, so do not mix in native takeoff here. */ private suspend fun startTemporaryWaylineMission(missionId: String): DjiCommandResult { - if (telemetryProvider().isReadyToStartTemporaryWayline()) { - return startTemporaryWaylineMissionWithRetry(missionId) - } - - Log.d(CloudCommandLogTag, "temporary wayline is ground-started; try direct startMission first missionId=$missionId") - val directStart = startTemporaryWaylineMissionWithRetry( - missionId = missionId, - maxAttempts = WaylineDirectGroundStartRetryCount - ) - if (directStart.success) { - return directStart - } - Log.w( - CloudCommandLogTag, - "direct ground startMission failed, fallback to native takeoff then startMission missionId=$missionId " + - "message=${directStart.message}" - ) - - Log.d(CloudCommandLogTag, "issue native takeoff before retrying temporary wayline missionId=$missionId") - val takeoff = flightControl.startTakeoff() - if (!takeoff.success) { - return DjiCommandResult.failed("临时航线已上传，但地面直接启动失败：${directStart.message}；原生起飞也失败：${takeoff.message}") - } - - val readyDeadline = System.currentTimeMillis() + WaylineInitialTakeoffReadyTimeoutMs - while (System.currentTimeMillis() < readyDeadline) { - val telemetry = telemetryProvider() - if (telemetry.isReadyToStartTemporaryWayline()) { - delay(WaylineInitialTakeoffSettleDelayMs) - val settledTelemetry = telemetryProvider() - Log.d( - CloudCommandLogTag, - "native takeoff ready for temporary wayline missionId=$missionId " + - "isFlying=${settledTelemetry.isFlying} motorsOn=${settledTelemetry.motorsOn} " + - "altitude=${settledTelemetry.altitude} mode=${settledTelemetry.flightMode}" - ) - return startTemporaryWaylineMissionWithRetry(missionId) - } - delay(200L) - } val telemetry = telemetryProvider() - return DjiCommandResult.failed( - "临时航线已上传，但原生起飞未进入稳定悬停：isFlying=${telemetry.isFlying}，" + - "altitude=${telemetry.altitude.formatMeters()} 米" + Log.d( + CloudCommandLogTag, + "start temporary point-flight mission missionId=$missionId " + + "isFlying=${telemetry.isFlying} motorsOn=${telemetry.motorsOn} " + + "altitude=${telemetry.altitude.formatMeters()} mode=${telemetry.flightMode} " + + "rtkEnabled=${telemetry.rtkEnabled} rtkHealthy=${telemetry.rtkHealthy} " + + "rtkSolution=${telemetry.rtkPositioningSolution}" ) + return startTemporaryWaylineMissionWithRetry(missionId) } private suspend fun startTemporaryWaylineMissionWithRetry( @@ -1192,36 +1493,241 @@ private fun TelemetrySnapshot.requiresTemporaryWaylinePointFlight(): Boolean { product.contains("M350") } +private fun TelemetrySnapshot.wpmlDroneInfoForTemporaryWayline(): MissionDroneInfo? { + val product = productType.uppercase() + return when { + product.contains("MATRICE_350") || product.contains("M350") -> + MissionDroneInfo(enumValue = WpmlM350DroneEnumValue) + + else -> null + } +} + +private fun TelemetrySnapshot.wpmlPayloadInfoForTemporaryWayline(): List { + val identities = cameraIdentities.ifEmpty { + listOf( + TelemetryCameraIdentity( + component = cameraComponent, + cameraType = cameraType, + cameraTypeValue = cameraTypeValue, + payloadCameraType = payloadCameraType, + payloadCameraTypeValue = payloadCameraTypeValue + ) + ) + } + val detected = identities + .mapNotNull { it.toMissionPayloadInfo() } + .distinctBy { it.positionIndex } + if (detected.isNotEmpty()) return detected + + val fallbackEnum = wpmlPayloadEnumFor( + cameraTypeValue = cameraTypeValue, + cameraText = ( + listOf(cameraType, payloadCameraType) + + cameraIdentities.flatMap { listOf(it.cameraType, it.payloadCameraType) } + ).joinToString(" "), + productText = productType + ) ?: return emptyList() + return listOf( + MissionPayloadInfo( + enumValue = fallbackEnum, + positionIndex = cameraComponent.toWpmlPayloadPositionIndex() + ) + ) +} + +private fun TelemetryCameraIdentity.toMissionPayloadInfo(): MissionPayloadInfo? { + val enumValue = payloadCameraTypeValue.takeIf { it.isKnownWpmlEnum() } + ?: cameraTypeValue.takeIf { it.isKnownWpmlEnum() } + ?: wpmlPayloadEnumFor( + cameraTypeValue = cameraTypeValue, + cameraText = listOf(cameraType, payloadCameraType).joinToString(" "), + productText = "" + ) + ?: return null + return MissionPayloadInfo( + enumValue = enumValue, + positionIndex = component.toWpmlPayloadPositionIndex() + ) +} + +private fun Int.isKnownWpmlEnum(): Boolean = this > 0 && this != WpmlUnknownEnumValue + +private fun wpmlPayloadEnumFor(cameraTypeValue: Int, cameraText: String, productText: String): Int? { + val camera = cameraText.uppercase() + val product = productText.uppercase() + return when { + camera.contains("H30T") || camera.contains("ZENMUSE_H30T") -> 83 + camera.contains("H30") || camera.contains("ZENMUSE_H30") -> 82 + camera.contains("H20N") || camera.contains("ZENMUSE_H20N") -> 61 + camera.contains("H20T") || camera.contains("ZENMUSE_H20T") -> WpmlH20TPayloadEnumValue + camera.contains("H20") || camera.contains("ZENMUSE_H20") -> 42 + cameraTypeValue.isKnownWpmlEnum() -> cameraTypeValue + product.contains("MATRICE_350") || product.contains("M350") -> WpmlH20TPayloadEnumValue + else -> null + } +} + +private fun String.toWpmlPayloadPositionIndex(): Int = + when (uppercase()) { + "LEFT_OR_MAIN" -> 0 + "RIGHT" -> 1 + "UP" -> 2 + "PORT_1" -> 3 + "PORT_2" -> 4 + "PORT_3" -> 5 + "PORT_4" -> 6 + else -> 0 + } + private fun TelemetrySnapshot.isReadyToStartTemporaryWayline(): Boolean = isFlying || altitude >= 0.8 +private fun TelemetrySnapshot.isM350NativeTakeoffReadyForAutonomousCommand(): Boolean { + val mode = flightMode.uppercase() + val stillTakingOff = mode.contains("AUTO_TAKE") || + mode.contains("TAKE_OFF") || + mode.contains("TAKEOFF") || + mode.contains("MOTOR_START") + return motorsOn && + altitude >= 0.8 && + !stillTakingOff +} + +private fun TelemetrySnapshot.temporaryWaylineTakeoffSecurityHeight( + targetHeight: Double, + requestedTakeoffHeight: Int +): Double { + val minimumHeight = TemporaryWaylineMinimumTakeoffHeightMeters.coerceAtMost(targetHeight).coerceAtLeast(1.0) + if (!isReadyToStartTemporaryWayline()) { + val requestedOrDefault = requestedTakeoffHeight + .takeIf { it > 0 } + ?.toDouble() + ?: TemporaryWaylineDefaultGroundTakeoffHeightMeters + return requestedOrDefault + .coerceAtLeast(minimumHeight) + .coerceAtMost(targetHeight.coerceAtLeast(minimumHeight)) + } + return requestedTakeoffHeight + .coerceAtLeast(minimumHeight.roundToInt().coerceAtLeast(1)) + .coerceAtMost(targetHeight.roundToInt().coerceAtLeast(minimumHeight.roundToInt().coerceAtLeast(1))) + .toDouble() +} + +private fun TelemetrySnapshot.canAttemptAirborneTemporaryWaylineWithoutRtk(): Boolean = + isReadyToStartTemporaryWayline() && + gpsValid && + locationValid && + gpsSatelliteCount >= 10 && + isValidCoordinate(latitude, longitude) + +private fun DroneWarningState.preStartPointFlightBlocker(telemetry: TelemetrySnapshot): DroneWarningItem? { + if (!active || telemetry.isReadyToStartTemporaryWayline()) return null + return items.firstOrNull { item -> + item.isUnableTakeoffWarning() || (item.isPoorRtkWarning() && telemetry.isRtkBlockingWaypointMission()) + } +} + +private fun DroneWarningState.pointFlightStallBlocker(): DroneWarningItem? { + if (!active) return null + return items.firstOrNull { it.isUnableTakeoffWarning() || it.isPoorRtkWarning() } + ?: items.firstOrNull() +} + +private fun DroneWarningItem.isUnableTakeoffWarning(): Boolean = + code.equals(DjiHmsUnableTakeoffCode, ignoreCase = true) || + code.equals(DjiHmsUnableTakeoffCodeAlt, ignoreCase = true) || + warningText().contains("无法起飞") + +private fun DroneWarningItem.isHeightLimitZoneWarning(): Boolean = + code.equals(DjiHmsHeightLimitZoneCode, ignoreCase = true) || + warningText().contains("限高区") + +private fun DroneWarningItem.isPoorRtkWarning(): Boolean = + code.equals(DjiHmsPoorRtkSignalCode, ignoreCase = true) || + warningText().contains("RTK 信号差", ignoreCase = true) + +private fun DroneWarningItem.toPointFlightBlockerText(): String { + val text = listOf(title, description, message) + .map { it.trim() } + .firstOrNull { it.isNotBlank() } + .orEmpty() + return listOf(code.trim(), text) + .filter { it.isNotBlank() } + .distinct() + .joinToString(": ") + .ifBlank { "未知飞控告警" } +} + +private fun DroneWarningItem.warningText(): String = + listOf(code, title, description, message).joinToString(" ") + private fun temporaryPointFlightWaypoints( startLocation: PointFlightStartLocation, command: ParsedFlyToCommand, targetHeight: Double, - speed: Double + speed: Double, + groundStart: Boolean ): List { val targetLocation = PointFlightStartLocation(command.latitude, command.longitude) - val targetDistance = startLocation.distanceMetersTo(targetLocation) - val bearing = if (targetDistance > TemporaryWaylineSamePointThresholdMeters) { - startLocation.bearingRadiansTo(targetLocation) - } else { - 0.0 + val distanceToTarget = startLocation.distanceMetersTo(targetLocation) + if (groundStart && distanceToTarget <= TemporaryWaylineGroundVerticalDistanceThresholdMeters) { + return listOf( + MissionWaypoint( + latitude = targetLocation.latitude, + longitude = targetLocation.longitude, + altitude = targetHeight, + speed = speed + ), + MissionWaypoint( + latitude = targetLocation.offsetByMeters( + northMeters = TemporaryWaylineGroundVerticalSecondPointOffsetMeters, + eastMeters = 0.0 + ).latitude, + longitude = targetLocation.offsetByMeters( + northMeters = TemporaryWaylineGroundVerticalSecondPointOffsetMeters, + eastMeters = 0.0 + ).longitude, + altitude = targetHeight, + speed = speed + ) + ) } - val departureOffset = if (targetDistance > TemporaryWaylineDepartureOffsetMeters * 2) { - TemporaryWaylineDepartureOffsetMeters + val preferredLeadInDistance = if (groundStart) { + TemporaryWaylineGroundLeadInDistanceMeters } else { - min(TemporaryWaylineDepartureOffsetMeters, TemporaryWaylineSamePointThresholdMeters + 1.0) + TemporaryWaylineLeadInDistanceMeters } - val departureLocation = startLocation.offsetBy( - distanceMeters = departureOffset, - bearingRadians = bearing - ) + val minimumDistinctDistance = if (groundStart) { + TemporaryWaylineGroundLeadInDistanceMeters + } else { + TemporaryWaylineMinimumDistinctWaypointDistanceMeters + } + val leadInLocation = when { + distanceToTarget >= minimumDistinctDistance * 2 -> { + val distanceBeforeTarget = preferredLeadInDistance.coerceAtMost(distanceToTarget / 2.0) + startLocation.pointAlongTo( + targetLocation, + distanceFromStartMeters = distanceToTarget - distanceBeforeTarget + ) + } + distanceToTarget >= minimumDistinctDistance -> { + startLocation.pointAlongTo( + targetLocation, + distanceFromStartMeters = distanceToTarget / 2.0 + ) + } + + else -> targetLocation.offsetByMeters( + northMeters = minimumDistinctDistance, + eastMeters = 0.0 + ) + } return listOf( MissionWaypoint( - latitude = departureLocation.latitude, - longitude = departureLocation.longitude, + latitude = leadInLocation.latitude, + longitude = leadInLocation.longitude, altitude = targetHeight, speed = speed ), @@ -1234,6 +1740,36 @@ private fun temporaryPointFlightWaypoints( ) } +private fun PointFlightStartLocation.pointAlongTo( + other: PointFlightStartLocation, + distanceFromStartMeters: Double +): PointFlightStartLocation { + val totalDistance = distanceMetersTo(other) + if (totalDistance <= 0.01) return offsetByMeters( + northMeters = TemporaryWaylineMinimumDistinctWaypointDistanceMeters, + eastMeters = 0.0 + ) + val fraction = (distanceFromStartMeters / totalDistance).coerceIn(0.0, 1.0) + return PointFlightStartLocation( + latitude = latitude + (other.latitude - latitude) * fraction, + longitude = longitude + (other.longitude - longitude) * fraction + ) +} + +private fun PointFlightStartLocation.offsetByMeters( + northMeters: Double, + eastMeters: Double +): PointFlightStartLocation { + val latitudeRadians = latitude.toRadians() + val deltaLatitude = northMeters / CoordEarthRadiusMeters * 180.0 / CoordPi + val longitudeRadius = CoordEarthRadiusMeters * cos(latitudeRadians).coerceAtLeast(0.01) + val deltaLongitude = eastMeters / longitudeRadius * 180.0 / CoordPi + return PointFlightStartLocation( + latitude = latitude + deltaLatitude, + longitude = longitude + deltaLongitude + ) +} + private fun PointFlightStartLocation.distanceMetersTo(other: PointFlightStartLocation): Double { val lat1 = latitude.toRadians() val lat2 = other.latitude.toRadians() @@ -1245,33 +1781,6 @@ private fun PointFlightStartLocation.distanceMetersTo(other: PointFlightStartLoc return 2 * CoordEarthRadiusMeters * atan2(sqrt(normalized), sqrt(1 - normalized)) } -private fun PointFlightStartLocation.bearingRadiansTo(other: PointFlightStartLocation): Double { - val lat1 = latitude.toRadians() - val lat2 = other.latitude.toRadians() - val dLon = (other.longitude - longitude).toRadians() - val y = sin(dLon) * cos(lat2) - val x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon) - return atan2(y, x) -} - -private fun PointFlightStartLocation.offsetBy( - distanceMeters: Double, - bearingRadians: Double -): PointFlightStartLocation { - val angularDistance = distanceMeters / CoordEarthRadiusMeters - val lat1 = latitude.toRadians() - val lon1 = longitude.toRadians() - val lat2 = asin( - sin(lat1) * cos(angularDistance) + - cos(lat1) * sin(angularDistance) * cos(bearingRadians) - ) - val lon2 = lon1 + atan2( - sin(bearingRadians) * sin(angularDistance) * cos(lat1), - cos(angularDistance) - sin(lat1) * sin(lat2) - ) - return PointFlightStartLocation(lat2.toDegrees(), lon2.toDegrees()) -} - private fun List.toLogText(): String = joinToString(prefix = "[", postfix = "]") { "${it.latitude},${it.longitude},${it.altitude}" @@ -1279,8 +1788,6 @@ private fun List.toLogText(): String = private fun Double.toRadians(): Double = this / 180.0 * CoordPi -private fun Double.toDegrees(): Double = this / CoordPi * 180.0 - private fun parseFlyToCommand(data: JSONObject): ParsedFlyToCommand? { val target = data.optJSONObject("target_location") ?: data.optJSONObject("targetLocation") @@ -1530,6 +2037,12 @@ private fun String.toLookAtMode(): LookAtMode = else -> LookAtMode.LOOK_AT_GIMBAL_FREE } +private fun StickPosition.isNeutral(deadBand: Int = 0): Boolean = + abs(leftHorizontal) <= deadBand && + abs(leftVertical) <= deadBand && + abs(rightHorizontal) <= deadBand && + abs(rightVertical) <= deadBand + private fun JSONObject.hasAny(vararg names: String): Boolean = names.any { has(it) && !isNull(it) } diff --git a/sample/src/main/java/com/zklh/dronecontroller/core/livestream/LiveStreamingService.kt b/sample/src/main/java/com/zklh/dronecontroller/core/livestream/LiveStreamingService.kt index 5a4e025..cb65c0e 100644 --- a/sample/src/main/java/com/zklh/dronecontroller/core/livestream/LiveStreamingService.kt +++ b/sample/src/main/java/com/zklh/dronecontroller/core/livestream/LiveStreamingService.kt @@ -15,6 +15,7 @@ import dji.v5.manager.datacenter.livestream.LiveVideoBitrateMode import dji.v5.manager.datacenter.livestream.StreamQuality import dji.v5.manager.datacenter.livestream.settings.RtmpSettings import dji.v5.manager.interfaces.ICameraStreamManager +import java.util.concurrent.atomic.AtomicBoolean import kotlinx.coroutines.delay import kotlinx.coroutines.flow.MutableStateFlow import kotlinx.coroutines.flow.StateFlow @@ -146,16 +147,29 @@ class LiveStreamingService { } _state.update { it.copy(busy = true, message = "正在关闭直播", lastError = null) } return suspendCancellableCoroutine { continuation -> + val resumed = AtomicBoolean(false) liveStreamManager().stopStream(object : CommonCallbacks.CompletionCallback { override fun onSuccess() { + if (!resumed.compareAndSet(false, true)) { + Log.w(LiveStreamLogTag, "ignore duplicate stop stream success callback") + return + } _state.update { it.copy(streaming = false, busy = false, message = "直播已关闭", lastError = null) } - continuation.resume(DjiCommandResult.ok("直播已关闭")) + if (continuation.isActive) { + continuation.resume(DjiCommandResult.ok("直播已关闭")) + } } override fun onFailure(error: IDJIError) { + if (!resumed.compareAndSet(false, true)) { + Log.w(LiveStreamLogTag, "ignore duplicate stop stream failure callback: $error") + return + } val message = error.toString() _state.update { it.copy(busy = false, lastError = message, message = "关闭直播失败：$message") } - continuation.resume(DjiCommandResult.failed("关闭直播失败：$message")) + if (continuation.isActive) { + continuation.resume(DjiCommandResult.failed("关闭直播失败：$message")) + } } }) } @@ -219,16 +233,29 @@ class LiveStreamingService { ) } return suspendCancellableCoroutine { continuation -> + val resumed = AtomicBoolean(false) manager.startStream(object : CommonCallbacks.CompletionCallback { override fun onSuccess() { + if (!resumed.compareAndSet(false, true)) { + Log.w(LiveStreamLogTag, "ignore duplicate start stream success callback") + return + } _state.update { it.copy(streaming = true, busy = false, message = "直播已开启", lastError = null) } - continuation.resume(DjiCommandResult.ok("直播已开启")) + if (continuation.isActive) { + continuation.resume(DjiCommandResult.ok("直播已开启")) + } } override fun onFailure(error: IDJIError) { + if (!resumed.compareAndSet(false, true)) { + Log.w(LiveStreamLogTag, "ignore duplicate start stream failure callback: $error") + return + } val message = error.toString() _state.update { it.copy(streaming = false, busy = false, lastError = message, message = "开启直播失败：$message") } - continuation.resume(DjiCommandResult.failed("开启直播失败：$message")) + if (continuation.isActive) { + continuation.resume(DjiCommandResult.failed("开启直播失败：$message")) + } } }) } diff --git a/sample/src/main/java/com/zklh/dronecontroller/core/mission/MissionModels.kt b/sample/src/main/java/com/zklh/dronecontroller/core/mission/MissionModels.kt index 2c711dd..314bdd0 100644 --- a/sample/src/main/java/com/zklh/dronecontroller/core/mission/MissionModels.kt +++ b/sample/src/main/java/com/zklh/dronecontroller/core/mission/MissionModels.kt @@ -7,7 +7,21 @@ data class MissionPlan( val takeoffHeight: Double = 20.0, val globalHeight: Double = 100.0, val finishAction: MissionFinishAction = MissionFinishAction.GoHome, - val lostAction: MissionLostAction = MissionLostAction.GoBack + val lostAction: MissionLostAction = MissionLostAction.GoBack, + val includeGimbalPitch: Boolean = true, + val droneInfo: MissionDroneInfo? = null, + val payloadInfo: List = emptyList() +) + +data class MissionDroneInfo( + val enumValue: Int, + val subEnumValue: Int = 0 +) + +data class MissionPayloadInfo( + val enumValue: Int, + val subEnumValue: Int = 0, + val positionIndex: Int = 0 ) data class MissionWaypoint( diff --git a/sample/src/main/java/com/zklh/dronecontroller/core/mission/WaypointMissionService.kt b/sample/src/main/java/com/zklh/dronecontroller/core/mission/WaypointMissionService.kt index c4213dc..2e76a87 100644 --- a/sample/src/main/java/com/zklh/dronecontroller/core/mission/WaypointMissionService.kt +++ b/sample/src/main/java/com/zklh/dronecontroller/core/mission/WaypointMissionService.kt @@ -1,15 +1,22 @@ package com.zklh.dronecontroller.core.mission +import android.util.Log import com.zklh.dronecontroller.core.msdk.DjiCommandResult import com.zklh.dronecontroller.core.safety.SafetyInterlock import dji.v5.common.callback.CommonCallbacks import dji.v5.common.error.IDJIError +import dji.v5.manager.aircraft.waypoint3.WaylineExecutingInfoListener import dji.v5.manager.aircraft.waypoint3.WaypointMissionManager +import dji.v5.manager.aircraft.waypoint3.model.WaylineExecutingInfo import java.io.File import kotlin.coroutines.resume import kotlinx.coroutines.suspendCancellableCoroutine class WaypointMissionService { + init { + installWaypointDebugListenersOnce() + } + fun uploadKmzFile( kmzPath: String, onUpdate: (MissionUploadUpdate) -> Unit @@ -118,6 +125,41 @@ class WaypointMissionService { fun missionIdFromPath(path: String): String = File(path).nameWithoutExtension + private fun installWaypointDebugListenersOnce() { + synchronized(WaypointListenerLock) { + if (waypointDebugListenersInstalled) return + waypointDebugListenersInstalled = true + } + runCatching { + WaypointMissionManager.getInstance().addWaypointMissionExecuteStateListener { state -> + Log.d(WaypointMissionLogTag, "execute state=${state?.name ?: state}") + } + WaypointMissionManager.getInstance().addWaylineExecutingInfoListener( + object : WaylineExecutingInfoListener { + override fun onWaylineExecutingInfoUpdate(info: WaylineExecutingInfo) { + Log.d( + WaypointMissionLogTag, + "executing info mission=${info.missionFileName} waylineId=${info.waylineID} " + + "waypointIndex=${info.currentWaypointIndex}" + ) + } + + override fun onWaylineExecutingInterruptReasonUpdate(error: IDJIError?) { + if (error != null) { + Log.e( + WaypointMissionLogTag, + "executing interrupt code=${error.errorCode()} description=${error.description()} raw=$error" + ) + } + } + } + ) + Log.d(WaypointMissionLogTag, "waypoint debug listeners installed") + }.onFailure { error -> + Log.w(WaypointMissionLogTag, "install waypoint debug listeners failed: ${error.message}", error) + } + } + private fun callback( continuation: kotlinx.coroutines.CancellableContinuation, successMessage: String @@ -132,4 +174,11 @@ class WaypointMissionService { } } } + + private companion object { + private const val WaypointMissionLogTag = "ZklhWaypointMission" + private val WaypointListenerLock = Any() + @Volatile + private var waypointDebugListenersInstalled = false + } } diff --git a/sample/src/main/java/com/zklh/dronecontroller/core/mission/WpmlKmzBuilder.kt b/sample/src/main/java/com/zklh/dronecontroller/core/mission/WpmlKmzBuilder.kt index c9daef8..972ae90 100644 --- a/sample/src/main/java/com/zklh/dronecontroller/core/mission/WpmlKmzBuilder.kt +++ b/sample/src/main/java/com/zklh/dronecontroller/core/mission/WpmlKmzBuilder.kt @@ -21,6 +21,7 @@ import dji.sdk.wpmz.value.mission.WaylineAltitudeMode import dji.sdk.wpmz.value.mission.WaylineCoordinateMode import dji.sdk.wpmz.value.mission.WaylineCoordinateParam import dji.sdk.wpmz.value.mission.WaylineDroneInfo +import dji.sdk.wpmz.value.mission.WaylineDroneType import dji.sdk.wpmz.value.mission.WaylineExitOnRCLostAction import dji.sdk.wpmz.value.mission.WaylineExitOnRCLostBehavior import dji.sdk.wpmz.value.mission.WaylineFinishedAction @@ -32,6 +33,7 @@ import dji.sdk.wpmz.value.mission.WaylineMission import dji.sdk.wpmz.value.mission.WaylineMissionConfig import dji.sdk.wpmz.value.mission.WaylinePayloadInfo import dji.sdk.wpmz.value.mission.WaylinePayloadParam +import dji.sdk.wpmz.value.mission.WaylinePayloadType import dji.sdk.wpmz.value.mission.WaylinePositioningType import dji.sdk.wpmz.value.mission.WaylineTemplateWaypointInfo import dji.sdk.wpmz.value.mission.WaylineWaypoint @@ -65,13 +67,13 @@ class WpmlKmzBuilder { return WaylineMissionConfig().apply { flyToWaylineMode = WaylineFlyToWaylineMode.SAFELY finishAction = plan.finishAction.toDji() - droneInfo = WaylineDroneInfo() + droneInfo = plan.droneInfo.toDji() securityTakeOffHeight = plan.takeoffHeight isSecurityTakeOffHeightSet = true exitOnRCLostBehavior = WaylineExitOnRCLostBehavior.EXCUTE_RC_LOST_ACTION exitOnRCLostType = plan.lostAction.toDji() globalTransitionalSpeed = plan.globalSpeed - payloadInfo = ArrayList() + payloadInfo = ArrayList(plan.payloadInfo.map { it.toDji() }) } } @@ -117,7 +119,11 @@ class WpmlKmzBuilder { ) } isTemplateGlobalYawParamSet = true - pitchMode = WaylineWaypointPitchMode.USE_POINT_SETTING + pitchMode = if (plan.includeGimbalPitch) { + WaylineWaypointPitchMode.USE_POINT_SETTING + } else { + WaylineWaypointPitchMode.MANUALLY + } } } @@ -133,7 +139,9 @@ class WpmlKmzBuilder { ellipsoidHeight = point.altitude speed = point.speed ?: plan.globalSpeed useGlobalTurnParam = true - gimbalPitchAngle = point.gimbalPitch + if (plan.includeGimbalPitch) { + gimbalPitchAngle = point.gimbalPitch + } yawParam = WaylineWaypointYawParam().apply { enableYawAngle = point.heading != null yawAngle = point.heading ?: 0.0 @@ -264,4 +272,22 @@ class WpmlKmzBuilder { MissionLostAction.Hover -> WaylineExitOnRCLostAction.HOVER } } + + private fun MissionDroneInfo?.toDji(): WaylineDroneInfo { + return WaylineDroneInfo().apply { + this@toDji?.let { info -> + droneType = WaylineDroneType.find(info.enumValue) + droneSubType = info.subEnumValue + } + } + } + + private fun MissionPayloadInfo.toDji(): WaylinePayloadInfo { + return WaylinePayloadInfo().apply { + payloadType = WaylinePayloadType.find(enumValue) + payloadSubType = subEnumValue + payloadPositionIndex = positionIndex + isPayloadPositionIndexSet = true + } + } } diff --git a/sample/src/main/java/com/zklh/dronecontroller/core/telemetry/TelemetryRepository.kt b/sample/src/main/java/com/zklh/dronecontroller/core/telemetry/TelemetryRepository.kt index 6fccf09..4ea9c9b 100644 --- a/sample/src/main/java/com/zklh/dronecontroller/core/telemetry/TelemetryRepository.kt +++ b/sample/src/main/java/com/zklh/dronecontroller/core/telemetry/TelemetryRepository.kt @@ -995,7 +995,14 @@ class TelemetryRepository( return } val source = state.rtkReferenceStationSource ?: return - if (!source.isNetworkRtkSource() || rtkServiceStarted) return + if (!source.isNetworkRtkSource()) return + if (rtkServiceStarted) { + rtkServiceStarted = false + Log.d( + TelemetryLogTag, + "RTK network service became unhealthy; mark service stale and restart current source=$source" + ) + } val now = System.currentTimeMillis() if (rtkStartInProgress || now - lastRtkStartAtMs < RTK_START_RETRY_INTERVAL_MS) return Log.d(TelemetryLogTag, "restore existing RTK network service source=$source") diff --git a/sample/src/main/java/com/zklh/dronecontroller/ui/DroneControllerScreen.kt b/sample/src/main/java/com/zklh/dronecontroller/ui/DroneControllerScreen.kt index 482e20f..e083080 100644 --- a/sample/src/main/java/com/zklh/dronecontroller/ui/DroneControllerScreen.kt +++ b/sample/src/main/java/com/zklh/dronecontroller/ui/DroneControllerScreen.kt @@ -322,6 +322,8 @@ fun DroneControllerScreen() { val liveStreaming = remember { LiveStreamingService() } val simulator = remember { SimulatorService() } val warningRepository = remember { DroneWarningRepository() } + val warningState by warningRepository.state.collectAsState() + val latestWarningState by rememberUpdatedState(warningState) val cloudLoginClient = remember { CloudLoginClient() } val cloudMqttService = remember { CloudMqttService(cloudLoginClient) } val cloudMediaUpload = remember(appContext, cloudMqttService) { @@ -337,12 +339,12 @@ fun DroneControllerScreen() { waypointMission = waypointMission, mediaUpload = cloudMediaUpload, telemetryProvider = { latestTelemetry }, + warningProvider = { latestWarningState }, progressPublisher = cloudMqttService ) } val stickStatus by virtualStick.status.collectAsState() val simulatorStatus by simulator.status.collectAsState() - val warningState by warningRepository.state.collectAsState() val cloudState by cloudMqttService.state.collectAsState() val liveState by liveStreaming.state.collectAsState()